Fixed do_flip,

This commit is contained in:
Jason Short 2011-09-20 23:20:33 -07:00
parent 94313ea88f
commit 00fe8e0450
3 changed files with 8 additions and 10 deletions

View File

@ -345,7 +345,7 @@ static float simple_sin_y, simple_cos_x;
static float boost; // used to give a little extra to maintain altitude
// Acro
#if CH7_OPTION == DO_FLIP
#if CH7_OPTION == CH7_FLIP
static bool do_flip = false;
#endif
@ -1029,7 +1029,7 @@ void update_yaw_mode(void)
void update_roll_pitch_mode(void)
{
#if CH7_OPTION == DO_FLIP
#if CH7_OPTION == CH7_FLIP
if (do_flip){
roll_flip();
return;

View File

@ -425,11 +425,7 @@ static bool verify_loiter_unlim()
static bool verify_loiter_time()
{
//Serial.printf("vlt %ld\n",(millis() - loiter_time));
if ((millis() - loiter_time) > loiter_time_max) { // scale loiter_time_max from (sec*10) to milliseconds
//gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
//Serial.println("vlt done");
if ((millis() - loiter_time) > loiter_time_max) {
return true;
}
return false;
@ -578,6 +574,8 @@ static bool verify_yaw()
// time out
// make sure we hold at the final desired yaw angle
nav_yaw = command_yaw_end;
auto_yaw = nav_yaw;
//Serial.println("Y");
return true;

View File

@ -6,7 +6,7 @@
// Some states are fixed commands (for a fixed time)
// Some states are fixed commands (until some IMU condition)
// Some states include controls inside
#if CH7_OPTION == DO_FLIP
#if CH7_OPTION == CH7_FLIP
void roll_flip()
{
#define AAP_THR_INC 180
@ -18,7 +18,7 @@ void roll_flip()
static byte AAP_state = 0;
// Yaw
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
// Pitch
g.rc_2.servo_out = get_stabilize_pitch(0);
@ -33,7 +33,7 @@ void roll_flip()
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);
//g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
AAP_timer++;
}else{
AAP_state++;