From d2a5928c0662284495d9dd82ff703b4cad3b7e17 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 9 Nov 2012 21:55:51 -0800 Subject: [PATCH] Ap state updates --- ArduCopter/GCS_Mavlink.pde | 2 +- ArduCopter/Parameters.h | 13 ++-- ArduCopter/Parameters.pde | 14 +++- ArduCopter/commands.pde | 14 ++-- ArduCopter/commands_logic.pde | 9 ++- ArduCopter/commands_process.pde | 8 +- ArduCopter/control_modes.pde | 30 ++++---- ArduCopter/defines.h | 74 +++++++++++++++---- ArduCopter/events.pde | 5 +- ArduCopter/failsafe.pde | 7 +- ArduCopter/flip.pde | 8 +- ArduCopter/leds.pde | 64 ++++++++-------- ArduCopter/motors.pde | 15 +++- ArduCopter/navigation.pde | 99 ++++++++++++++++++------- ArduCopter/radio.pde | 4 +- ArduCopter/system.pde | 126 +++++++++++++++----------------- ArduCopter/test.pde | 48 +++--------- ArduCopter/toy.pde | 6 +- 18 files changed, 314 insertions(+), 232 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index aae4f86cbb..7976521f28 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1732,7 +1732,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) current_loc.lng = g_gps->longitude; current_loc.lat = g_gps->latitude; current_loc.alt = g_gps->altitude - gps_base_alt; - if (!home_is_set) { + if (!ap.home_is_set) { init_home(); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 8dc8f60952..ff89bc819b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -69,6 +69,8 @@ public: // Yaw Rate 1 = fast, // 2 = med, 3 = slow + k_param_crosstrack_min_distance, + // 65: AP_Limits Library k_param_limits = 65, k_param_gpslock_limit, @@ -198,9 +200,9 @@ public: // // 220: PI/D Controllers // - k_param_stabilize_d_schedule = 219, - k_param_stabilize_d = 220, - k_param_acro_p, + //k_param_stabilize_d_schedule = 219, + //k_param_stabilize_d = 220, + k_param_acro_p = 221, k_param_axis_lock_p, k_param_pid_rate_roll, k_param_pid_rate_pitch, @@ -269,6 +271,7 @@ public: AP_Int16 loiter_radius; AP_Int16 waypoint_speed_max; AP_Float crosstrack_gain; + AP_Int16 crosstrack_min_distance; AP_Int32 auto_land_timeout; @@ -342,8 +345,8 @@ public: #endif AP_Int16 rc_speed; // speed of fast RC Channels in Hz - AP_Float stabilize_d; - AP_Float stabilize_d_schedule; + //AP_Float stabilize_d; + //AP_Float stabilize_d_schedule; // Acro parameters AP_Float acro_p; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index de26fae1af..030e31f9f0 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -166,6 +166,16 @@ const AP_Param::Info var_info[] PROGMEM = { // @Units: Dimensionless // @User: Standard GSCALAR(crosstrack_gain, "XTRK_GAIN_SC", CROSSTRACK_GAIN), + + // @Param: XTRK_MIN_DIST + // @DisplayName: Crosstrack mininum distance + // @Description: Minimum distance in meters between waypoints to do crosstrack correction. + // @Units: Meters + // @Range: 0 32767 + // @Increment: 1 + // @User: Standard + GSCALAR(crosstrack_min_distance, "XTRK_MIN_DIST", CROSSTRACK_MIN_DISTANCE), + GSCALAR(auto_land_timeout, "AUTO_LAND", AUTO_LAND_TIME*1000), // @Param: THR_MIN @@ -297,7 +307,7 @@ const AP_Param::Info var_info[] PROGMEM = { // variable //--------- - GSCALAR(stabilize_d, "STAB_D", STABILIZE_D), + //GSCALAR(stabilize_d, "STAB_D", STABILIZE_D), // @Param: STAB_D_S // @DisplayName: Stabilize D Schedule @@ -305,7 +315,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Range: 0 1 // @Increment: .01 // @User: Advanced - GSCALAR(stabilize_d_schedule, "STAB_D_S", STABILIZE_D_SCHEDULE), + //GSCALAR(stabilize_d_schedule, "STAB_D_S", STABILIZE_D_SCHEDULE), // Acro parameters GSCALAR(acro_p, "ACRO_P", ACRO_P), diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 3b131b2b13..1c769e8b96 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -2,14 +2,14 @@ static void init_commands() { - g.command_index = NO_COMMAND; - command_nav_index = NO_COMMAND; - command_cond_index = NO_COMMAND; - prev_nav_index = NO_COMMAND; + g.command_index = NO_COMMAND; + command_nav_index = NO_COMMAND; + command_cond_index = NO_COMMAND; + prev_nav_index = NO_COMMAND; command_cond_queue.id = NO_COMMAND; command_nav_queue.id = NO_COMMAND; - fast_corner = false; + ap.fast_corner = false; reset_desired_speed(); // default Yaw tracking @@ -162,7 +162,7 @@ static void set_next_WP(struct Location *wp) set_new_altitude(next_WP.alt); // this is used to offset the shrinking longitude as we go towards the poles - float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; + float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; scaleLongDown = cos(rads); scaleLongUp = 1.0f/cos(rads); @@ -184,7 +184,7 @@ static void set_next_WP(struct Location *wp) // ------------------------------- static void init_home() { - home_is_set = true; + set_home_is_set(true); home.id = MAV_CMD_NAV_WAYPOINT; home.lng = g_gps->longitude; // Lon * 10**7 home.lat = g_gps->latitude; // Lat * 10**7 diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 1d60542012..b4a18692f7 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -278,7 +278,7 @@ static void do_land() wp_control = LOITER_MODE; // just to make sure - land_complete = false; + set_land_complete(false); // landing boost lowers the main throttle to mimmick // the effect of a user's hand @@ -385,7 +385,7 @@ static bool verify_land_sonar() if(ground_detector < 30) { ground_detector++; }else if (ground_detector == 30) { - land_complete = true; + set_land_complete(true); if(g.rc_3.control_in == 0) { ground_detector++; init_disarm_motors(); @@ -419,7 +419,7 @@ static bool verify_land_baro() if(ground_detector < 30) { ground_detector++; }else if (ground_detector == 30) { - land_complete = true; + set_land_complete(true); if(g.rc_3.control_in == 0) { ground_detector++; init_disarm_motors(); @@ -792,7 +792,8 @@ static void do_set_home() home.lng = command_cond_queue.lng; // Lon * 10**7 home.lat = command_cond_queue.lat; // Lat * 10**7 home.alt = 0; - home_is_set = true; + //home_is_set = true; + set_home_is_set(true); } } diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index b290531658..e57a764859 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -77,16 +77,16 @@ static void update_commands() Location tmp_loc = get_cmd_with_index(tmp_index); if(tmp_loc.lat == 0) { - fast_corner = false; + ap.fast_corner = false; }else{ int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing; temp = wrap_180(temp); - fast_corner = labs(temp) < 6000; + ap.fast_corner = labs(temp) < 6000; } // If we try and stop at a corner, lets reset our desired speed to prevent // too much jerkyness. - if(false == fast_corner){ + if(false == ap.fast_corner){ reset_desired_speed(); } } @@ -213,7 +213,7 @@ static void exit_mission() g.command_index = 255; // if we are on the ground, enter stabilize, else Land - if(land_complete == true) { + if(ap.land_complete) { // we will disarm the motors after landing. }else{ // If the approach altitude is valid (above 1m), do approach, else land diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index b2052226a6..1a77c1034f 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -16,7 +16,8 @@ static void read_control_switch() if(g.ch7_option != CH7_SIMPLE_MODE) { // set Simple mode using stored paramters from Mission planner // rather than by the control switch - do_simple = (g.simple_modes & (1 << switchPosition)); + //ap.simple_mode = (g.simple_modes & (1 << switchPosition)); + set_simple_mode(g.simple_modes & (1 << switchPosition)); } }else{ switch_debouncer = true; @@ -60,29 +61,30 @@ static void read_trim_switch() } if(option == CH7_SIMPLE_MODE) { - do_simple = (g.rc_7.radio_in > CH_7_PWM_TRIGGER); + //ap.simple_mode = (g.rc_7.radio_in > CH_7_PWM_TRIGGER); + set_simple_mode(g.rc_7.radio_in > CH_7_PWM_TRIGGER); }else if (option == CH7_FLIP) { - if (CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) { - CH7_flag = true; + if (system.CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) { + system.CH7_flag = true; // don't flip if we accidentally engaged flip, but didn't notice and tried to takeoff - if(g.rc_3.control_in != 0 && takeoff_complete) { + if(g.rc_3.control_in != 0 && ap.takeoff_complete) { init_flip(); } } - if (CH7_flag == true && g.rc_7.control_in < 800) { - CH7_flag = false; + if (system.CH7_flag == true && g.rc_7.control_in < 800) { + system.CH7_flag = false; } }else if (option == CH7_RTL) { - if (CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) { - CH7_flag = true; + if (system.CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) { + system.CH7_flag = true; set_mode(RTL); } - if (CH7_flag == true && g.rc_7.control_in < 800) { - CH7_flag = false; + if (system.CH7_flag == true && g.rc_7.control_in < 800) { + system.CH7_flag = false; if (control_mode == RTL || control_mode == LOITER) { reset_control_switch(); } @@ -90,11 +92,11 @@ static void read_trim_switch() }else if (option == CH7_SAVE_WP) { if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) { // switch is engaged - CH7_flag = true; + system.CH7_flag = true; }else{ // switch is disengaged - if(CH7_flag) { - CH7_flag = false; + if(system.CH7_flag) { + system.CH7_flag = false; if(control_mode == AUTO) { // reset the mission diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index eecf59b46b..d5f74d8e52 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -111,13 +111,13 @@ #define RC_CHANNEL_ANGLE_RAW 2 // HIL enumerations -#define HIL_MODE_DISABLED 0 -#define HIL_MODE_ATTITUDE 1 -#define HIL_MODE_SENSORS 2 +#define HIL_MODE_DISABLED 0 +#define HIL_MODE_ATTITUDE 1 +#define HIL_MODE_SENSORS 2 -#define ASCENDING 1 -#define DESCENDING -1 #define REACHED_ALT 0 +#define DESCENDING 1 +#define ASCENDING 2 // Auto Pilot modes // ---------------- @@ -131,10 +131,10 @@ #define CIRCLE 7 // AUTO control #define POSITION 8 // AUTO control #define LAND 9 // AUTO control -#define OF_LOITER 10 // Hold a single location using optical flow - // sensor -#define TOY_A 11 // THOR Enum for Toy mode -#define TOY_M 12 // THOR Enum for Toy mode +#define OF_LOITER 10 // Hold a single location using optical flow + // sensor +#define TOY_A 11 // THOR Enum for Toy mode +#define TOY_M 12 // THOR Enum for Toy mode #define NUM_MODES 13 #define SIMPLE_1 1 @@ -225,12 +225,12 @@ // Waypoint options #define MASK_OPTIONS_RELATIVE_ALT 1 #define WP_OPTION_ALT_CHANGE 2 -#define WP_OPTION_YAW 4 +#define WP_OPTION_YAW 4 #define WP_OPTION_ALT_REQUIRED 8 -#define WP_OPTION_RELATIVE 16 +#define WP_OPTION_RELATIVE 16 //#define WP_OPTION_ 32 //#define WP_OPTION_ 64 -#define WP_OPTION_NEXT_CMD 128 +#define WP_OPTION_NEXT_CMD 128 //repeating events #define NO_REPEAT 0 @@ -303,8 +303,8 @@ enum gcs_severity { #define LOG_INDEX_MSG 0xF0 #define MAX_NUM_LOGS 50 -#define MASK_LOG_ATTITUDE_FAST (1<<0) -#define MASK_LOG_ATTITUDE_MED (1<<1) +#define MASK_LOG_ATTITUDE_FAST (1<<0) +#define MASK_LOG_ATTITUDE_MED (1<<1) #define MASK_LOG_GPS (1<<2) #define MASK_LOG_PM (1<<3) #define MASK_LOG_CTUN (1<<4) @@ -320,6 +320,52 @@ enum gcs_severity { #define MASK_LOG_INAV (1<<14) +#define MASK_LOG_CTUN (1<<4) +#define MASK_LOG_NTUN (1<<5) +#define MASK_LOG_MODE (1<<6) +#define MASK_LOG_RAW (1<<7) +#define MASK_LOG_CMD (1<<8) +#define MASK_LOG_CUR (1<<9) +#define MASK_LOG_MOTORS (1<<10) +#define MASK_LOG_OPTFLOW (1<<11) +#define MASK_LOG_PID (1<<12) +#define MASK_LOG_ITERM (1<<13) + + +// DATA - event logging +#define DATA_MAVLINK_FLOAT 1 +#define DATA_MAVLINK_INT32 2 +#define DATA_MAVLINK_INT16 3 +#define DATA_MAVLINK_INT8 4 +#define DATA_FAST_LOOP 5 +#define DATA_MED_LOOP 6 +#define DATA_AP_STATE 7 +#define DATA_SIMPLE_BEARING 8 +#define DATA_INIT_SIMPLE_BEARING 9 +#define DATA_ARMED 10 +#define DATA_DISARMED 11 +#define DATA_FAILSAFE_ON 12 +#define DATA_FAILSAFE_OFF 13 +#define DATA_LOW_BATTERY 14 +#define DATA_AUTO_ARMED 15 +#define DATA_TAKEOFF 16 +#define DATA_DID_REACH_ALT 17 +#define DATA_LAND_COMPLETE 18 +#define DATA_LOST_GPS 19 +#define DATA_LOST_COMPASS 20 +#define DATA_BEGIN_FLIP 21 +#define DATA_END_FLIP 22 +#define DATA_EXIT_FLIP 23 +#define DATA_FLIP_ABORTED 24 +#define DATA_SET_HOME 25 +#define DATA_SET_SIMPLE_ON 26 +#define DATA_SET_SIMPLE_OFF 27 +#define DATA_REACHED_ALT 28 +#define DATA_ASCENDING 29 +#define DATA_DESCENDING 30 +#define DATA_RTL_REACHED_ALT 31 + + // Waypoint Modes // ---------------- #define ABS_WP 0 diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index 6fed35bcfc..46bc264285 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -22,7 +22,7 @@ static void failsafe_on_event() break; default: - if(home_is_set == true) { + if(ap.home_is_set) { // same as above ^ // do_rtl sets the altitude to the current altitude by default set_mode(RTL); @@ -73,8 +73,7 @@ static void low_battery_event(void) last_low_battery_message = tnow; } - low_batt = true; - + set_low_battery(true); // if we are in Auto mode, come home if(control_mode >= AUTO) set_mode(RTL); diff --git a/ArduCopter/failsafe.pde b/ArduCopter/failsafe.pde index e53bd16b5b..c39dc7d9c1 100644 --- a/ArduCopter/failsafe.pde +++ b/ArduCopter/failsafe.pde @@ -27,7 +27,7 @@ void failsafe_disable() { failsafe_enabled = false; } - + // // failsafe_check - this function is called from the core timer interrupt at 1kHz. // @@ -42,7 +42,7 @@ void failsafe_check(uint32_t tnow) } if (failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) { - // motors are running but we have gone 2 second since the + // motors are running but we have gone 2 second since the // main loop ran. That means we're in trouble and should // disarm the motors. in_failsafe = true; @@ -51,8 +51,9 @@ void failsafe_check(uint32_t tnow) if (in_failsafe && tnow - failsafe_last_timestamp > 1000000) { // disarm motors every second failsafe_last_timestamp = tnow; - if( motors.armed() ) { + if(motors.armed()) { motors.armed(false); + set_armed(true); motors.output(); } } diff --git a/ArduCopter/flip.pde b/ArduCopter/flip.pde index 9aa20df2c0..1f4c6b712c 100644 --- a/ArduCopter/flip.pde +++ b/ArduCopter/flip.pde @@ -17,10 +17,11 @@ static int8_t flip_dir; void init_flip() { - if(do_flip == false) { - do_flip = true; + 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); } } @@ -61,7 +62,8 @@ void roll_flip() g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; flip_timer++; }else{ - do_flip = false; + Log_Write_Event(DATA_END_FLIP); + ap.do_flip = false; flip_state = 0; } break; diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index e158d7660d..8957a133f7 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -19,7 +19,7 @@ static void update_GPS_light(void) switch (g_gps->status()) { case (2): - if(home_is_set) { // JLN update + if(ap.home_is_set) { // JLN update digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set. } else { digitalWrite(C_LED_PIN, LED_OFF); @@ -28,8 +28,8 @@ static void update_GPS_light(void) case (1): if (g_gps->valid_read == true) { - GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock - if (GPS_light) { + system.GPS_light = !system.GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (system.GPS_light) { digitalWrite(C_LED_PIN, LED_OFF); }else{ digitalWrite(C_LED_PIN, LED_ON); @@ -47,17 +47,17 @@ static void update_GPS_light(void) static void update_motor_light(void) { if(motors.armed() == false) { - motor_light = !motor_light; + system.motor_light = !system.motor_light; // blink - if(motor_light) { + if(system.motor_light) { digitalWrite(A_LED_PIN, LED_ON); }else{ digitalWrite(A_LED_PIN, LED_OFF); } }else{ - if(!motor_light) { - motor_light = true; + if(!system.motor_light) { + system.motor_light = true; digitalWrite(A_LED_PIN, LED_ON); } } @@ -93,7 +93,7 @@ static void clear_leds() digitalWrite(A_LED_PIN, LED_OFF); digitalWrite(B_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_OFF); - motor_light = false; + system.motor_light = false; led_mode = NORMAL_LEDS; } @@ -121,28 +121,28 @@ static void clear_leds() static void update_copter_leds(void) { if (g.copter_leds_mode == 0) { - copter_leds_reset(); //method of reintializing LED state + copter_leds_reset(); //method of reintializing LED state } if ( bitRead(g.copter_leds_mode, 0) ) { if (motors.armed() == true) { - if (low_batt == true) { + if (ap.low_battery == true) { if ( bitRead(g.copter_leds_mode, 4 )) { - copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink + copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink } else { - copter_leds_fast_blink(); //if motors are armed, but battery level is low, motor leds oscillate + copter_leds_fast_blink(); //if motors are armed, but battery level is low, motor leds oscillate } } else if ( !bitRead(g.copter_leds_mode, 5 ) ) { - copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON + copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON } else if ( bitRead(g.copter_leds_mode, 5 ) ) { if ( copter_leds_nav_blink >0 ) { - copter_leds_slow_blink(); //if nav command was seen, blink LEDs. + copter_leds_slow_blink(); //if nav command was seen, blink LEDs. } else { copter_leds_on(); } } } else { - copter_leds_slow_blink(); //if motors are not armed, blink motor leds + copter_leds_slow_blink(); //if motors are not armed, blink motor leds } } @@ -153,38 +153,38 @@ static void update_copter_leds(void) switch (g_gps->status()) { case (2): - if(home_is_set) { + if(ap.home_is_set) { if ( !bitRead(g.copter_leds_mode, 6 ) ) { - copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set + copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set } else if (bitRead(g.copter_leds_mode, 6 ) ) { if ( copter_leds_nav_blink >0 ) { - copter_leds_GPS_slow_blink(); //if nav command was seen, blink LEDs. + copter_leds_GPS_slow_blink(); //if nav command was seen, blink LEDs. } else { copter_leds_GPS_on(); } } } else { - copter_leds_GPS_fast_blink(); //if GPS has fix, but home is not set, blink GPS LED fast + copter_leds_GPS_fast_blink(); //if GPS has fix, but home is not set, blink GPS LED fast } break; case (1): - copter_leds_GPS_slow_blink(); //if GPS has valid reads, but no fix, blink GPS LED slow + copter_leds_GPS_slow_blink(); //if GPS has valid reads, but no fix, blink GPS LED slow break; default: - copter_leds_GPS_off(); //if no valid GPS signal, turn GPS LED off + copter_leds_GPS_off(); //if no valid GPS signal, turn GPS LED off break; } } if ( bitRead(g.copter_leds_mode, 2) ) { if (200 <= g.rc_7.control_in && g.rc_7.control_in < 400) { - copter_leds_aux_on(); //if sub-control of Ch7 is high, turn Aux LED on + copter_leds_aux_on(); //if sub-control of Ch7 is high, turn Aux LED on } else if (g.rc_7.control_in < 200) { - copter_leds_aux_off(); //if sub-control of Ch7 is low, turn Aux LED off + copter_leds_aux_off(); //if sub-control of Ch7 is low, turn Aux LED off } } @@ -308,7 +308,7 @@ static void copter_leds_oscillate(void) { digitalWrite(COPTER_LED_7, COPTER_LED_ON); digitalWrite(COPTER_LED_8, COPTER_LED_ON); } - else copter_leds_motor_blink = 0; // start blink cycle again + else copter_leds_motor_blink = 0; // start blink cycle again } @@ -322,26 +322,26 @@ static void copter_leds_GPS_off(void) { } static void copter_leds_GPS_slow_blink(void) { - copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop - if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 6 ) { // when the counter reaches 5 (1/2 sec), then toggle the leds + copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop + if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 6 ) { // when the counter reaches 5 (1/2 sec), then toggle the leds copter_leds_GPS_off(); - if ( bitRead(g.copter_leds_mode, 6 ) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker... + if ( bitRead(g.copter_leds_mode, 6 ) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker... copter_leds_nav_blink--; // decrement the Nav Blink counter } }else if (5 < copter_leds_GPS_blink && copter_leds_GPS_blink < 11) { copter_leds_GPS_on(); } - else copter_leds_GPS_blink = 0; // start blink cycle again + else copter_leds_GPS_blink = 0; // start blink cycle again } static void copter_leds_GPS_fast_blink(void) { - copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop - if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds + copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop + if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds copter_leds_GPS_off(); }else if (2 < copter_leds_GPS_blink && copter_leds_GPS_blink < 5) { copter_leds_GPS_on(); } - else copter_leds_GPS_blink = 0; // start blink cycle again + else copter_leds_GPS_blink = 0; // start blink cycle again } static void copter_leds_aux_off(void){ @@ -360,7 +360,7 @@ void piezo_off(){ digitalWrite(PIEZO_PIN,LOW); } -void piezo_beep(){ // Note! This command should not be used in time sensitive loops +void piezo_beep(){ // Note! This command should not be used in time sensitive loops piezo_on(); delay(100); piezo_off(); diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index d074452852..7d639177e1 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -87,6 +87,7 @@ static void arm_motors() static void init_arm_motors() { + // arming marker // Flag used to track if we have armed the motors the first time. // This is used to decide if we should run the ground_start routine // which calibrates the IMU @@ -122,7 +123,7 @@ static void init_arm_motors() // Reset home position // ------------------- - if(home_is_set) + if(ap.home_is_set) init_home(); // all I terms are invalid @@ -142,7 +143,7 @@ static void init_arm_motors() #endif // temp hack - motor_light = true; + system.motor_light = true; digitalWrite(A_LED_PIN, LED_ON); // go back to normal AHRS gains @@ -153,6 +154,7 @@ static void init_arm_motors() // finally actually arm the motors motors.armed(true); + set_armed(true); // reenable failsafe failsafe_enable(); @@ -161,12 +163,19 @@ static void init_arm_motors() static void init_disarm_motors() { + // disarming marker + //Serial.printf("\nDISARM\n"); #if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD) gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); #endif motors.armed(false); + set_armed(false); + + motors.auto_armed(false); + set_auto_armed(false); + compass.save_offsets(); g.throttle_cruise.save(); @@ -176,7 +185,7 @@ static void init_disarm_motors() #endif // we are not in the air - takeoff_complete = false; + set_takeoff_complete(false); #if COPTER_LEDS == ENABLED if ( bitRead(g.copter_leds_mode, 3) ) { diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 353dd9e7bf..e422df4574 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -194,9 +194,10 @@ static void run_navigation_contollers() case RTL: // have we reached the desired Altitude? - if(alt_change_flag <= REACHED_ALT) { // we are at or above the target alt - if(rtl_reached_alt == false) { - rtl_reached_alt = true; + if(alt_change_flag == REACHED_ALT || alt_change_flag == DESCENDING) { + // we are at or above the target alt + if(false == ap.rtl_reached_alt) { + set_rtl_reached_alt(true); do_RTL(); } wp_control = WP_MODE; @@ -223,12 +224,13 @@ static void run_navigation_contollers() // go of the sticks if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500) { - if(wp_distance > 500) - loiter_override = true; + if(wp_distance > 500){ + ap.loiter_override = true; + } } // Allow the user to take control temporarily, - if(loiter_override) { + if(ap.loiter_override) { // this sets the copter to not try and nav while we control it wp_control = NO_NAV_MODE; @@ -237,8 +239,8 @@ static void run_navigation_contollers() next_WP.lng = current_loc.lng; if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0) { - loiter_override = false; - wp_control = LOITER_MODE; + wp_control = LOITER_MODE; + ap.loiter_override = false; } }else{ wp_control = LOITER_MODE; @@ -294,7 +296,7 @@ static void run_navigation_contollers() } // are we in SIMPLE mode? - if(do_simple && g.super_simple) { + if(ap.simple_mode && g.super_simple) { // get distance to home if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home // we reset the angular offset to be a vector from home to the quad @@ -304,7 +306,7 @@ static void run_navigation_contollers() } if(yaw_mode == YAW_LOOK_AT_HOME) { - if(home_is_set) { + if(ap.home_is_set) { nav_yaw = get_bearing_cd(¤t_loc, &home); } else { nav_yaw = 0; @@ -329,7 +331,7 @@ static void update_nav_RTL() // If failsafe OR auto approach altitude is set // we will go into automatic land, (g.rtl_approach_alt) is the lowest point // -1 means disable feature - if(failsafe || g.rtl_approach_alt >= 0) + if(ap.failsafe || g.rtl_approach_alt >= 0) loiter_timer = millis(); else loiter_timer = 0; @@ -493,10 +495,10 @@ static int16_t get_desired_speed(int16_t max_speed) V1 = sqrt(sq(V2) - 2*A*(X2-X1)) */ - if(fast_corner) { + if(ap.fast_corner) { // don't slow down }else{ - if(wp_distance < 20000){ // limit the size of numbers we're dealing with to avoide overflow + if(wp_distance < 20000){ // limit the size of numbers we're dealing with to avoid overflow // go slower int32_t temp = 2 * 100 * (int32_t)(wp_distance - g.waypoint_radius * 100); int32_t s_min = WAYPOINT_SPEED_MIN; @@ -517,7 +519,43 @@ static void reset_desired_speed() max_speed_old = 0; } +#define MAX_CLIMB_RATE 200 +#define MIN_CLIMB_RATE 50 +#define DECEL_CLIMB_RATE 30 + + static int16_t get_desired_climb_rate() +{ + static int16_t climb_old = 0; + + if(alt_change_flag == REACHED_ALT) { + climb_old = 0; + return 0; + } + + int16_t climb = 0; + int32_t dist = labs(altitude_error); + + if(dist < 20000){ // limit the size of numbers we're dealing with to avoid overflow + dist -= 300; // give ourselves 3 meter buffer to the desired alt + float temp = 2 * DECEL_CLIMB_RATE * dist + (MIN_CLIMB_RATE * MIN_CLIMB_RATE); // 50cm minium climb_rate; + climb = sqrt(temp); + climb = min(climb, MAX_CLIMB_RATE); // don't go to fast + }else{ + climb = MAX_CLIMB_RATE; // no need to calc speed, just go the max + } + + //climb = min(climb, climb_old + (100 * .02));// limit going faster + climb = max(climb, MIN_CLIMB_RATE); // don't go too slow + climb_old = climb; + + if(alt_change_flag == DESCENDING){ + climb = -climb; + } + return climb; +} + +static int16_t get_desired_climb_rate_old() { if(alt_change_flag == ASCENDING) { return constrain(altitude_error / 4, 100, 180); // 180cm /s up, minimum is 100cm/s @@ -534,9 +572,15 @@ static void update_crosstrack(void) { // Crosstrack Error // ---------------- - // If we are too far off or too close we don't do track following - float temp = (target_bearing - original_target_bearing) * RADX100; - crosstrack_error = sin(temp) * wp_distance; // Meters we are off track line + if (wp_distance >= (g.crosstrack_min_distance * 100) && + abs(wrap_180(target_bearing - original_target_bearing)) < 4500) { + + float temp = (target_bearing - original_target_bearing) * RADX100; + crosstrack_error = sin(temp) * wp_distance; // Meters we are off track line + }else{ + // fade out crosstrack + crosstrack_error >>= 1; + } } static int32_t get_altitude_error() @@ -555,30 +599,30 @@ static int32_t get_altitude_error() static void clear_new_altitude() { - alt_change_flag = REACHED_ALT; + set_alt_change(REACHED_ALT); } static void force_new_altitude(int32_t new_alt) { next_WP.alt = new_alt; - alt_change_flag = REACHED_ALT; + set_alt_change(REACHED_ALT); } static void set_new_altitude(int32_t new_alt) { next_WP.alt = new_alt; - if(next_WP.alt > current_loc.alt + 20) { + if(next_WP.alt > (current_loc.alt + 80)) { // we are below, going up - alt_change_flag = ASCENDING; + set_alt_change(ASCENDING); - }else if(next_WP.alt < current_loc.alt - 20) { + }else if(next_WP.alt < (current_loc.alt - 80)) { // we are above, going down - alt_change_flag = DESCENDING; + set_alt_change(DESCENDING); }else{ // No Change - alt_change_flag = REACHED_ALT; + set_alt_change(REACHED_ALT); } } @@ -587,12 +631,13 @@ static void verify_altitude() if(alt_change_flag == ASCENDING) { // we are below, going up if(current_loc.alt > next_WP.alt - 50) { - alt_change_flag = REACHED_ALT; + set_alt_change(REACHED_ALT); } }else if (alt_change_flag == DESCENDING) { // we are above, going down - if(current_loc.alt <= next_WP.alt + 50) - alt_change_flag = REACHED_ALT; + if(current_loc.alt <= next_WP.alt + 50){ + set_alt_change(REACHED_ALT); + } } } @@ -609,4 +654,4 @@ static int32_t wrap_180(int32_t error) if (error > 18000) error -= 36000; if (error < -18000) error += 36000; return error; -} \ No newline at end of file +} diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 90c1f34319..b45f989f45 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -129,7 +129,7 @@ void output_min() static void read_radio() { if (APM_RC.GetState() == 1) { - new_radio_frame = true; + system.new_radio_frame = true; g.rc_1.set_pwm(APM_RC.InputCh(CH_1)); g.rc_2.set_pwm(APM_RC.InputCh(CH_2)); g.rc_3.set_pwm(APM_RC.InputCh(CH_3)); @@ -167,7 +167,7 @@ static void throttle_failsafe(uint16_t pwm) // Don't enter Failsafe if we are not armed // home distance is in meters // This is to prevent accidental RTL - if(motors.armed() && takeoff_complete) { + if(motors.armed() && ap.takeoff_complete) { Serial.print_P(PSTR("MSG FS ON ")); Serial.println(pwm, DEC); set_failsafe(true); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index f32827b7aa..910235f82e 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -62,8 +62,8 @@ static void init_ardupilot() // USB_MUX_PIN pinMode(USB_MUX_PIN, INPUT); - usb_connected = !digitalRead(USB_MUX_PIN); - if (!usb_connected) { + system.usb_connected = !digitalRead(USB_MUX_PIN); + if (!system.usb_connected) { // USB is not connected, this means UART0 may be a Xbee, with // its darned bricking problem. We can't write to it for at // least one second after powering up. Simplest solution for @@ -163,7 +163,7 @@ static void init_ardupilot() gcs0.init(&Serial); #if USB_MUX_PIN > 0 - if (!usb_connected) { + if (!system.usb_connected) { // we are not connected via USB, re-init UART0 with right // baud rate Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); @@ -192,12 +192,14 @@ static void init_ardupilot() } #endif +/* #ifdef RADIO_OVERRIDE_DEFAULTS { int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS; APM_RC.setHIL(rc_override); } #endif +*/ #if FRAME_CONFIG == HELI_FRAME motors.servo_manual = false; @@ -291,26 +293,6 @@ static void init_ardupilot() #if LOGGING_ENABLED == ENABLED Log_Write_Startup(); - Log_Write_Data(10, (float)g.pi_stabilize_roll.kP()); - Log_Write_Data(11, (float)g.pi_stabilize_roll.kI()); - - Log_Write_Data(12, (float)g.pid_rate_roll.kP()); - Log_Write_Data(13, (float)g.pid_rate_roll.kI()); - Log_Write_Data(14, (float)g.pid_rate_roll.kD()); - Log_Write_Data(15, (float)g.stabilize_d.get()); - - Log_Write_Data(16, (float)g.pi_loiter_lon.kP()); - Log_Write_Data(17, (float)g.pi_loiter_lon.kI()); - - Log_Write_Data(18, (float)g.pid_nav_lon.kP()); - Log_Write_Data(19, (float)g.pid_nav_lon.kI()); - Log_Write_Data(20, (float)g.pid_nav_lon.kD()); - - Log_Write_Data(21, (int32_t)g.auto_slew_rate.get()); - - Log_Write_Data(22, (float)g.pid_loiter_rate_lon.kP()); - Log_Write_Data(23, (float)g.pid_loiter_rate_lon.kI()); - Log_Write_Data(24, (float)g.pid_loiter_rate_lon.kD()); #endif @@ -389,10 +371,8 @@ static void startup_ground(void) static void set_mode(byte mode) { - Log_Write_Data(31, initial_simple_bearing); - // if we don't have GPS lock - if(home_is_set == false) { + if(false == ap.home_is_set) { // THOR // We don't care about Home if we don't have lock yet in Toy mode if(mode == TOY_A || mode == TOY_M || mode == OF_LOITER) { @@ -414,6 +394,7 @@ static void set_mode(byte mode) // used to stop fly_aways // set to false if we have low throttle motors.auto_armed(g.rc_3.control_in > 0); + set_auto_armed(g.rc_3.control_in > 0); // clearing value used in interactive alt hold reset_throttle_counter = 0; @@ -425,24 +406,28 @@ static void set_mode(byte mode) loiter_timer = 0; // if we change modes, we must clear landed flag - land_complete = false; + set_land_complete(false); - // have we acheived the proper altitude before RTL is enabled - rtl_reached_alt = false; + // have we achieved the proper altitude before RTL is enabled + set_rtl_reached_alt(false); // debug to Serial terminal //Serial.println(flight_mode_strings[control_mode]); + ap.loiter_override = false; + // report the GPS and Motor arming status led_mode = NORMAL_LEDS; switch(control_mode) { case ACRO: + ap.manual_throttle = true; + ap.manual_attitude = true; yaw_mode = YAW_ACRO; roll_pitch_mode = ROLL_PITCH_ACRO; throttle_mode = THROTTLE_MANUAL; // reset acro axis targets to current attitude - if( g.axis_enabled ) { + if(g.axis_enabled){ roll_axis = ahrs.roll_sensor; pitch_axis = ahrs.pitch_sensor; nav_yaw = ahrs.yaw_sensor; @@ -450,20 +435,25 @@ static void set_mode(byte mode) break; case STABILIZE: + ap.manual_throttle = true; + ap.manual_attitude = true; yaw_mode = YAW_HOLD; roll_pitch_mode = ROLL_PITCH_STABLE; throttle_mode = THROTTLE_MANUAL; break; case ALT_HOLD: + ap.manual_throttle = false; + ap.manual_attitude = true; yaw_mode = ALT_HOLD_YAW; roll_pitch_mode = ALT_HOLD_RP; throttle_mode = ALT_HOLD_THR; - force_new_altitude(max(current_loc.alt, 100)); break; case AUTO: + ap.manual_throttle = false; + ap.manual_attitude = false; yaw_mode = AUTO_YAW; roll_pitch_mode = AUTO_RP; throttle_mode = AUTO_THR; @@ -473,6 +463,8 @@ static void set_mode(byte mode) break; case CIRCLE: + ap.manual_throttle = false; + ap.manual_attitude = false; yaw_mode = CIRCLE_YAW; roll_pitch_mode = CIRCLE_RP; throttle_mode = CIRCLE_THR; @@ -482,6 +474,8 @@ static void set_mode(byte mode) break; case LOITER: + ap.manual_throttle = false; + ap.manual_attitude = false; yaw_mode = LOITER_YAW; roll_pitch_mode = LOITER_RP; throttle_mode = LOITER_THR; @@ -489,6 +483,8 @@ static void set_mode(byte mode) break; case POSITION: + ap.manual_throttle = true; + ap.manual_attitude = false; yaw_mode = YAW_HOLD; roll_pitch_mode = ROLL_PITCH_AUTO; throttle_mode = THROTTLE_MANUAL; @@ -496,6 +492,8 @@ static void set_mode(byte mode) break; case GUIDED: + ap.manual_throttle = false; + ap.manual_attitude = false; yaw_mode = YAW_AUTO; roll_pitch_mode = ROLL_PITCH_AUTO; throttle_mode = THROTTLE_AUTO; @@ -504,6 +502,8 @@ static void set_mode(byte mode) break; case LAND: + ap.manual_throttle = false; + ap.manual_attitude = false; yaw_mode = LOITER_YAW; roll_pitch_mode = LOITER_RP; throttle_mode = THROTTLE_AUTO; @@ -511,15 +511,19 @@ static void set_mode(byte mode) break; case RTL: + ap.manual_throttle = false; + ap.manual_attitude = false; yaw_mode = RTL_YAW; roll_pitch_mode = RTL_RP; throttle_mode = RTL_THR; - rtl_reached_alt = false; + set_rtl_reached_alt(false); set_next_WP(¤t_loc); set_new_altitude(get_RTL_alt()); break; case OF_LOITER: + ap.manual_throttle = false; + ap.manual_attitude = false; yaw_mode = OF_LOITER_YAW; roll_pitch_mode = OF_LOITER_RP; throttle_mode = OF_LOITER_THR; @@ -530,10 +534,12 @@ static void set_mode(byte mode) // These are the flight modes for Toy mode // See the defines for the enumerated values case TOY_A: - yaw_mode = YAW_TOY; + ap.manual_throttle = false; + ap.manual_attitude = true; + yaw_mode = YAW_TOY; roll_pitch_mode = ROLL_PITCH_TOY; throttle_mode = THROTTLE_AUTO; - wp_control = NO_NAV_MODE; + wp_control = NO_NAV_MODE; // save throttle for fast exit of Alt hold saved_toy_throttle = g.rc_3.control_in; @@ -543,25 +549,34 @@ static void set_mode(byte mode) break; case TOY_M: - yaw_mode = YAW_TOY; - roll_pitch_mode = ROLL_PITCH_TOY; - wp_control = NO_NAV_MODE; - throttle_mode = THROTTLE_HOLD; + ap.manual_throttle = false; + ap.manual_attitude = true; + yaw_mode = YAW_TOY; + roll_pitch_mode = ROLL_PITCH_TOY; + wp_control = NO_NAV_MODE; + throttle_mode = THROTTLE_HOLD; break; default: break; } - if(failsafe) { + if(ap.failsafe) { // this is to allow us to fly home without interactive throttle control throttle_mode = THROTTLE_AUTO; + ap.manual_throttle = false; + // does not wait for us to be in high throttle, since the // Receiver will be outputting low throttle motors.auto_armed(true); + set_auto_armed(true); } - if(roll_pitch_mode <= ROLL_PITCH_ACRO) { + if(ap.manual_throttle) { + desired_climb_rate = 0; + } + + if(ap.manual_attitude) { // We are under manual attitude control // remove the navigation from roll and pitch command reset_nav_params(); @@ -572,36 +587,13 @@ static void set_mode(byte mode) } Log_Write_Mode(control_mode); - Log_Write_Data(32, initial_simple_bearing); -} - -static void set_failsafe(boolean mode) -{ - // only act on changes - // ------------------- - if(failsafe != mode) { - - // store the value so we don't trip the gate twice - // ----------------------------------------------- - failsafe = mode; - - if (failsafe == false) { - // We've regained radio contact - // ---------------------------- - failsafe_off_event(); - - }else{ - // We've lost radio contact - // ------------------------ - failsafe_on_event(); - } - } } static void init_simple_bearing() { initial_simple_bearing = ahrs.yaw_sensor; + Log_Write_Data(DATA_INIT_SIMPLE_BEARING, initial_simple_bearing); } static void update_throttle_cruise(int16_t tmp) @@ -648,13 +640,13 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) static void check_usb_mux(void) { bool usb_check = !digitalRead(USB_MUX_PIN); - if (usb_check == usb_connected) { + if (usb_check == system.usb_connected) { return; } // the user has switched to/from the telemetry port - usb_connected = usb_check; - if (usb_connected) { + system.usb_connected = usb_check; + if (system.usb_connected) { Serial.begin(SERIAL0_BAUD); } else { Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 820e7b40b7..b3122b5850 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -187,7 +187,15 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv) //static int8_t //test_toy(uint8_t argc, const Menu::arg *argv) { - wp_distance = 0; + set_alt_change(ASCENDING) + + for(altitude_error = 2000; altitude_error > -100; altitude_error--){ + int16_t temp = get_desired_climb_rate(); + Serial.printf("%ld, %d\n", altitude_error, temp); + } + return 0; +} +{ wp_distance = 0; int16_t max_speed = 0; for(int16_t i = 0; i < 200; i++){ @@ -198,7 +206,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv) wp_distance += 100; } return 0; -} + } //*/ /*static int8_t @@ -517,42 +525,6 @@ test_gps(uint8_t argc, const Menu::arg *argv) #endif } -// used to test the gain scheduler for Stab_D -/* - * static int8_t - * test_stab_d(uint8_t argc, const Menu::arg *argv) - * { - * int16_t i = 0; - * g.stabilize_d = 1; - * - * g.stabilize_d_schedule = 1 - * for (i = -4600; i < 4600; i+=10) { - * new_radio_frame = true; - * g.rc_1.control_in = i; - * g.rc_2.control_in = i; - * update_roll_pitch_mode(); - * Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d); - * } - * g.stabilize_d_schedule = .5 - * for (i = -4600; i < 4600; i+=10) { - * new_radio_frame = true; - * g.rc_1.control_in = i; - * g.rc_2.control_in = i; - * update_roll_pitch_mode(); - * Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d); - * } - * - * g.stabilize_d_schedule = 0 - * for (i = -4600; i < 4600; i+=10) { - * new_radio_frame = true; - * g.rc_1.control_in = i; - * g.rc_2.control_in = i; - * update_roll_pitch_mode(); - * Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d); - * } - * - * }*/ - /* * //static int8_t * //test_dcm(uint8_t argc, const Menu::arg *argv) diff --git a/ArduCopter/toy.pde b/ArduCopter/toy.pde index e3723aeed7..a0a7a88bb5 100644 --- a/ArduCopter/toy.pde +++ b/ArduCopter/toy.pde @@ -110,15 +110,15 @@ void roll_pitch_toy() if(g.rc_1.control_in != 0) { // roll get_acro_yaw(yaw_rate/2); - yaw_stopped = false; + system.yaw_stopped = false; yaw_timer = 150; - }else if (!yaw_stopped) { + }else if (!system.yaw_stopped) { get_acro_yaw(0); yaw_timer--; if((yaw_timer == 0) || (fabs(omega.z) < .17)) { - yaw_stopped = true; + system.yaw_stopped = true; nav_yaw = ahrs.yaw_sensor; } }else{