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.
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