Modified Roll Controller


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;

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?

Hi Mohrad,
I’d suggest you get SITL setup so you can experiment with your new controller in an environment where you can use a debugger. See the SITL pages on the website.
You also could use hal.console->printf() to add some debug code to see if you can work out why the code isn’t behaving as you expect.
Cheers, Tridge

Hello Tridge,

I am a Pixhawk newbie but have coded for quite a while.
I am really stuck trying to find working code to print to a console to debug my Rover code.

Could you share something like that with me, please.
Would I expect to use MissionPlanner or QGroundcontrol, etc.?
I noticed you suggested hal.console above but I have not got it to work at all.