AP_Motors: added hook for vehicle based thrust compensation

allow vehicle code to compensate for thrust effectiveness changes due
to properties outside the scope of AP_Motors. This allows for
compensation in tiltrotors and tiltwings.

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -406,3 +406,16 @@ void AP_MotorsMatrix::normalise_rpy_factors()
         }
     }
 }
+
+
+/*
+  call vehicle supplied thrust compensation if set. This allows
+  vehicle code to compensate for vehicle specific motor arrangements
+  such as tiltrotors or tiltwings
+*/
+void AP_MotorsMatrix::thrust_compensation(void)
+{
+    if (_thrust_compensation_callback) {
+        _thrust_compensation_callback(_thrust_rpyt_out, AP_MOTORS_MAX_NUM_MOTORS);
+    }
+}

libraries/AP_Motors/AP_MotorsMatrix.h

@@ -71,6 +71,9 @@ protected:
     // normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
     void                normalise_rpy_factors();
 
+    // call vehicle supplied thrust compensation if set
+    void                thrust_compensation(void) override;
+    
     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)

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -22,6 +22,7 @@
 
 #include "AP_MotorsMulticopter.h"
 #include <AP_HAL/AP_HAL.h>
+
 extern const AP_HAL::HAL& hal;
 
 // parameters for the motor class
@@ -152,6 +153,9 @@ void AP_MotorsMulticopter::output()
     // calculate thrust
     output_armed_stabilizing();
 
+    // apply any thrust compensation for the frame
+    thrust_compensation();
+    
     // convert rpy_thrust values to pwm
     output_to_motors();
 };

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -104,7 +104,13 @@ public:
     // mask. This is used to control tiltrotor motors in forward
     // flight. Thrust is in the range 0 to 1
     void                output_motor_mask(float thrust, uint8_t mask);
-        
+
+    // set thrust compensation callback
+    FUNCTOR_TYPEDEF(thrust_compensation_fn_t, void, float *, uint8_t);
+    void                set_thrust_compensation_callback(thrust_compensation_fn_t callback) {
+        _thrust_compensation_callback = callback;
+    }
+    
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo        var_info[];
 
@@ -137,6 +143,9 @@ protected:
     // convert thrust (0~1) range back to pwm range
     int16_t             calc_thrust_to_pwm(float thrust_in) const;
 
+    // apply any thrust compensation for the frame
+    virtual void        thrust_compensation(void) {}
+    
     // flag bitmask
     struct {
         spool_up_down_mode     spool_mode       : 3;    // motor's current spool mode
@@ -175,4 +184,7 @@ protected:
     int16_t             _batt_timer;            // timer used in battery resistance calcs
     float               _lift_max;              // maximum lift ratio from battery voltage
     float               _throttle_limit;        // ratio of throttle limit between hover and maximum
+
+    // vehicle supplied callback for thrust compensation. Used for tiltrotors and tiltwings
+    thrust_compensation_fn_t _thrust_compensation_callback;
 };

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -344,3 +344,24 @@ int16_t AP_MotorsTri::calc_yaw_radio_output(float yaw_input, float yaw_input_max
 
     return ret;
 }
+
+/*
+  call vehicle supplied thrust compensation if set. This allows for
+  vehicle specific thrust compensation for motor arrangements such as
+  the forward motors tilting
+*/
+void AP_MotorsTri::thrust_compensation(void)
+{
+    if (_thrust_compensation_callback) {
+        // convert 3 thrust values into an array indexed by motor number
+        float thrust[4] { _thrust_right, _thrust_left, 0, _thrust_rear };
+
+        // apply vehicle supplied compensation function
+        _thrust_compensation_callback(thrust, 4);
+
+        // extract compensated thrust values
+        _thrust_right = thrust[0];
+        _thrust_left  = thrust[1];
+        _thrust_rear  = thrust[3];
+    }
+}

libraries/AP_Motors/AP_MotorsTri.h

@@ -54,6 +54,9 @@ protected:
     // output - sends commands to the motors
     void                output_armed_stabilizing();
 
+    // call vehicle supplied thrust compensation if set
+    void                thrust_compensation(void) override;
+    
     // calc_yaw_radio_output - calculate final radio output for yaw channel
     int16_t             calc_yaw_radio_output(float yaw_input, float yaw_input_max);        // calculate radio output for yaw servo, typically in range of 1100-1900