aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

TradHeli - DIP switches now allow you to turn on/off control of roll, pitch, yaw axis.

Browse Source 
Added capturing yaw trim on start up to allow I term of Yaw PID controller to be lower
  Added D term to Yaw's PID controller
  Modified default PID settings for all axis.  Now includes some D term.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1016 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2010-12-05 07:06:27 +00:00
parent a37d1ddb91
commit c747c43c0b
2 changed files with 164 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArducopterNG/Heli.h
View File

@ -76,10 +76,6 @@ TODO:
#define HELI_STICK_TO_ANGLE_FACTOR 2.0 // To convert ccpm values (-50 ~ 50 ) to absolute angles. larger number means less lean #define HELI_STICK_TO_ANGLE_FACTOR 2.0 // To convert ccpm values (-50 ~ 50 ) to absolute angles. larger number means less lean
#define HELI_YAW_STICK_TO_ANGLE_FACTOR 0.5 // convert yaw (-50 ~ 50) to turn rate in degrees per second. larger number means slower turn rate #define HELI_YAW_STICK_TO_ANGLE_FACTOR 0.5 // convert yaw (-50 ~ 50) to turn rate in degrees per second. larger number means slower turn rate
// roll and pitch adjustment - attitude close to what's required to keep heli in one place when hovering. This will be added to the roll/pitch commands from the pilot
#define HELI_ADJUST_ROLL 4
#define HELI_ADJUST_PITCH -3
// CCPM Types // CCPM Types
#define HELI_CCPM_120_TWO_FRONT_ONE_BACK 0 #define HELI_CCPM_120_TWO_FRONT_ONE_BACK 0
#define HELI_CCPM_120_ONE_FRONT_TWO_BACK 1 #define HELI_CCPM_120_ONE_FRONT_TWO_BACK 1
@ -150,6 +146,17 @@ float command_rx_collective;
float yawPercent; float yawPercent;
float targetHeading; float targetHeading;
// trims
float trim_roll = 0.0;
float trim_pitch = 0.0;
float trim_yaw = 0.0;
// axis under APM control
int roll_control_switch = 1;
int pitch_control_switch = 1;
int yaw_control_switch = 1;
int collective_control_switch = 0;
/// for sending values out to servos /// for sending values out to servos
Vector3f ccpmPercents_out; // Array of ccpm input values, converted to percents Vector3f ccpmPercents_out; // Array of ccpm input values, converted to percents
Vector3f pitchRollCollPercent_out; // Array containing deallocated pitch, roll, and collective percent commands Vector3f pitchRollCollPercent_out; // Array containing deallocated pitch, roll, and collective percent commands

111
ArducopterNG/Heli.pde
View File

@ -72,21 +72,21 @@ void heli_defaultUserConfig()
yawMax = 1800; yawMax = 1800;
// default PID values - Roll // default PID values - Roll
KP_QUAD_ROLL = 1.000; KP_QUAD_ROLL = 1.100;
KI_QUAD_ROLL = 0.150; KI_QUAD_ROLL = 0.200;
STABLE_MODE_KP_RATE_ROLL = 0.000; STABLE_MODE_KP_RATE_ROLL = -0.001;
// default PID values - Pitch // default PID values - Pitch
KP_QUAD_PITCH = 1.200; KP_QUAD_PITCH = 1.100;
KI_QUAD_PITCH = 0.120; KI_QUAD_PITCH = 0.120;
STABLE_MODE_KP_RATE_PITCH = 0.000; STABLE_MODE_KP_RATE_PITCH = -0.001;
// default PID values - Yaw // default PID values - Yaw
Kp_RateYaw = 2.000; // heading P term Kp_RateYaw = 3.500; // heading P term
Ki_RateYaw = 0.100; // heading I term Ki_RateYaw = 0.100; // heading I term
KP_QUAD_YAW = 0.150; // yaw rate P term KP_QUAD_YAW = 0.200; // yaw rate P term
KI_QUAD_YAW = 0.030; // yaw rate I term KI_QUAD_YAW = 0.040; // yaw rate I term
STABLE_MODE_KP_RATE_YAW = 0.000; // not used STABLE_MODE_KP_RATE_YAW = -0.010; // yaw rate D term
} }
/**********************************************************************/ /**********************************************************************/
@ -151,11 +151,67 @@ void heli_setup()
yawSlope = 100 / (yawMax - yawMin); yawSlope = 100 / (yawMax - yawMin);
yawIntercept = 50 - (yawSlope * yawMax); yawIntercept = 50 - (yawSlope * yawMax);
// capture trims
heli_read_radio_trims();
// hardcode mids because we will use ccpm // hardcode mids because we will use ccpm
roll_mid = ROLL_MID; roll_mid = ROLL_MID;
pitch_mid = PITCH_MID; pitch_mid = PITCH_MID;
collective_mid = 1500; collective_mid = 1500;
yaw_mid = (yawMin+yawMax)/2; yaw_mid = (yawMin+yawMax)/2;
// determine which axis APM will control
roll_control_switch = !SW_DIP1;
pitch_control_switch = !SW_DIP2;
yaw_control_switch = !SW_DIP3;
collective_control_switch = !SW_DIP4;
}
/*****************************************************************************************************/
// heli_read_radio_trims - captures roll, pitch and yaw trims (mids) although only yaw is actually used
// trim_yaw is used to center output to the tail which tends to be far from the
// physical middle of where the rudder can move. This helps keep the PID's I
// value low and avoid sudden turns left on takeoff
void heli_read_radio_trims()
{
int i;
float sumRoll = 0, sumPitch = 0, sumYaw = 0;
// initialiase trims to zero incase this is called more than once
trim_roll = 0.0;
trim_pitch = 0.0;
trim_yaw = 0.0;
// read radio a few times
for(int i=0; i<10; i++ )
{
// make sure new data has arrived
while( APM_RC.GetState() != 1 )
{
delay(20);
}
heli_read_radio();
sumRoll += ch_roll;
sumPitch += ch_pitch;
sumYaw += ch_yaw;
}
// set trim to average
trim_roll = sumRoll / 10.0;
trim_pitch = sumPitch / 10.0;
trim_yaw = sumYaw / 10.0;
// double check all is ok
if( trim_roll > 50.0 || trim_roll < -50 )
trim_roll = 0.0;
if( trim_pitch >50.0 || trim_roll < -50.0 )
trim_pitch = 0.0;
if( trim_yaw > 50.0 || trim_yaw < -50.0 )
trim_yaw = 0.0;
//Serial.print("trim_yaw = " );
//Serial.print(trim_yaw);
//Serial.println();
} }
/**********************************************************************/ /**********************************************************************/
@ -190,10 +246,10 @@ void heli_read_radio()
ch_yaw = yawPercent; ch_yaw = yawPercent;
// convert to absolute angles // convert to absolute angles
command_rx_roll = ch_roll / HELI_STICK_TO_ANGLE_FACTOR + HELI_ADJUST_ROLL; // Convert stick position to absolute angles command_rx_roll = ch_roll / HELI_STICK_TO_ANGLE_FACTOR; // Convert stick position to absolute angles
command_rx_pitch = ch_pitch / HELI_STICK_TO_ANGLE_FACTOR + HELI_ADJUST_PITCH; // Convert stick position to absolute angles command_rx_pitch = ch_pitch / HELI_STICK_TO_ANGLE_FACTOR; // Convert stick position to absolute angles
command_rx_collective = ch_collective; command_rx_collective = ch_collective;
command_rx_yaw = ch_yaw / HELI_YAW_STICK_TO_ANGLE_FACTOR; // Convert stick position to turn rate command_rx_yaw = (ch_yaw-trim_yaw) / HELI_YAW_STICK_TO_ANGLE_FACTOR; // Convert stick position to turn rate
// hardcode flight mode // hardcode flight mode
flightMode = STABLE_MODE; flightMode = STABLE_MODE;
@ -255,7 +311,9 @@ void heli_attitude_control(int command_roll, int command_pitch, int command_coll
// always pass through collective command // always pass through collective command
control_collective = command_rx_collective; control_collective = command_rx_collective;
// ROLL CONTROL -- TWO PIDS - ANGLE CONTROL + RATE CONTROL // ROLL CONTROL -- ONE PID
if( roll_control_switch )
{
// P term // P term
err_roll = command_roll - ToDeg(roll); err_roll = command_roll - ToDeg(roll);
err_roll = constrain(err_roll,-25,25); // to limit max roll command... err_roll = constrain(err_roll,-25,25); // to limit max roll command...
@ -269,8 +327,14 @@ void heli_attitude_control(int command_roll, int command_pitch, int command_coll
// PID control // PID control
control_roll = KP_QUAD_ROLL*err_roll + roll_I + roll_D; control_roll = KP_QUAD_ROLL*err_roll + roll_I + roll_D;
control_roll = constrain(control_roll,-50,50); control_roll = constrain(control_roll,-50,50);
}else{
// straight pass through
control_roll = ch_roll;
}
// PITCH CONTROL -- TWO PIDS - ANGLE CONTROL + RATE CONTROL // PITCH CONTROL -- ONE PIDS
if( pitch_control_switch )
{
// P term // P term
err_pitch = command_pitch - ToDeg(pitch); err_pitch = command_pitch - ToDeg(pitch);
err_pitch = constrain(err_pitch,-25,25); // to limit max pitch command... err_pitch = constrain(err_pitch,-25,25); // to limit max pitch command...
@ -283,9 +347,13 @@ void heli_attitude_control(int command_roll, int command_pitch, int command_coll
// PID control // PID control
control_pitch = KP_QUAD_PITCH*err_pitch + pitch_I + pitch_D; control_pitch = KP_QUAD_PITCH*err_pitch + pitch_I + pitch_D;
control_pitch = constrain(control_pitch,-50,50); control_pitch = constrain(control_pitch,-50,50);
}else{
control_pitch = ch_pitch;
}
// YAW CONTROL // YAW CONTROL
if( yaw_control_switch )
{
if( command_yaw == 0 ) // heading hold mode if( command_yaw == 0 ) // heading hold mode
{ {
// check we don't need to reset targetHeading // check we don't need to reset targetHeading
@ -302,6 +370,7 @@ void heli_attitude_control(int command_roll, int command_pitch, int command_coll
err_heading = Normalize_angle(targetHeading - ToDeg(yaw)); err_heading = Normalize_angle(targetHeading - ToDeg(yaw));
err_heading = constrain(err_heading,-90,90); // don't make it travel any faster beyond 90 degrees err_heading = constrain(err_heading,-90,90); // don't make it travel any faster beyond 90 degrees
// I term
heading_I += err_heading * heli_G_Dt * Ki_RateYaw; heading_I += err_heading * heli_G_Dt * Ki_RateYaw;
heading_I = constrain(heading_I,-20,20); heading_I = constrain(heading_I,-20,20);
@ -319,15 +388,21 @@ void heli_attitude_control(int command_roll, int command_pitch, int command_coll
//currentYawRate = ToDeg(Omega_Vector[2]); <-- makes things very unstable!! //currentYawRate = ToDeg(Omega_Vector[2]); <-- makes things very unstable!!
err_yaw = control_yaw_rate-currentYawRate; err_yaw = control_yaw_rate-currentYawRate;
// I term
yaw_I += err_yaw * heli_G_Dt * KI_QUAD_YAW; yaw_I += err_yaw * heli_G_Dt * KI_QUAD_YAW;
yaw_I = constrain(yaw_I, -20, 20); yaw_I = constrain(yaw_I, -20, 20);
// D term
//yaw_D = currentYawRate - previousYawRate; yaw_D = (currentYawRate-previousYawRate) * STABLE_MODE_KP_RATE_YAW; // Take into account the change in angular velocity
yaw_D = constrain(yaw_D,-25,25);
previousYawRate = currentYawRate; previousYawRate = currentYawRate;
// PID control // PID control
control_yaw = (KP_QUAD_YAW*err_yaw + yaw_I); control_yaw = trim_yaw + (KP_QUAD_YAW*err_yaw + yaw_I + yaw_D); // add back in the yaw trim so that it is our center point
control_yaw = constrain(control_yaw,-50,50); control_yaw = constrain(control_yaw,-50,50);
}else{
// straight pass through
control_yaw = ch_yaw;
}
} }
#endif // #if AIRFRAME == HELI #endif // #if AIRFRAME == HELI