Updates to Flip:

Ability to abort flip mode if things go wrong
init function to re-enter flip mode if aborted
This commit is contained in:
Jason Short 2012-06-03 22:15:04 -07:00
parent 72a2ecaa11
commit 60c89ee117
3 changed files with 44 additions and 32 deletions

View File

@ -1459,8 +1459,12 @@ void update_roll_pitch_mode(void)
// hack to do auto_flip - need to remove, no one is using. // hack to do auto_flip - need to remove, no one is using.
#if CH7_OPTION == CH7_FLIP #if CH7_OPTION == CH7_FLIP
if (do_flip){ if (do_flip){
roll_flip(); if(g.rc_1.control_in == 0){
return; roll_flip();
return;
}else{
do_flip = false;
}
} }
#endif #endif

View File

@ -51,9 +51,8 @@ static void read_trim_switch()
{ {
#if CH7_OPTION == CH7_FLIP #if CH7_OPTION == CH7_FLIP
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER && g.rc_3.control_in != 0){ if (g.rc_7.radio_in > CH_7_PWM_TRIGGER && g.rc_3.control_in != 0){
do_flip = true; init_flip();
} }
#elif CH7_OPTION == CH7_SET_HOVER #elif CH7_OPTION == CH7_SET_HOVER
// switch is engaged // switch is engaged
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){
@ -142,7 +141,7 @@ static void read_trim_switch()
set_cmd_with_index(current_loc, CH7_wp_index); set_cmd_with_index(current_loc, CH7_wp_index);
copter_leds_nav_blink = 10; // Cause the CopterLEDs to blink twice to indicate saved waypoint copter_leds_nav_blink = 10; // Cause the CopterLEDs to blink twice to indicate saved waypoint
// 0 = home // 0 = home
// 1 = takeoff // 1 = takeoff
// 2 = WP 2 // 2 = WP 2

View File

@ -7,35 +7,44 @@
// Some states are fixed commands (until some IMU condition) // Some states are fixed commands (until some IMU condition)
// Some states include controls inside // Some states include controls inside
#if CH7_OPTION == CH7_FLIP #if CH7_OPTION == CH7_FLIP
uint8_t flip_timer;
uint8_t flip_state;
#define AAP_THR_INC 170
#define AAP_THR_DEC 90
#define AAP_ROLL_OUT 2000
void init_flip()
{
if(do_flip == false){
do_flip = true;
flip_timer = 0;
flip_state = 0;
}
}
void roll_flip() void roll_flip()
{ {
#define AAP_THR_INC 180
#define AAP_THR_DEC 90
#define AAP_ROLL_OUT 2000
#define AAP_ROLL_RATE 3000 // up to 1250
static int AAP_timer = 0;
static byte AAP_state = 0;
// Yaw // Yaw
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
// Pitch // Pitch
g.rc_2.servo_out = get_stabilize_pitch(0); g.rc_2.servo_out = get_stabilize_pitch(0);
// State machine // Roll State machine
switch (AAP_state){ switch (flip_state){
case 0: // Step 1 : Initialize case 0: // Step 1 : Initialize
AAP_timer = 0; flip_timer = 0;
AAP_state++; flip_state++;
break; break;
case 1: // Step 2 : Increase throttle to start maneuver case 1: // Step 2 : Increase throttle to start maneuver
if (AAP_timer < 95){ // .5 seconds if (flip_timer < 95){ // .5 seconds
g.rc_1.servo_out = get_stabilize_roll(0); g.rc_1.servo_out = get_stabilize_roll(0);
g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC;
AAP_timer++; flip_timer++;
}else{ }else{
AAP_state++; flip_state++;
AAP_timer = 0; flip_timer = 0;
} }
break; break;
@ -43,35 +52,35 @@ void roll_flip()
if (ahrs.roll_sensor < 4500){ if (ahrs.roll_sensor < 4500){
// Roll control // Roll control
g.rc_1.servo_out = AAP_ROLL_OUT; g.rc_1.servo_out = AAP_ROLL_OUT;
g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC; g.rc_3.servo_out = g.rc_3.control_in;
}else{ }else{
AAP_state++; flip_state++;
} }
break; break;
case 3: // Step 4 : CONTINUE ROLL (until we reach a certain angle [-45deg]) case 3: // Step 4 : CONTINUE ROLL (until we reach a certain angle [-45deg])
if ((ahrs.roll_sensor >= 4500) || (ahrs.roll_sensor < -4500)){ if((ahrs.roll_sensor >= 4500) || (ahrs.roll_sensor < -9000)){// we are in second half of roll
g.rc_1.servo_out = 0; g.rc_1.servo_out = 0;
g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC; g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC;
}else{ }else{
AAP_state++; flip_state++;
} }
break; break;
case 4: // Step 5 : Increase throttle to stop the descend case 4: // Step 5 : Increase throttle to stop the descend
if (AAP_timer < 90){ // .5 seconds if (flip_timer < 90){ // .5 seconds
g.rc_1.servo_out = get_stabilize_roll(0); g.rc_1.servo_out = get_stabilize_roll(0);
g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC + 30;
AAP_timer++; flip_timer++;
}else{ }else{
AAP_state++; flip_state++;
AAP_timer = 0; flip_timer = 0;
} }
break; break;
case 5: // exit mode case 5: // exit mode
AAP_timer = 0; flip_timer = 0;
AAP_state = 0; flip_state = 0;
do_flip = false; do_flip = false;
break; break;
} }