You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
428 lines
15 KiB
428 lines
15 KiB
/* |
|
www.ArduCopter.com - www.DIYDrones.com |
|
Copyright (c) 2010. All rights reserved. |
|
An Open Source Arduino based multicopter. |
|
|
|
File : Heli.pde |
|
Desc : code specific to traditional helicopters |
|
Version : v1.0, Aug 27, 2010 |
|
Author(s): ArduCopter Team |
|
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, |
|
Jani Hirvinen, Ken McEwans, Roberto Navoni, |
|
Sandro Benigno, Chris Anderson |
|
|
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
|
|
* ************************************************************** * |
|
ChangeLog: |
|
|
|
|
|
* ************************************************************** * |
|
TODO: |
|
|
|
|
|
* ************************************************************** */ |
|
|
|
#if AIRFRAME == HELI |
|
|
|
/**********************************************************************/ |
|
// heli_readUserConfig - reads values in from EEPROM |
|
void heli_readUserConfig() |
|
{ |
|
float magicNum = 0; |
|
magicNum = readEEPROM(EEPROM_MAGIC_NUMBER_ADDR); |
|
if( magicNum != EEPROM_MAGIC_NUMBER ) { |
|
SerPri("No heli settings found in EEPROM. Using defaults"); |
|
heli_defaultUserConfig(); |
|
}else{ |
|
frontLeftCCPMmin = readEEPROM(FRONT_LEFT_CCPM_MIN_ADDR); |
|
frontLeftCCPMmax = readEEPROM(FRONT_LEFT_CCPM_MAX_ADDR); |
|
frontRightCCPMmin = readEEPROM(FRONT_RIGHT_CCPM_MIN_ADDR); |
|
frontRightCCPMmax = readEEPROM(FRONT_RIGHT_CCPM_MAX_ADDR); |
|
rearCCPMmin = readEEPROM(REAR_CCPM_MIN_ADDR); |
|
rearCCPMmax = readEEPROM(REAR_CCPM_MAX_ADDR); |
|
yawMin = readEEPROM(YAW_MIN_ADDR); |
|
yawMax = readEEPROM(YAW_MAX_ADDR); |
|
} |
|
} |
|
|
|
/**********************************************************************/ |
|
// default the heli specific values to defaults |
|
void heli_defaultUserConfig() |
|
{ |
|
// default CCPM values. |
|
frontLeftCCPMmin = 1200; |
|
frontLeftCCPMmax = 1800; |
|
frontRightCCPMmin = 1900; |
|
frontRightCCPMmax = 1100; |
|
rearCCPMmin = 1200; |
|
rearCCPMmax = 1800; |
|
yawMin = 1200; |
|
yawMax = 1800; |
|
|
|
// default PID values - Roll |
|
KP_QUAD_ROLL = 1.100; |
|
KI_QUAD_ROLL = 0.200; |
|
STABLE_MODE_KP_RATE_ROLL = -0.001; |
|
|
|
// default PID values - Pitch |
|
KP_QUAD_PITCH = 1.100; |
|
KI_QUAD_PITCH = 0.120; |
|
STABLE_MODE_KP_RATE_PITCH = -0.001; |
|
|
|
// default PID values - Yaw |
|
Kp_RateYaw = 3.500; // heading P term |
|
Ki_RateYaw = 0.100; // heading I term |
|
KP_QUAD_YAW = 0.200; // yaw rate P term |
|
KI_QUAD_YAW = 0.040; // yaw rate I term |
|
STABLE_MODE_KP_RATE_YAW = -0.010; // yaw rate D term |
|
} |
|
|
|
/**********************************************************************/ |
|
// displaySettings - displays heli specific user settings |
|
void heli_displaySettings() |
|
{ |
|
SerPri("frontLeftCCPM min: "); |
|
SerPri(frontLeftCCPMmin); |
|
SerPri("\tmax:"); |
|
SerPri(frontLeftCCPMmax); |
|
|
|
if( abs(frontLeftCCPMmin-frontLeftCCPMmax)<50 || frontLeftCCPMmin < 900 || frontLeftCCPMmin > 2100 || frontLeftCCPMmax < 900 || frontLeftCCPMmax > 2100 ) |
|
SerPrln("\t\t<-- check"); |
|
else |
|
SerPrln(); |
|
|
|
SerPri("frontRightCCPM min: "); |
|
SerPri(frontRightCCPMmin); |
|
SerPri("\tmax:"); |
|
SerPri(frontRightCCPMmax); |
|
if( abs(frontRightCCPMmin-frontRightCCPMmax)<50 || frontRightCCPMmin < 900 || frontRightCCPMmin > 2100 || frontRightCCPMmax < 900 || frontRightCCPMmax > 2100 ) |
|
SerPrln("\t\t<-- check"); |
|
else |
|
SerPrln(); |
|
|
|
SerPri("rearCCPM min: "); |
|
SerPri(rearCCPMmin); |
|
SerPri("\tmax:"); |
|
SerPri(rearCCPMmax); |
|
if( abs(rearCCPMmin-rearCCPMmax)<50 || rearCCPMmin < 900 || rearCCPMmin > 2100 || rearCCPMmax < 900 || rearCCPMmax > 2100 ) |
|
SerPrln("\t\t<-- check"); |
|
else |
|
SerPrln(); |
|
|
|
SerPri("yaw min: "); |
|
SerPri(yawMin); |
|
SerPri("\tmax:"); |
|
SerPri(yawMax); |
|
if( abs(yawMin-yawMax)<50 || yawMin < 900 || yawMin > 2100 || yawMax < 900 || yawMax > 2100 ) |
|
SerPrln("\t\t<-- check"); |
|
else |
|
SerPrln(); |
|
|
|
SerPrln(); |
|
} |
|
|
|
//////////////////////////////////////////////////////////////////////////////// |
|
// Setup Procedure |
|
//////////////////////////////////////////////////////////////////////////////// |
|
void heli_setup() |
|
{ |
|
// read heli specific settings (like CCPM values) from EEPROM |
|
heli_readUserConfig(); |
|
|
|
// update CCPM values |
|
frontLeftCCPMslope = 100 / (frontLeftCCPMmax - frontLeftCCPMmin); |
|
frontLeftCCPMintercept = 100 - (frontLeftCCPMslope * frontLeftCCPMmax); |
|
frontRightCCPMslope = 100 / (frontRightCCPMmax - frontRightCCPMmin); |
|
frontRightCCPMintercept = 100 - (frontRightCCPMslope * frontRightCCPMmax); |
|
rearCCPMslope = 100 / (rearCCPMmax - rearCCPMmin); |
|
rearCCPMintercept = 100 - (rearCCPMslope * rearCCPMmax); |
|
yawSlope = 100 / (yawMax - yawMin); |
|
yawIntercept = 50 - (yawSlope * yawMax); |
|
|
|
// capture trims |
|
heli_read_radio_trims(); |
|
|
|
// hardcode mids because we will use ccpm |
|
roll_mid = ROLL_MID; |
|
pitch_mid = PITCH_MID; |
|
collective_mid = 1500; |
|
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; |
|
//position_control_switch = !SW_DIP4; // switch 4 controls whether we will do GPS hold or not |
|
} |
|
|
|
/*****************************************************************************************************/ |
|
// 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; |
|
|
|
} |
|
|
|
/**********************************************************************/ |
|
// Radio decoding |
|
void heli_read_radio() |
|
{ |
|
// left channel |
|
ccpmPercents.x = frontLeftCCPMslope * APM_RC.InputCh(CHANNEL_FRONT_LEFT) + frontLeftCCPMintercept; |
|
|
|
// right channel |
|
ccpmPercents.y = frontRightCCPMslope * APM_RC.InputCh(CHANNEL_FRONT_RIGHT) + frontRightCCPMintercept; |
|
|
|
// rear channel |
|
ccpmPercents.z = rearCCPMslope * APM_RC.InputCh(CHANNEL_REAR) + rearCCPMintercept; |
|
|
|
// decode the ccpm |
|
rollPitchCollPercent = ccpmDeallocation * ccpmPercents; |
|
|
|
// get the yaw (not coded) |
|
yawPercent = (yawSlope * APM_RC.InputCh(CHANNEL_YAW) + yawIntercept) - trim_yaw; |
|
|
|
// put decoded values into the global variables |
|
ch_roll = rollPitchCollPercent.x; |
|
ch_pitch = rollPitchCollPercent.y; |
|
ch_collective = rollPitchCollPercent.z; |
|
|
|
// allow a bit of a dead zone for the yaw |
|
if( fabs(yawPercent) < 2 ) |
|
ch_yaw = 0; |
|
else |
|
ch_yaw = yawPercent; |
|
|
|
// get the aux channel (for tuning on/off autopilot) |
|
ch_aux = APM_RC.InputCh(CH_5) * ch_aux_slope + ch_aux_offset; |
|
|
|
// convert 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; // Convert stick position to absolute angles |
|
command_rx_collective = ch_collective; |
|
command_rx_yaw = ch_yaw / HELI_YAW_STICK_TO_ANGLE_FACTOR; // Convert stick position to turn rate |
|
|
|
// for use by non-heli parts of code |
|
ch_throttle = 1000 + (ch_collective * 10); |
|
|
|
// hardcode flight mode |
|
flightMode = STABLE_MODE; |
|
|
|
// Autopilot mode (only works on Stable mode) |
|
if (flightMode == STABLE_MODE) |
|
{ |
|
if(ch_aux < 1300) { |
|
AP_mode = AP_AUTOMATIC_MODE; // Automatic mode : GPS position hold mode + altitude hold |
|
//SerPrln("autopilot ON!"); |
|
}else{ |
|
AP_mode = AP_NORMAL_MODE; // Normal mode |
|
//SerPrln("autopilot OFF!"); |
|
} |
|
} |
|
} |
|
|
|
/**********************************************************************/ |
|
// output to swash plate based on control variables |
|
// Uses these global variables: |
|
// control_roll : -50 ~ 50 |
|
// control_pitch : -50 ~ 50 |
|
// control_collective : 0 ~ 100 |
|
// control_yaw : -50 ~ 50 |
|
void heli_moveSwashPlate() |
|
{ |
|
static int count = 0; |
|
// turn pitch, roll, collective commands into ccpm values (i.e. values for each servo) |
|
ccpmPercents_out = ccpmAllocation * Vector3f(control_roll, control_pitch, control_collective); |
|
|
|
// calculate values to be sent out to RC Output channels |
|
leftOut = (ccpmPercents_out.x - frontLeftCCPMintercept) / frontLeftCCPMslope; |
|
rightOut = (ccpmPercents_out.y - frontRightCCPMintercept) / frontRightCCPMslope; |
|
rearOut = (ccpmPercents_out.z - rearCCPMintercept) / rearCCPMslope; |
|
yawOut = (control_yaw - yawIntercept) / yawSlope; |
|
|
|
APM_RC.OutputCh(CHANNEL_FRONT_LEFT,leftOut); |
|
APM_RC.OutputCh(CHANNEL_FRONT_RIGHT,rightOut); |
|
APM_RC.OutputCh(CHANNEL_REAR,rearOut); |
|
APM_RC.OutputCh(CHANNEL_YAW,yawOut); |
|
// InstantPWM |
|
APM_RC.Force_Out0_Out1(); |
|
APM_RC.Force_Out2_Out3(); |
|
} |
|
|
|
/**********************************************************************/ |
|
// ROLL, PITCH and YAW PID controls... |
|
// Input : desired Roll, Pitch absolute angles |
|
// collective as a percentage from 0~100 |
|
// yaw as a rate of rotation |
|
// Output : control_roll - roll servo as a percentage (-50 to 50) |
|
// control_pitch - pitch servo as a percentage (-50 to 50) |
|
// control_collective - collective servo as a percentage (0 to 100) |
|
// control_yaw - yaw servo as a percentage (0 to 100) |
|
void heli_attitude_control(int command_roll, int command_pitch, int command_collective, int command_yaw) |
|
{ |
|
static float firstIteration = 1; |
|
static float command_yaw_previous = 0; |
|
static float previousYawRate = 0; |
|
float stable_roll, stable_pitch; |
|
float currentYawRate; |
|
float control_yaw_rate; |
|
float err_heading; |
|
static int aCounter = 0; |
|
float heli_G_Dt; |
|
|
|
// get current time |
|
heli_G_Dt = (currentTimeMicros-heli_previousTimeMicros) * 0.000001; // Microseconds!!! |
|
heli_previousTimeMicros = currentTimeMicros; |
|
|
|
// always pass through collective command |
|
control_collective = command_collective; |
|
|
|
// ROLL CONTROL -- ONE PID |
|
if( roll_control_switch ) |
|
{ |
|
// P term |
|
err_roll = command_roll - ToDeg(roll); |
|
err_roll = constrain(err_roll,-25,25); // to limit max roll command... |
|
// I term |
|
roll_I += err_roll*heli_G_Dt*KI_QUAD_ROLL; |
|
roll_I = constrain(roll_I,-10,10); |
|
// D term |
|
roll_D = ToDeg(Omega[0]) * STABLE_MODE_KP_RATE_ROLL; // Take into account Angular velocity |
|
roll_D = constrain(roll_D,-25,25); |
|
|
|
// PID control |
|
control_roll = KP_QUAD_ROLL*err_roll + roll_I + roll_D; |
|
control_roll = constrain(control_roll,-50,50); |
|
}else{ |
|
// straight pass through |
|
control_roll = ch_roll; |
|
} |
|
|
|
// PITCH CONTROL -- ONE PIDS |
|
if( pitch_control_switch ) |
|
{ |
|
// P term |
|
err_pitch = command_pitch - ToDeg(pitch); |
|
err_pitch = constrain(err_pitch,-25,25); // to limit max pitch command... |
|
// I term |
|
pitch_I += err_pitch * heli_G_Dt * KI_QUAD_PITCH; |
|
pitch_I = constrain(pitch_I,-10,10); |
|
// D term |
|
pitch_D = ToDeg(Omega[1]) * STABLE_MODE_KP_RATE_PITCH; // Take into account Angular velocity |
|
pitch_D = constrain(pitch_D,-25,25); |
|
// PID control |
|
control_pitch = KP_QUAD_PITCH*err_pitch + pitch_I + pitch_D; |
|
control_pitch = constrain(control_pitch,-50,50); |
|
}else{ |
|
control_pitch = ch_pitch; |
|
} |
|
|
|
// YAW CONTROL |
|
if( yaw_control_switch ) |
|
{ |
|
if( command_yaw == 0 ) // heading hold mode |
|
{ |
|
// check we don't need to reset targetHeading |
|
if( command_yaw_previous != 0 ) |
|
targetHeading = ToDeg(yaw); |
|
|
|
// ensure reasonable targetHeading |
|
if( firstIteration || targetHeading > 180 || targetHeading < -180 ) |
|
{ |
|
firstIteration = 0; |
|
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 |
|
|
|
// I term |
|
heading_I += err_heading * heli_G_Dt * Ki_RateYaw; |
|
heading_I = constrain(heading_I,-20,20); |
|
|
|
// PID control - a bit bad - we're using the acro mode's PID values because there's not PID for heading |
|
control_yaw_rate = Kp_RateYaw*err_heading + heading_I; |
|
control_yaw_rate = constrain(control_yaw_rate,-100,100); // to limit max yaw command |
|
}else{ // rate mode |
|
err_heading = 0; |
|
control_yaw_rate = command_yaw; |
|
} |
|
command_yaw_previous = command_yaw; |
|
|
|
// YAW RATE CONTROL |
|
currentYawRate = ToDeg(Gyro_Scaled_Z(read_adc(2))); |
|
//currentYawRate = ToDeg(Omega_Vector[2]); <-- makes things very unstable!! |
|
err_yaw = control_yaw_rate-currentYawRate; |
|
|
|
// I term |
|
yaw_I += err_yaw * heli_G_Dt * KI_QUAD_YAW; |
|
yaw_I = constrain(yaw_I, -20, 20); |
|
// D term |
|
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; |
|
|
|
// PID control |
|
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); |
|
}else{ |
|
// straight pass through |
|
control_yaw = ch_yaw; |
|
} |
|
|
|
Log_Write_PID(7,KP_QUAD_ROLL*err_roll,roll_I,roll_D,control_roll); |
|
Log_Write_PID(8,KP_QUAD_PITCH*err_pitch,pitch_I,pitch_D,control_pitch); |
|
Log_Write_PID(9,Kp_RateYaw*err_heading,heading_I,0,control_yaw_rate); |
|
Log_Write_PID(10,KP_QUAD_YAW*err_yaw,yaw_I,yaw_D,control_yaw); |
|
} |
|
|
|
#endif // #if AIRFRAME == HELI
|
|
|