AP_Motors: add throttle factor to Matrix

libraries/AP_Motors/AP_Motors6DOF.h

@@ -69,7 +69,6 @@ protected:
     AP_Int8             _motor_reverse[AP_MOTORS_MAX_NUM_MOTORS];
     AP_Float            _forwardVerticalCouplingFactor;
 
-    float               _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to throttle (climb/descent)
     float               _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward/backward
     float               _lateral_factor[AP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to lateral (left/right)
 

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -91,6 +91,24 @@ bool AP_MotorsMatrix::init(uint8_t expected_num_motors)
 
     return true;
 }
+
+// Set throttle factor from scripting
+bool AP_MotorsMatrix::set_throttle_factor(int8_t motor_num, float throttle_factor)
+{
+    if ((_active_frame_class != MOTOR_FRAME_SCRIPTING_MATRIX) ) {
+        // not the correct class
+        return false;
+    }
+
+    if (initialised_ok() || !motor_enabled[motor_num]) {
+        // Already setup or given motor is not enabled
+        return false;
+    }
+
+    _throttle_factor[motor_num] = throttle_factor;
+    return true;
+}
+
 #endif // ENABLE_SCRIPTING
 
 // set update rate to motors - a value in hertz
@@ -370,14 +388,14 @@ void AP_MotorsMatrix::output_armed_stabilizing()
     }
 
     // add scaled roll, pitch, constrained yaw and throttle for each motor
+    const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
     for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
-            _thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + (rpy_scale * _thrust_rpyt_out[i]);
+            _thrust_rpyt_out[i] = (throttle_thrust_best_plus_adj * _throttle_factor[i]) + (rpy_scale * _thrust_rpyt_out[i]);
         }
     }
 
     // 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;
 
@@ -484,7 +502,7 @@ bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm)
 }
 
 // add_motor
-void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order)
+void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor)
 {
     if (initialised_ok()) {
         // do not allow motors to be set if the current frame type has init correctly
@@ -494,15 +512,14 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc
     // ensure valid motor number is provided
     if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
 
-        // increment number of motors if this motor is being newly motor_enabled
+        // enable motor
-        if (!motor_enabled[motor_num]) {
+        motor_enabled[motor_num] = true;
-            motor_enabled[motor_num] = true;
-        }
 
-        // set roll, pitch, thottle factors and opposite motor (for stability patch)
+        // set roll, pitch, yaw and throttle factors
         _roll_factor[motor_num] = roll_fac;
         _pitch_factor[motor_num] = pitch_fac;
         _yaw_factor[motor_num] = yaw_fac;
+        _throttle_factor[motor_num] = throttle_factor;
 
         // set order that motor appears in test
         _test_order[motor_num] = testing_order;
@@ -536,9 +553,10 @@ void AP_MotorsMatrix::remove_motor(int8_t motor_num)
     if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
         // disable the motor, set all factors to zero
         motor_enabled[motor_num] = false;
-        _roll_factor[motor_num] = 0;
+        _roll_factor[motor_num] = 0.0f;
-        _pitch_factor[motor_num] = 0;
+        _pitch_factor[motor_num] = 0.0f;
-        _yaw_factor[motor_num] = 0;
+        _yaw_factor[motor_num] = 0.0f;
+        _throttle_factor[motor_num] = 0.0f;
     }
 }
 
@@ -1040,24 +1058,21 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
 }
 
 // normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
+// normalizes throttle factors so max value is 1 and no value is less than 0
 void AP_MotorsMatrix::normalise_rpy_factors()
 {
     float roll_fac = 0.0f;
     float pitch_fac = 0.0f;
     float yaw_fac = 0.0f;
+    float throttle_fac = 0.0f;
 
     // find maximum roll, pitch and yaw factors
     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
-            if (roll_fac < fabsf(_roll_factor[i])) {
+            roll_fac = MAX(roll_fac,fabsf(_roll_factor[i]));
-                roll_fac = fabsf(_roll_factor[i]);
+            pitch_fac = MAX(pitch_fac,fabsf(_pitch_factor[i]));
-            }
+            yaw_fac = MAX(yaw_fac,fabsf(_yaw_factor[i]));
-            if (pitch_fac < fabsf(_pitch_factor[i])) {
+            throttle_fac = MAX(throttle_fac,MAX(0.0f,_throttle_factor[i]));
-                pitch_fac = fabsf(_pitch_factor[i]);
-            }
-            if (yaw_fac < fabsf(_yaw_factor[i])) {
-                yaw_fac = fabsf(_yaw_factor[i]);
-            }
         }
     }
 
@@ -1073,6 +1088,9 @@ void AP_MotorsMatrix::normalise_rpy_factors()
             if (!is_zero(yaw_fac)) {
                 _yaw_factor[i] = 0.5f * _yaw_factor[i] / yaw_fac;
             }
+            if (!is_zero(throttle_fac)) {
+                _throttle_factor[i] = MAX(0.0f,_throttle_factor[i] / throttle_fac);
+            }
         }
     }
 }

libraries/AP_Motors/AP_MotorsMatrix.h

@@ -35,6 +35,10 @@ public:
 #ifdef ENABLE_SCRIPTING
     // Init to be called from scripting
     virtual bool        init(uint8_t expected_num_motors);
+
+    // Set throttle factor from scripting
+    bool                set_throttle_factor(int8_t motor_num, float throttle_factor);
+
 #endif // ENABLE_SCRIPTING
 
     // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
@@ -78,7 +82,7 @@ public:
     void                disable_yaw_torque(void) override;
 
     // add_motor using raw roll, pitch, throttle and yaw factors
-    void                add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order);
+    void                add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor = 1.0f);
 
 protected:
     // output - sends commands to the motors
@@ -108,6 +112,7 @@ protected:
     float               _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll
     float               _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch
     float               _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to yaw (normally 1 or -1)
+    float               _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to throttle 0~1
     float               _thrust_rpyt_out[AP_MOTORS_MAX_NUM_MOTORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
     uint8_t             _test_order[AP_MOTORS_MAX_NUM_MOTORS];  // order of the motors in the test sequence
 

libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h

@@ -46,7 +46,6 @@ protected:
     // nothing to do for setup, scripting will mark as initalized when done
     void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override {};
 
-    float _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS];     // each motors contribution to up thrust
     float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS];      // each motors contribution to forward thrust
     float _right_factor[AP_MOTORS_MAX_NUM_MOTORS];        // each motors contribution to right thrust