mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
7d5b975da2
Flight tested it today and made some flips. Throttle adjusted in Manual throttle modes. Not tested in AP throttle modes such as alt hold.
83 lines
2.1 KiB
Plaintext
83 lines
2.1 KiB
Plaintext
// 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(ap.manual_throttle){
|
|
// 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 = 30000 * flip_dir;
|
|
#else
|
|
roll_rate_target_bf = 30000 * flip_dir;
|
|
#endif
|
|
// decrease throttle while inverted
|
|
if(ap.manual_throttle){
|
|
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(ap.manual_throttle){
|
|
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;
|
|
}
|
|
}
|