Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: flip records starting attitude

Browse Source 
Previously flip relied on the attitude controller not updating the earth
frame target during the flip which can't be guaranteed.  Safer for flip
to maintain it's own copy of the original attitude .
This commit is contained in:
Randy Mackay 2014-06-06 12:07:23 +09:00
parent 75c328a752
commit 4221833028
2 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/ArduCopter.pde
View File

@ -556,6 +556,10 @@ static float acro_level_mix; // scales back roll, pitch and yaw i
static uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) static uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds)
static uint32_t loiter_time; // How long have we been loitering - The start time in millis static uint32_t loiter_time; // How long have we been loitering - The start time in millis
////////////////////////////////////////////////////////////////////////////////
// Flip
////////////////////////////////////////////////////////////////////////////////
static Vector3f flip_orig_attitude; // original copter attitude before flip
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Battery Sensors // Battery Sensors

12
ArduCopter/control_flip.pde
View File

@ -16,7 +16,6 @@
* Flip_Start (while copter is leaning <45deg) : roll right at 400deg/sec, increase throttle * Flip_Start (while copter is leaning <45deg) : roll right at 400deg/sec, increase throttle
* Flip_Roll (while copter is between +45deg ~ -90) : roll right at 400deg/sec, reduce throttle * Flip_Roll (while copter is between +45deg ~ -90) : roll right at 400deg/sec, reduce throttle
* Flip_Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude * Flip_Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude
* Note: this final stage relies upon the AttitudeControl libraries angle_ef_targets having been saved by the attitude_control.init_targets() call and not modified afterwards
*/ */
#define FLIP_THR_INC 170 // throttle increase during Flip_Start stage (under 45deg lean angle) #define FLIP_THR_INC 170 // throttle increase during Flip_Start stage (under 45deg lean angle)
@ -73,9 +72,10 @@ static bool flip_init(bool ignore_checks)
// log start of flip // log start of flip
Log_Write_Event(DATA_FLIP_START); Log_Write_Event(DATA_FLIP_START);
// capture current attitude in angle_ef_targets while will be used as attitude targets during the Flip_Recovery stage // capture current attitude which will be used during the Flip_Recovery stage
// clear stabilized rate errors which will be used during the Flip_Start and Flip_Roll stages flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -aparm.angle_max, aparm.angle_max);
attitude_control.init_targets(); flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -aparm.angle_max, aparm.angle_max);
flip_orig_attitude.z = ahrs.yaw_sensor;
return true; return true;
} }
@ -143,13 +143,13 @@ static void flip_run()
case Flip_Recover: case Flip_Recover:
// use originally captured earth-frame angle targets to recover // use originally captured earth-frame angle targets to recover
attitude_control.angle_ef_roll_pitch_yaw(curr_ef_targets.x, curr_ef_targets.y, curr_ef_targets.z,false); attitude_control.angle_ef_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false);
// increase throttle to gain any lost alitude // increase throttle to gain any lost alitude
throttle_out += FLIP_THR_INC; throttle_out += FLIP_THR_INC;
// check for successful recovery // check for successful recovery
if (fabs(curr_ef_targets.x - (float)ahrs.roll_sensor) <= FLIP_RECOVERY_ANGLE) { if (fabs(flip_orig_attitude.x - (float)ahrs.roll_sensor) <= FLIP_RECOVERY_ANGLE) {
// restore original flight mode // restore original flight mode
if (!set_mode(flip_orig_control_mode)) { if (!set_mode(flip_orig_control_mode)) {
// this should never happen but just in case // this should never happen but just in case