diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 5b82bdf11c..0dbd09fd43 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -70,17 +70,21 @@ void Copter::set_throttle_takeoff() pos_control->init_takeoff(); } +float Copter::Mode::throttle_hover() const +{ + return motors->get_throttle_hover(); +} + // transform pilot's manual throttle input to make hover throttle mid stick // used only for manual throttle modes // thr_mid should be in the range 0 to 1 // returns throttle output 0 to 1 -float Copter::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid) +float Copter::Mode::get_pilot_desired_throttle() const { - if (thr_mid <= 0.0f) { - thr_mid = motors->get_throttle_hover(); - } + const float thr_mid = throttle_hover(); + int16_t throttle_control = channel_throttle->get_control_in(); - int16_t mid_stick = get_throttle_mid(); + int16_t mid_stick = copter.get_throttle_mid(); // protect against unlikely divide by zero if (mid_stick <= 0) { mid_stick = 500; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index ce1c67a2c7..a186bdf370 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -618,7 +618,6 @@ private: float get_pilot_desired_yaw_rate(int16_t stick_angle); void update_throttle_hover(); void set_throttle_takeoff(); - float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f); float get_pilot_desired_climb_rate(float throttle_control); float get_non_takeoff_throttle(); float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index ced681d5bf..4886eb3509 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -563,11 +563,6 @@ float Copter::Mode::get_pilot_desired_climb_rate(float throttle_control) return copter.get_pilot_desired_climb_rate(throttle_control); } -float Copter::Mode::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid) -{ - return copter.get_pilot_desired_throttle(throttle_control, thr_mid); -} - float Copter::Mode::get_non_takeoff_throttle() { return copter.get_non_takeoff_throttle(); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 91a77047af..5019e5ba0c 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -121,6 +121,9 @@ protected: void land_run_horizontal_control(); void land_run_vertical_control(bool pause_descent = false); + // return expected input throttle setting to hover: + virtual float throttle_hover() const; + // convenience references to avoid code churn in conversion: Parameters &g; ParametersG2 &g2; @@ -185,7 +188,7 @@ protected: float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); float get_pilot_desired_yaw_rate(int16_t stick_angle); float get_pilot_desired_climb_rate(float throttle_control); - float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f); + float get_pilot_desired_throttle() const; float get_non_takeoff_throttle(void); void update_simple_mode(void); bool set_mode(control_mode_t mode, mode_reason_t reason); @@ -222,6 +225,13 @@ protected: void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); + float throttle_hover() const override { + if (g2.acro_thr_mid > 0) { + return g2.acro_thr_mid; + } + return Copter::Mode::throttle_hover(); + } + private: }; diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 3f74661143..0e91efc3d7 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -12,7 +12,7 @@ bool Copter::ModeAcro::init(bool ignore_checks) { // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() && - (get_pilot_desired_throttle(channel_throttle->get_control_in(), copter.g2.acro_thr_mid) > copter.get_non_takeoff_throttle())) { + (get_pilot_desired_throttle() > copter.get_non_takeoff_throttle())) { return false; } @@ -22,7 +22,6 @@ bool Copter::ModeAcro::init(bool ignore_checks) void Copter::ModeAcro::run() { float target_roll, target_pitch, target_yaw; - float pilot_throttle_scaled; // if not armed set throttle to zero and exit immediately if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) { @@ -38,14 +37,13 @@ void Copter::ModeAcro::run() // convert the input to the desired body frame rate get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); - // get pilot's desired throttle - pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid); - // run attitude controller attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); // output pilot's throttle without angle boost - attitude_control->set_throttle_out(pilot_throttle_scaled, false, copter.g.throttle_filt); + attitude_control->set_throttle_out(get_pilot_desired_throttle(), + false, + copter.g.throttle_filt); } diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 1d8f203df1..0e79ead597 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -42,7 +42,6 @@ void Copter::ModeDrift::run() static float roll_input = 0.0f; float target_roll, target_pitch; float target_yaw_rate; - float pilot_throttle_scaled; // if landed and throttle at zero, set throttle to zero and exit immediately if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) { @@ -58,9 +57,6 @@ void Copter::ModeDrift::run() // convert pilot input to lean angles get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); - // get pilot's desired throttle - pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in()); - // Grab inertial velocity const Vector3f& vel = inertial_nav.get_velocity(); @@ -101,7 +97,8 @@ void Copter::ModeDrift::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // output pilot's throttle with angle boost - attitude_control->set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); + const float assisted_throttle = get_throttle_assist(vel.z, get_pilot_desired_throttle()); + attitude_control->set_throttle_out(assisted_throttle, true, g.throttle_filt); } // get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and z-axis velocity diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index 01468bf282..d260ab08a8 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -100,7 +100,7 @@ void Copter::ModeFlip::run() } // get pilot's desired throttle - float throttle_out = get_pilot_desired_throttle(channel_throttle->get_control_in()); + float throttle_out = get_pilot_desired_throttle(); // get corrected angle based on direction and axis of rotation // we flip the sign of flip_angle to minimize the code repetition diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 37fb77d71c..5d1c0b607c 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -9,7 +9,7 @@ bool Copter::ModeStabilize::init(bool ignore_checks) { // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() && - (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) { + (get_pilot_desired_throttle() > get_non_takeoff_throttle())) { return false; } @@ -22,7 +22,6 @@ void Copter::ModeStabilize::run() { float target_roll, target_pitch; float target_yaw_rate; - float pilot_throttle_scaled; // if not armed set throttle to zero and exit immediately if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) { @@ -44,14 +43,13 @@ void Copter::ModeStabilize::run() // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - // get pilot's desired throttle - pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in()); - // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // body-frame rate controller is run directly from 100hz loop // output pilot's throttle - attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt); + attitude_control->set_throttle_out(get_pilot_desired_throttle(), + true, + g.throttle_filt); }