diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index dd222c8592..93d7bd78ee 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -186,6 +186,8 @@ void AP_MotorsCoax::output_armed_stabilizing() // calculate the throttle setting for the lift fan thrust_out = throttle_avg_max + thr_adj; + // compensation_gain can never be zero + _throttle_out = thrust_out / compensation_gain; if (fabsf(yaw_thrust) > thrust_out) { yaw_thrust = constrain_float(yaw_thrust, -thrust_out, thrust_out); diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 3f1efc7fa0..31a8e1bbe9 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -302,8 +302,13 @@ void AP_MotorsMatrix::output_armed_stabilizing() } } + // determine throttle thrust for harmonic notch + const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj; + // compensation_gain can never be zero + _throttle_out = throttle_thrust_best_plus_adj / compensation_gain; + // check for failed motor - check_for_failed_motor(throttle_thrust_best_rpy + thr_adj); + check_for_failed_motor(throttle_thrust_best_plus_adj); } // check for failed motor diff --git a/libraries/AP_Motors/AP_MotorsMatrixTS.cpp b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp index aab1148b4b..fa6bcb33d1 100644 --- a/libraries/AP_Motors/AP_MotorsMatrixTS.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp @@ -114,6 +114,9 @@ void AP_MotorsMatrixTS::output_armed_stabilizing() } } + // compensation_gain can never be zero + _throttle_out = (throttle_thrust + thr_adj) / compensation_gain; + } void AP_MotorsMatrixTS::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index d454c4f3ec..ea67ee1efe 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -202,6 +202,8 @@ void AP_MotorsSingle::output_armed_stabilizing() // calculate the throttle setting for the lift fan _thrust_out = throttle_avg_max + thr_adj; + // compensation_gain can never be zero + _throttle_out = _thrust_out / compensation_gain; if (is_zero(_thrust_out)) { limit.roll = true; diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.cpp b/libraries/AP_Motors/AP_MotorsTailsitter.cpp index 0f7a272868..2a072c15f0 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.cpp +++ b/libraries/AP_Motors/AP_MotorsTailsitter.cpp @@ -169,6 +169,8 @@ void AP_MotorsTailsitter::output_armed_stabilizing() _thrust_left = constrain_float(_thrust_left + thr_adj, 0.0f, 1.0f); _thrust_right = constrain_float(_thrust_right + thr_adj, 0.0f, 1.0f); _throttle = throttle_thrust + thr_adj; + // compensation_gain can never be zero + _throttle_out = _throttle / compensation_gain; // thrust vectoring _tilt_left = pitch_thrust - yaw_thrust; diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 836adb32cc..c9ad76b805 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -255,10 +255,15 @@ void AP_MotorsTri::output_armed_stabilizing() } } + // determine throttle thrust for harmonic notch + const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj; + // compensation_gain can never be zero + _throttle_out = throttle_thrust_best_plus_adj / compensation_gain; + // add scaled roll, pitch, constrained yaw and throttle for each motor - _thrust_right = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_right; - _thrust_left = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_left; - _thrust_rear = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_rear; + _thrust_right = throttle_thrust_best_plus_adj + rpy_scale * _thrust_right; + _thrust_left = throttle_thrust_best_plus_adj + rpy_scale * _thrust_left; + _thrust_rear = throttle_thrust_best_plus_adj + rpy_scale * _thrust_rear; // scale pivot thrust to account for pivot angle // we should not need to check for divide by zero as _pivot_angle is constrained to the 5deg ~ 80 deg range diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index e01131eab7..ddd69d5186 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -97,6 +97,7 @@ public: float get_roll() const { return _roll_in; } float get_pitch() const { return _pitch_in; } float get_yaw() const { return _yaw_in; } + float get_throttle_out() const { return _throttle_out; } float get_throttle() const { return constrain_float(_throttle_filter.get(), 0.0f, 1.0f); } float get_throttle_bidirectional() const { return constrain_float(2 * (_throttle_filter.get() - 0.5f), -1.0f, 1.0f); } float get_forward() const { return _forward_in; } @@ -225,6 +226,7 @@ protected: float _yaw_in; // desired yaw control from attitude controller, -1 ~ +1 float _yaw_in_ff; // desired yaw feed forward control from attitude controller, -1 ~ +1 float _throttle_in; // last throttle input from set_throttle caller + float _throttle_out; // throttle after mixing is complete float _forward_in; // last forward input from set_forward caller float _lateral_in; // last lateral input from set_lateral caller float _throttle_avg_max; // last throttle input from set_throttle_avg_max