zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: add pitch axis flipping 

Added Pitch axis flipping. You must be pushing forward or back 3° to make it flip in Pitch axis or it will flip in the default roll axis.
Added some extra throttle control to maintain altitude
removed the CH7 ability to exit Flip mode. It will exit only if completing flip or user aborts with control sticks.
mission-4.1.18
Jason Short 11 years ago committed by Randy Mackay
parent
5a66ff1ef9
commit
882edaf787
3 changed files with 81 additions and 20 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 97
      ArduCopter/control_flip.pde
  2. 2
      ArduCopter/defines.h
  3. 2
      ArduCopter/switches.pde

97
ArduCopter/control_flip.pde
Unescape View File

@ -18,19 +18,23 @@ @@ -18,19 +18,23 @@
* Flip_Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude
*/
#define FLIP_THR_INC 170 // throttle increase during Flip_Start stage (under 45deg lean angle)
#define FLIP_THR_DEC 120 // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll)
#define FLIP_ROLL_RATE 40000 // roll rate request in centi-degrees / sec (i.e. 400 deg/sec)
#define FLIP_THR_INC 200 // throttle increase during Flip_Start stage (under 45deg lean angle)
#define FLIP_THR_DEC 240 // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll)
#define FLIP_ROTATION_RATE 40000 // rotation rate request in centi-degrees / sec (i.e. 400 deg/sec)
#define FLIP_TIMEOUT_MS 2500 // timeout after 2.5sec. Vehicle will switch back to original flight mode
#define FLIP_RECOVERY_ANGLE 500 // consider successful recovery when roll is back within 5 degrees of original
#define FLIP_ROLL_RIGHT 1 // used to set flip_dir
#define FLIP_ROLL_LEFT -1 // used to set flip_dir
#define FLIP_PITCH_BACK 1 // used to set flip_dir
#define FLIP_PITCH_FORWARD -1 // used to set flip_dir
FlipState flip_state; // current state of flip
uint8_t flip_orig_control_mode; // flight mode when flip was initated
uint32_t flip_start_time; // time since flip began
int8_t flip_dir; // roll direction (-1 = roll left, 1 = roll right)
int8_t flip_roll_dir; // roll direction (-1 = roll left, 1 = roll right)
int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back)
// flip_init - initialise flip controller
static bool flip_init(bool ignore_checks)
@ -61,12 +65,20 @@ static bool flip_init(bool ignore_checks) @@ -61,12 +65,20 @@ static bool flip_init(bool ignore_checks)
// initialise state
flip_state = Flip_Start;
flip_start_time = millis();
// choose direction based on pilot's roll stick
flip_roll_dir = flip_pitch_dir = 0;
// choose direction based on pilot's roll and pitch sticks
if (g.rc_2.control_in > 300) {
flip_pitch_dir = FLIP_PITCH_BACK;
}else if(g.rc_2.control_in < -300) {
flip_pitch_dir = FLIP_PITCH_FORWARD;
}else{
if (g.rc_1.control_in >= 0) {
flip_dir = FLIP_ROLL_RIGHT;
flip_roll_dir = FLIP_ROLL_RIGHT;
}else{
flip_dir = FLIP_ROLL_LEFT;
flip_roll_dir = FLIP_ROLL_LEFT;
}
}
// log start of flip
@ -103,39 +115,80 @@ static void flip_stop() @@ -103,39 +115,80 @@ static void flip_stop()
static void flip_run()
{
int16_t throttle_out;
float recovery_angle;
// if pilot inputs roll > 40deg or timeout occurs abandon flip
if (!motors.armed() || (abs(g.rc_1.control_in) >= 4000) || ((millis() - flip_start_time) > FLIP_TIMEOUT_MS)) {
if (!motors.armed() || (abs(g.rc_1.control_in) >= 4000) || (abs(g.rc_2.control_in) >= 4000) || ((millis() - flip_start_time) > FLIP_TIMEOUT_MS)) {
flip_state = Flip_Abandon;
}
// get pilot's desired throttle
throttle_out = get_pilot_desired_throttle(g.rc_3.control_in);
// get roll rate
int32_t roll_angle = ahrs.roll_sensor * flip_dir;
// get corrected angle based on direction and axis of rotation
// we flip the sign of flip_angle to minimize the code repetition
int32_t flip_angle;
if (flip_roll_dir != 0) {
flip_angle = ahrs.roll_sensor * flip_roll_dir;
} else {
flip_angle = ahrs.pitch_sensor * flip_pitch_dir;
}
// state machine
switch (flip_state) {
case Flip_Start:
// under 45 degrees request 400deg/sec roll
attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROLL_RATE * flip_dir, 0.0, 0.0);
// under 45 degrees request 400deg/sec roll or pitch
attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// increase throttle
throttle_out += FLIP_THR_INC;
// beyond 45deg lean angle move to next stage
if (roll_angle >= 4500) {
if (flip_angle >= 4500) {
if (flip_roll_dir != 0) {
// we are rolling
flip_state = Flip_Roll;
} else {
// we are pitching
flip_state = Flip_Pitch_A;
}
}
break;
case Flip_Roll:
// between 45deg ~ -90deg request 400deg/sec roll
attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROLL_RATE * flip_dir, 0.0, 0.0);
attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0);
// decrease throttle
throttle_out -= FLIP_THR_DEC;
// beyond -90deg move on to recovery
if((roll_angle < 4500) && (roll_angle > -9000)) {
if ((flip_angle < 4500) && (flip_angle > -9000)) {
flip_state = Flip_Recover;
}
break;
case Flip_Pitch_A:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control.rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// decrease throttle
throttle_out -= FLIP_THR_DEC;
// check roll for inversion
if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) {
flip_state = Flip_Pitch_B;
}
break;
case Flip_Pitch_B:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control.rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// decrease throttle
throttle_out -= FLIP_THR_DEC;
// check roll for inversion
if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) {
flip_state = Flip_Recover;
}
break;
@ -144,11 +197,19 @@ static void flip_run() @@ -144,11 +197,19 @@ static void flip_run()
// use originally captured earth-frame angle targets to recover
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 altitude
throttle_out += FLIP_THR_INC;
if (flip_roll_dir != 0) {
// we are rolling
recovery_angle = fabs(flip_orig_attitude.x - (float)ahrs.roll_sensor);
} else {
// we are pitching
recovery_angle = fabs(flip_orig_attitude.y - (float)ahrs.pitch_sensor);
}
// check for successful recovery
if (fabs(flip_orig_attitude.x - (float)ahrs.roll_sensor) <= FLIP_RECOVERY_ANGLE) {
if (fabs(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
// restore original flight mode
if (!set_mode(flip_orig_control_mode)) {
// this should never happen but just in case

2
ArduCopter/defines.h
Unescape View File

@ -202,6 +202,8 @@ enum RTLState { @@ -202,6 +202,8 @@ enum RTLState {
enum FlipState {
Flip_Start,
Flip_Roll,
Flip_Pitch_A,
Flip_Pitch_B,
Flip_Recover,
Flip_Abandon
};

2
ArduCopter/switches.pde
Unescape View File

@ -165,8 +165,6 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) @@ -165,8 +165,6 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
// flip if switch is on, positive throttle and we're actually flying
if(ch_flag == AUX_SWITCH_HIGH) {
set_mode(FLIP);
}else{
flip_stop();
}
break;

Write Preview
Loading…
Cancel
Save