AP_Motors: Add variable to record the final thrust value

Add Throttle_Out for other frames

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);

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

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)

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;

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;

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

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