Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: add access to per-motor thrust values

Browse Source
This commit is contained in:
Andy Piper 2024-04-02 21:03:30 +01:00 committed by Andrew Tridgell
parent fd19c3f401
commit a974f3fd42
3 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -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 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 // parameter checks for MOT_PWM_MIN/MAX, returns true if parameters are valid
bool AP_MotorsMulticopter::check_mot_pwm_params() const bool AP_MotorsMulticopter::check_mot_pwm_params() const
{ {

3
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -89,6 +89,9 @@ public:
// convert values to PWM min and max if not configured // convert values to PWM min and max if not configured
void convert_pwm_min_max_param(int16_t radio_min, int16_t radio_max); 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 #if HAL_LOGGING_ENABLED
// 10hz logging of voltage scaling and max trust // 10hz logging of voltage scaling and max trust
void Log_Write() override; void Log_Write() override;

1
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -155,6 +155,7 @@ public:
float get_yaw() const { return _yaw_in; } float get_yaw() const { return _yaw_in; }
float get_yaw_ff() const { return _yaw_in_ff; } float get_yaw_ff() const { return _yaw_in_ff; }
float get_throttle_out() const { return _throttle_out; } 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() 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_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; } float get_throttle_slew_rate() const { return _throttle_slew_rate; }