From 35ef761deb10bfd7a6b1aaab4d6f9a0565ba1439 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 21 Mar 2016 21:27:39 +1030 Subject: [PATCH] AC_AttitudeControl: Move set_throttle_out to _Multi and _Heli --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 14 -------------- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 2 +- .../AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 11 ++++++----- .../AC_AttitudeControl/AC_AttitudeControl_Heli.h | 3 +++ .../AC_AttitudeControl_Multi.cpp | 14 ++++++++++++++ .../AC_AttitudeControl/AC_AttitudeControl_Multi.h | 3 +++ 6 files changed, 27 insertions(+), 20 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index b8f552b965..72e0756b0c 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -600,20 +600,6 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits) } } -void AC_AttitudeControl::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff) -{ - _throttle_in = throttle_in; - _throttle_in_filt.apply(throttle_in, _dt); - _motors.set_throttle_filter_cutoff(filter_cutoff); - if (apply_angle_boost) { - _motors.set_throttle(get_boosted_throttle(throttle_in)); - }else{ - _motors.set_throttle(throttle_in); - // Clear angle_boost for logging purposes - _angle_boost = 0.0f; - } -} - void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff) { _throttle_in = throttle_in; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index dde7604a66..9cc8a9177f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -191,7 +191,7 @@ public: void accel_limiting(bool enable_or_disable); // Set output throttle - void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff); + virtual void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) = 0; // Set output throttle and disable stabilization void set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filt_cutoff); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index ff25288aad..e6c3afea59 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -404,11 +404,12 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_rads) // throttle functions // -// returns a throttle including compensation for roll/pitch angle -// throttle value should be 0 ~ 1000 -float AC_AttitudeControl_Heli::get_boosted_throttle(float throttle_in) +void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff) { - // no angle boost for trad helis + _throttle_in = throttle_in; + _throttle_in_filt.apply(throttle_in, _dt); + _motors.set_throttle_filter_cutoff(filter_cutoff); + _motors.set_throttle(throttle_in); + // Clear angle_boost for logging purposes _angle_boost = 0.0f; - return throttle_in; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 440bb8eaa6..1212442690 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -95,6 +95,9 @@ public: // set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition. void set_hover_roll_trim_scalar(float scalar) {_hover_roll_trim_scalar = constrain_float(scalar, 0.0f, 1.0f);} + // Set output throttle + void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override; + // user settable parameters static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 4de2bc1744..41b44c18a4 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -147,6 +147,20 @@ float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const return ToDeg(acos(constrain_float(_throttle_in_filt.get()/(0.9f * thr_max), 0.0f, 1.0f))) * 100.0f; } +void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff) +{ + _throttle_in = throttle_in; + _throttle_in_filt.apply(throttle_in, _dt); + _motors.set_throttle_filter_cutoff(filter_cutoff); + if (apply_angle_boost) { + _motors.set_throttle(get_boosted_throttle(throttle_in)); + }else{ + _motors.set_throttle(throttle_in); + // Clear angle_boost for logging purposes + _angle_boost = 0.0f; + } +} + // returns a throttle including compensation for roll/pitch angle // throttle value should be 0 ~ 1 float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 66254f1303..cf9fa1409b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -55,6 +55,9 @@ public: // get lean angle max for pilot input that prioritises altitude hold over lean angle float get_althold_lean_angle_max() const; + // Set output throttle + void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override; + // calculate total body frame throttle required to produce the given earth frame throttle float get_boosted_throttle(float throttle_in);