Hello,
I am trying to write new control algorithms for a foamie that I am working on. Instead of being a rate based controller, mine will be more of a classical PID controller where the error term, angle_err, is the instantaneous angle error (in the roll axis for instance). I get this value by passing airs.roll_sensor to the controller such that angle_err = airs.roll_sensor * .01f (degrees not centi-degrees). Here is the basic controller code:
float controllerErr = (angle_err * _kpr) + (omega_x * _kdr) + (_integrator * _kir);
This is the term that is then passed to get_servo_out, which I have set up to just pass that value straight back to channel_roll->servo_out.
The integrator is handled pretty much the same as the original APM code:
[code] if (!disable_integrator && _kig > 0) {
//only integrate if gain and time step are positive and airspeed above min value.
if (dt > 0) {
float integrator_delta = angle_err * delta_time;
if (_last_out < -45) {
// prevent the integrator from increasing if surface defln demand is above the upper limit
integrator_delta = max(integrator_delta , 0);
} else if (_last_out > 45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit
integrator_delta = min(integrator_delta , 0);
}
_integrator += integrator_delta;
}
} else {
_integrator = 0;
}[/code]
and the derivative term is calculated using the instantaneous roll rate:
This code is taking the current angle error (angle_err) away from being level, and tries to calculate the instantaneous control surface deflection needed to start pushing the foamie back to level flight. It is my understanding that the proportional term should drop as it gets closer to being level and the integrator should wind up if I hold my model at a nonzero roll angle.
Right now, the servos are definitely not doing that. They don’t wind up at all, even if I hold the model at a non-zero roll angle. I have checked to make sure that everything is in terms of degrees for the controller.
Can anyone give me any insight into why my code isn’t doing what I am expecting it to do?