AP_Motors: add access to per-motor thrust values

mirkix · Aug 7, 2024 · a974f3f · a974f3f
1 parent fd19c3f
commit a974f3f
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 0 deletions.
diff --git 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
@@ -89,6 +89,9 @@ class AP_MotorsMulticopter : public AP_Motors {
  // 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
@@ -155,6 +155,7 @@ class AP_Motors {
  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; }