From 60b36259503ec5fcc88a95945a944cb4e6828630 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 2 May 2016 08:00:45 +1000 Subject: [PATCH] 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 | 13 ++++++++++++ libraries/AP_Motors/AP_MotorsMatrix.h | 3 +++ libraries/AP_Motors/AP_MotorsMulticopter.cpp | 4 ++++ libraries/AP_Motors/AP_MotorsMulticopter.h | 14 ++++++++++++- libraries/AP_Motors/AP_MotorsTri.cpp | 21 ++++++++++++++++++++ libraries/AP_Motors/AP_MotorsTri.h | 3 +++ 6 files changed, 57 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 40e9a342e7..da831824ab 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/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); + } +} diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index 958eb00935..24d50f1170 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/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) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 6c2570e820..5a331ebf18 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -22,6 +22,7 @@ #include "AP_MotorsMulticopter.h" #include + 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(); }; diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index bb4736df5b..c95d944039 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/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; }; diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 0d5876fcb1..0d3f226ac6 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/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]; + } +} diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 1e28a757d5..2ae0ac41e8 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/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