1 changed files with 0 additions and 82 deletions
@ -1,82 +0,0 @@
@@ -1,82 +0,0 @@
|
||||
// 2010 Jose Julio |
||||
// 2011 Adapted and updated for AC2 by Jason Short |
||||
// |
||||
// Automatic Acrobatic Procedure (AAP) v1 : Roll flip |
||||
// State machine aproach: |
||||
// Some states are fixed commands (for a fixed time) |
||||
// Some states are fixed commands (until some IMU condition) |
||||
// Some states include controls inside |
||||
uint8_t flip_timer; |
||||
uint8_t flip_state; |
||||
|
||||
#define AAP_THR_INC 170 |
||||
#define AAP_THR_DEC 120 |
||||
#define AAP_ROLL_OUT 2000 |
||||
|
||||
static int8_t flip_dir; |
||||
|
||||
void init_flip() |
||||
{ |
||||
if(false == ap.do_flip) { |
||||
ap.do_flip = true; |
||||
flip_state = 0; |
||||
flip_dir = (ahrs.roll_sensor >= 0) ? 1 : -1; |
||||
Log_Write_Event(DATA_BEGIN_FLIP); |
||||
} |
||||
} |
||||
|
||||
void roll_flip() |
||||
{ |
||||
int32_t roll = ahrs.roll_sensor * flip_dir; |
||||
|
||||
// Roll State machine |
||||
switch (flip_state) { |
||||
case 0: |
||||
if (roll < 4500) { |
||||
// Roll control |
||||
roll_rate_target_bf = 40000 * flip_dir; |
||||
if (throttle_mode_manual(throttle_mode)){ |
||||
// increase throttle right before flip |
||||
set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false); |
||||
} |
||||
}else{ |
||||
flip_state++; |
||||
} |
||||
break; |
||||
|
||||
case 1: |
||||
if((roll >= 4500) || (roll < -9000)) { |
||||
#if FRAME_CONFIG == HELI_FRAME |
||||
roll_rate_target_bf = 40000 * flip_dir; |
||||
#else |
||||
roll_rate_target_bf = 40000 * flip_dir; |
||||
#endif |
||||
// decrease throttle while inverted |
||||
if (throttle_mode_manual(throttle_mode)){ |
||||
set_throttle_out(g.rc_3.control_in - AAP_THR_DEC, false); |
||||
} |
||||
}else{ |
||||
flip_state++; |
||||
flip_timer = 0; |
||||
} |
||||
break; |
||||
|
||||
case 2: |
||||
// 100 = 1 second with 100hz |
||||
if (flip_timer < 100) { |
||||
// we no longer need to adjust the roll_rate. |
||||
// It will be handled by normal flight control loops |
||||
|
||||
// increase throttle to gain any lost alitude |
||||
if (throttle_mode_manual(throttle_mode)){ |
||||
set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false); |
||||
} |
||||
flip_timer++; |
||||
}else{ |
||||
Log_Write_Event(DATA_END_FLIP); |
||||
ap.do_flip = false; |
||||
flip_state = 0; |
||||
} |
||||
break; |
||||
} |
||||
} |
Loading…
Reference in new issue