Page 1 of 1

Attitude controller modelling

Posted: Mon Feb 11, 2019 4:19 pm
by greensebastian
Hi!

I'm trying to model the cascading PID controller implemented in the firmware in simulink for testing external control methods. The problem I'm running into is that i cannot seem to get anywhere near the rate parameters in the firmware ([KP KI KD] at [250 500 2.5]) before the model becomes unstable, the highest I've managed is [50 50 6]. The attitude parameters are [6 3 0] as in the firmware and I've implemented the cascading controller using deg and deg/s same as the firmware. I've implemented the controller as a matlab function emulating the firmware as seen below but already at [50 50 6] the PWM duty cycle oscillates significantly.The filter is very basic as I'm using the actual system state for feedback at the moment. The code and a ramp/step response is visible below, left figure is roll angle and ref and right side is roll rate and ref.

Has this been done before with the current parameters, and is there something else significant i am missing? I'm using the Tait-Bryan model developed by Marcus Greiff which can be found in his publication here.
Image

Code: Select all

function u = attitude_PID_rate(error, inner_arc_KP, inner_arc_KI, inner_arc_KD, inner_rate_h, inner_arc_integr_sat)
    % Initialization of persistent variables
    persistent prev_error integ prev_deriv
    if isempty(prev_error)
        prev_error = 0;
    end
    if isempty(integ)
        integ = 0;
    end
    if isempty(prev_deriv)
        prev_deriv = [0 0];
    end
    
    out = 0;
    out = out + error*inner_arc_KP;
    
    deriv = (error-prev_error)/inner_rate_h;
    filter_enabled = true;
    if(filter_enabled)
        deriv = 0.6*deriv + 0.3*prev_deriv(1) + 0.1*prev_deriv(2);
    end
    out = out + deriv*inner_arc_KD;
    prev_deriv(2) = prev_deriv(1);
    prev_deriv(1) = deriv;
    
    integ = integ + error*inner_rate_h;
    if integ > inner_arc_integr_sat
        integ = inner_arc_integr_sat;
    elseif integ < -inner_arc_integr_sat
        integ = -inner_arc_integr_sat;
    end
    out = out + integ*inner_arc_KI;
    
    prev_error = error;
    u = out;
end