diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e2e3657884..a5fcef1794 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -431,8 +431,6 @@ static int16_t lon_speed; // expressed in cm/s. positive numbers mean mov static int16_t lat_speed; // expressed in cm/s. positive numbers when moving north static int16_t ground_bearing; // expressed in centidegrees -static int16_t desired_climb_rate; - // The difference between the desired rate of travel and the actual rate of travel // updated after GPS read - 5-10hz static int16_t x_rate_error; @@ -616,6 +614,7 @@ static int16_t throttle_accel_target_ef = 0; // earth frame throttle accelera static bool throttle_accel_controller_active = false; // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directly static float z_accel_meas = 0; // filtered throttle acceleration static float throttle_avg; // g.throttle_cruise as a float +static int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only //////////////////////////////////////////////////////////////////////////////// @@ -698,8 +697,6 @@ static int16_t sonar_rate; static int32_t baro_alt; // The climb_rate as reported by Baro in cm/s static int16_t baro_rate; -// used to switch out of Manual Boost -static uint8_t reset_throttle_counter; static int16_t saved_toy_throttle; @@ -1669,6 +1666,64 @@ void update_simple_mode(void) g.rc_2.control_in = _pitch; } +// set_throttle_mode - sets the throttle mode and initialises any variables as required +bool set_throttle_mode( uint8_t new_throttle_mode ) +{ + // boolean to ensure proper initialisation of throttle modes + bool throttle_initialised = false; + + // initialise any variables required for the new throttle mode + switch(new_throttle_mode) { + case THROTTLE_MANUAL: + case THROTTLE_MANUAL_TILT_COMPENSATED: + throttle_accel_controller_active = false; // this controller does not use accel based throttle controller + throttle_initialised = true; + break; + + case THROTTLE_ACCELERATION: + case THROTTLE_RATE: + case THROTTLE_STABILIZED_RATE: + case THROTTLE_DIRECT_ALT: + throttle_accel_controller_active = true; // this controller uses accel based throttle controller + throttle_initialised = true; + break; + + case THROTTLE_HOLD: + case THROTTLE_AUTO: + set_new_altitude(current_loc.alt); // by default hold the current altitude + if ( throttle_mode <= THROTTLE_HOLD ) { // reset the alt hold I terms if previous throttle mode was manual + reset_throttle_I(); + } + throttle_accel_controller_active = true; // this controller uses accel based throttle controller + throttle_initialised = true; + break; + + case THROTTLE_LAND: + set_land_complete(false); // mark landing as incomplete + ground_detector = 0; // A counter that goes up if our climb rate stalls out. + set_new_altitude(0); // Set a new target altitude + throttle_accel_controller_active = true; // this controller uses accel based throttle controller + throttle_initialised = true; + break; + + default: + cliSerial->printf_P(PSTR("Unsupported throttle mode: %d!!"),new_throttle_mode); + break; + } + + // update the throttle mode + if( throttle_initialised ) { + throttle_mode = new_throttle_mode; + + // reset some variables used for logging + desired_climb_rate = 0; + nav_throttle = 0; + } + + // return success or failure + return throttle_initialised; +} + // 50 hz update rate // controls all throttle behavior void update_throttle_mode(void) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 0557292508..aca3a08149 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -864,6 +864,9 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control) desired_rate = 0; } + // desired climb rate for logging + desired_climb_rate = desired_rate; + return desired_rate; } diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index ad9bf0dbbc..32e17dab1e 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -272,19 +272,10 @@ static void do_nav_wp() static void do_land() { wp_control = LOITER_MODE; - throttle_mode = THROTTLE_LAND; - - // just to make sure - set_land_complete(false); - - // A counter that goes up if our climb rate stalls out. - ground_detector = 0; + set_throttle_mode(THROTTLE_LAND); // hold at our current location set_next_WP(¤t_loc); - - // Set a new target altitude - set_new_altitude(0); } static void do_loiter_unlimited() diff --git a/ArduCopter/limits.pde b/ArduCopter/limits.pde index bcf97a2def..bba90570c6 100644 --- a/ArduCopter/limits.pde +++ b/ArduCopter/limits.pde @@ -269,7 +269,7 @@ static void limits_loop() { //set_recovery_home_alt(); set_mode(POSITION); - throttle_mode = THROTTLE_AUTO; + set_throttle_mode(THROTTLE_AUTO); limits.last_action = millis(); gcs_send_message(MSG_LIMITS_STATUS); break; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 8afd6304bb..d6f0986277 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -410,9 +410,6 @@ static void set_mode(byte mode) 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; - // do not auto_land if we are leaving RTL loiter_timer = 0; @@ -436,7 +433,7 @@ static void set_mode(byte mode) ap.manual_attitude = true; yaw_mode = YAW_ACRO; roll_pitch_mode = ROLL_PITCH_ACRO; - throttle_mode = THROTTLE_MANUAL; + set_throttle_mode(THROTTLE_MANUAL); // reset acro axis targets to current attitude if(g.axis_enabled){ roll_axis = ahrs.roll_sensor; @@ -450,7 +447,7 @@ static void set_mode(byte mode) ap.manual_attitude = true; yaw_mode = YAW_HOLD; roll_pitch_mode = ROLL_PITCH_STABLE; - throttle_mode = THROTTLE_MANUAL_TILT_COMPENSATED; + set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED); break; case ALT_HOLD: @@ -458,7 +455,7 @@ static void set_mode(byte mode) ap.manual_attitude = true; yaw_mode = ALT_HOLD_YAW; roll_pitch_mode = ALT_HOLD_RP; - throttle_mode = ALT_HOLD_THR; + set_throttle_mode(ALT_HOLD_THR); force_new_altitude(max(current_loc.alt, 100)); break; @@ -467,7 +464,7 @@ static void set_mode(byte mode) ap.manual_attitude = false; yaw_mode = AUTO_YAW; roll_pitch_mode = AUTO_RP; - throttle_mode = AUTO_THR; + set_throttle_mode(AUTO_THR); // loads the commands from where we left off init_commands(); @@ -478,7 +475,7 @@ static void set_mode(byte mode) ap.manual_attitude = false; yaw_mode = CIRCLE_YAW; roll_pitch_mode = CIRCLE_RP; - throttle_mode = CIRCLE_THR; + set_throttle_mode(CIRCLE_THR); set_next_WP(¤t_loc); circle_WP = next_WP; circle_angle = 0; @@ -489,7 +486,7 @@ static void set_mode(byte mode) ap.manual_attitude = false; yaw_mode = LOITER_YAW; roll_pitch_mode = LOITER_RP; - throttle_mode = LOITER_THR; + set_throttle_mode(LOITER_THR); set_next_WP(¤t_loc); break; @@ -498,7 +495,7 @@ static void set_mode(byte mode) ap.manual_attitude = false; yaw_mode = YAW_HOLD; roll_pitch_mode = ROLL_PITCH_AUTO; - throttle_mode = THROTTLE_MANUAL; + set_throttle_mode(THROTTLE_MANUAL); set_next_WP(¤t_loc); break; @@ -507,7 +504,7 @@ static void set_mode(byte mode) ap.manual_attitude = false; yaw_mode = YAW_AUTO; roll_pitch_mode = ROLL_PITCH_AUTO; - throttle_mode = THROTTLE_AUTO; + set_throttle_mode(THROTTLE_AUTO); next_WP = current_loc; set_next_WP(&guided_WP); break; @@ -525,7 +522,6 @@ static void set_mode(byte mode) roll_pitch_mode = ROLL_PITCH_STABLE; } ap.manual_throttle = false; - throttle_mode = THROTTLE_LAND; do_land(); break; @@ -534,7 +530,7 @@ static void set_mode(byte mode) ap.manual_attitude = false; yaw_mode = RTL_YAW; roll_pitch_mode = RTL_RP; - throttle_mode = RTL_THR; + set_throttle_mode(RTL_THR); set_rtl_reached_alt(false); set_next_WP(¤t_loc); set_new_altitude(get_RTL_alt()); @@ -545,7 +541,7 @@ static void set_mode(byte mode) ap.manual_attitude = false; yaw_mode = OF_LOITER_YAW; roll_pitch_mode = OF_LOITER_RP; - throttle_mode = OF_LOITER_THR; + set_throttle_mode(OF_LOITER_THR); set_next_WP(¤t_loc); break; @@ -557,14 +553,12 @@ static void set_mode(byte mode) ap.manual_attitude = true; yaw_mode = YAW_TOY; roll_pitch_mode = ROLL_PITCH_TOY; - throttle_mode = THROTTLE_AUTO; + set_throttle_mode(THROTTLE_AUTO); wp_control = NO_NAV_MODE; // save throttle for fast exit of Alt hold saved_toy_throttle = g.rc_3.control_in; - // hold the current altitude - set_new_altitude(current_loc.alt); break; case TOY_M: @@ -573,7 +567,7 @@ static void set_mode(byte mode) yaw_mode = YAW_TOY; roll_pitch_mode = ROLL_PITCH_TOY; wp_control = NO_NAV_MODE; - throttle_mode = THROTTLE_HOLD; + set_throttle_mode(THROTTLE_HOLD); break; default: @@ -582,7 +576,7 @@ static void set_mode(byte mode) if(ap.failsafe) { // this is to allow us to fly home without interactive throttle control - throttle_mode = THROTTLE_AUTO; + set_throttle_mode(THROTTLE_AUTO); ap.manual_throttle = false; // does not wait for us to be in high throttle, since the @@ -591,18 +585,12 @@ static void set_mode(byte mode) set_auto_armed(true); } - 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(); // remove the wind compenstaion reset_wind_I(); - // Clears the alt hold compensation - reset_throttle_I(); } Log_Write_Mode(control_mode);