Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added Jose Julio's Flip code, edited to work with AC2. Never flown or tested! 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3019 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-08-05 16:17:26 +00:00
parent 1e4f463298
commit a78b057cec
1 changed files with 78 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
ArduCopterMega/flip.pde Normal file
View File

@ -0,0 +1,78 @@
// 2010 Jose Julio
// 2011 Adapted 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
void roll_flip()
{
#define AAP_THR_INC 180
#define AAP_THR_DEC 90
#define AAP_ROLL_OUT 200
#define AAP_ROLL_RATE 3000 // up to 1250
static int AAP_timer = 0;
static byte AAP_state = 0;
// Yaw
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
// Pitch
g.rc_2.servo_out = get_stabilize_pitch(0);
// State machine
switch (AAP_state){
case 0: // Step 1 : Initialize
AAP_timer = 0;
AAP_state++;
break;
case 1: // Step 2 : Increase throttle to start maneuver
if (AAP_timer < 95){ // .5 seconds
g.rc_1.servo_out = get_stabilize_roll(0);
g.rc_3.servo_out = get_throttle(g.rc_3.control_in + AAP_THR_INC);
//g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
AAP_timer++;
}else{
AAP_state++;
AAP_timer = 0;
}
break;
case 2: // Step 3 : ROLL (until we reach a certain angle [45º])
if (dcm.roll_sensor < 4500){
// Roll control
g.rc_1.servo_out = AAP_ROLL_OUT; //get_rate_roll(AAP_ROLL_RATE);
g.rc_3.servo_out = get_throttle(g.rc_3.control_in - AAP_THR_DEC);
}else{
AAP_state++;
}
break;
case 3: // Step 4 : CONTINUE ROLL (until we reach a certain angle [-45º])
if ((dcm.roll_sensor >= 4500) || (dcm.roll_sensor < -4500)){
g.rc_1.servo_out = 150; //get_rate_roll(AAP_ROLL_RATE);
g.rc_3.servo_out = get_throttle(g.rc_3.control_in - AAP_THR_DEC);
}else{
AAP_state++;
}
break;
case 4: // Step 5 : Increase throttle to stop the descend
if (AAP_timer < 90){ // .5 seconds
g.rc_1.servo_out = get_stabilize_roll(0);
g.rc_3.servo_out = get_throttle(g.rc_3.control_in + AAP_THR_INC);
AAP_timer++;
}else{
AAP_state++;
AAP_timer = 0;
}
break;
case 5: // exit mode
//control_mode =
do_flip = false;
break;
}
}