From a974f3fd42c9c85cb0dfa06efde53b08b79251c7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 2 Apr 2024 21:03:30 +0100 Subject: [PATCH] AP_Motors: add access to per-motor thrust values --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 15 +++++++++++++++ libraries/AP_Motors/AP_MotorsMulticopter.h | 3 +++ libraries/AP_Motors/AP_Motors_Class.h | 1 + 3 files changed, 19 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index abec6ab26e..e8a099998a 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -457,6 +457,21 @@ float AP_MotorsMulticopter::actuator_spin_up_to_ground_idle() const return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * thr_lin.get_spin_min(); } +// return throttle out for motor motor_num, returns true if value is valid false otherwise +bool AP_MotorsMulticopter::get_thrust(uint8_t motor_num, float& thr_out) const +{ + if (motor_num >= AP_MOTORS_MAX_NUM_MOTORS || !motor_enabled[motor_num]) { + return false; + } + + // Constrain to linearization range. + const float actuator = constrain_float(_actuator[motor_num], thr_lin.get_spin_min(), thr_lin.get_spin_max()); + + // Remove linearization and compensation gain + thr_out = thr_lin.actuator_to_thrust(actuator) / thr_lin.get_compensation_gain(); + return true; +} + // parameter checks for MOT_PWM_MIN/MAX, returns true if parameters are valid bool AP_MotorsMulticopter::check_mot_pwm_params() const { diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index d6fdf6a309..7e8491c849 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -89,6 +89,9 @@ public: // convert values to PWM min and max if not configured void convert_pwm_min_max_param(int16_t radio_min, int16_t radio_max); + // return thrust for motor motor_num, returns true if value is valid false otherwise + bool get_thrust(uint8_t motor_num, float& thr_out) const override; + #if HAL_LOGGING_ENABLED // 10hz logging of voltage scaling and max trust void Log_Write() override; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index c4456029fb..76e4928cb3 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -155,6 +155,7 @@ public: float get_yaw() const { return _yaw_in; } float get_yaw_ff() const { return _yaw_in_ff; } float get_throttle_out() const { return _throttle_out; } + virtual bool get_thrust(uint8_t motor_num, float& thr_out) const { return false; } 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_throttle_slew_rate() const { return _throttle_slew_rate; }