Control Stratagey

The four things I can control are: Rudder, Collective, Elevator, Aileron.

My scheme for control is to use a set of PID loops.
Most likely some of these will only have P gain, but the concept is to use the following:(Copied from code) The PIDs marked with * in the comments will be different in the rocket.


OnePidElement Yaw_to_Rudder; //Takes IMU yaw and drives rudder *
OnePidElement Alt_to_VSpeed; // target altitude drives desired vspeed.
OnePidElement VSpeed_To_Collective;// desired vspeed drives collective.*
OnePidElement Pitch_To_Elevator;//desired pitch drives Elevator.*
OnePidElement Roll_To_Alieron;// desired roll drives Alieron.*
OnePidElement V_Forward_To_Pitch;//desired Vforward drives pitch.
OnePidElement V_Right_To_Roll; //desired Vright and drives roll
OnePidElement Long_to_V_East; //desired Longitude and drives VEast.
OnePidElement Lat_to_V_North; //desired Lattitude and drives VNorth;

And one more conversion function:
void ConvertDirection(double V_North, //Desired V_North input
double V_East,//Desired V_East input
double Yaw,//Current True YAW corrected for magnetic deviation input,
double &V_Forward,// Converted V_Forward target out
double & V_Right, //Converted V_Right target out
);

OnePidElement is an object that holds the a PIDGainConstants object and the current integration state.

The Yaw to rudder , Pitch to Elevator and Roll to Alieron PIDs will be run for each IMU sample.
All other PID's will be run for each GPS data set.
I'm not planning to integrate the accelerometers. (Yet)
An example would be:
Elevator=Pitch_To_Elevator.DoServoPidAdjust(target_pitch, imu.pitch , imu.x_rate);

Each OnePidElement contins a PID gainObject that has the following gain limit elements:
double p_gain;//Proportional Gain
double i_gain;//Integrator Gain;
double d_delta_gain; //Sample to Sample delta gain
double d_meas_gain; //Rate as measured by IMU or GPS
double max_i_windup;// Maximum integrator value limit.
double min_i_windup;//Minimum integrator value limit.
double max_out_limit;//Maximum allowable value or adjustment
double min_out_limit;//Minimum allowable value or adjustment

In fact I have two D terms in my PID. One is change in the sampled values (IE the delta term from sample n-1 to n) and the directly measured rate. The directly measured differential/change rates are from the IMU and GPS.

I'm currently working on the interface to selectively change these PID constants in flight. This might be easier if I switch to a two person operation, one to fly the helicopter and one to twiddle the gains. For Now I'm just going to work one axis/ PID at a time.

Above all of this is the rather simple executive that generates the sequence of target lat,lon and altitude. At this point that excutive just tried to freeze the vehicle in space when activated. (IE use the current Yaw, Alt ,Lat,Lon as the target)

(Sorry for the crummy code formatting blogger does not seem to like formatting code in a reasonable way)