diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 42ee2edc59..ed4a7f991a 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -404,7 +404,7 @@ run_rate_controllers() #endif // run throttle controller if accel based throttle controller is enabled and active (active means it has been given a target) - if( g.throttle_accel_enabled && throttle_accel_controller_active ) { + if( throttle_accel_controller_active ) { set_throttle_out(get_throttle_accel(throttle_accel_target_ef), true); } } @@ -938,13 +938,8 @@ void set_throttle_out( int16_t throttle_out, bool apply_angle_boost ) // set_throttle_accel_target - to be called by upper throttle controllers to set desired vertical acceleration in earth frame void set_throttle_accel_target( int16_t desired_acceleration ) { - if( g.throttle_accel_enabled ) { - throttle_accel_target_ef = desired_acceleration; - throttle_accel_controller_active = true; - }else{ - // To-Do log dataflash or tlog error - cliSerial->print_P(PSTR("Err: target sent to inactive acc thr controller!\n")); - } + throttle_accel_target_ef = desired_acceleration; + throttle_accel_controller_active = true; } // disable_throttle_accel - disables the accel based throttle controller @@ -1154,13 +1149,8 @@ get_throttle_rate(float z_target_speed) } #endif - // send output to accelerometer based throttle controller if enabled otherwise send directly to motors - if( g.throttle_accel_enabled ) { - // set target for accel based throttle controller - set_throttle_accel_target(output); - }else{ - set_throttle_out(g.throttle_cruise+output, true); - } + // set target for accel based throttle controller + set_throttle_accel_target(output); // update throttle cruise // TO-DO: this may not be correct because g.rc_3.servo_out has not been updated for this iteration diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 73c593ec83..7bc1929bb8 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -80,7 +80,7 @@ public: k_param_crosstrack_min_distance, // deprecated - remove with next eeprom number change k_param_rssi_pin, - k_param_throttle_accel_enabled, + k_param_throttle_accel_enabled, // deprecated - remove k_param_wp_yaw_behavior, k_param_acro_trainer_enabled, k_param_pilot_velocity_z_max, @@ -295,7 +295,6 @@ public: AP_Int8 battery_volt_pin; AP_Int8 battery_curr_pin; AP_Int8 rssi_pin; - AP_Int8 throttle_accel_enabled; // enable/disable accel based throttle controller AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions // Waypoints diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 55ca748a06..8f8563615e 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -191,13 +191,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(rssi_pin, "RSSI_PIN", -1), - // @Param: THR_ACC_ENABLE - // @DisplayName: Enable Accel based throttle controller - // @Description: This allows enabling and disabling the accelerometer based throttle controller. If disabled a velocity based controller is used. - // @Values: 0:Disabled, 1:Enabled - // @User: Standard - GSCALAR(throttle_accel_enabled, "THR_ACC_ENABLE", 1), - // @Param: WP_YAW_BEHAVIOR // @DisplayName: Yaw behaviour during missions // @Description: Determines how the autopilot controls the yaw during missions and RTL