Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsMulticopter: add inverse thrust_to_actuator and thrust curve functions

This commit is contained in:
Iampete1 2021-09-20 17:56:13 +01:00 committed by Peter Hall
parent d5286ec533
commit 0c1fe0b373
2 changed files with 40 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -338,20 +338,37 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) const
{
float throttle_ratio = thrust;
float battery_scale = 1.0;
if (is_positive(_batt_voltage_filt.get())) {
battery_scale = 1.0 / _batt_voltage_filt.get();
}
// apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
if (is_zero(thrust_curve_expo)) {
// zero expo means linear, avoid floating point exception for small values
return _lift_max * thrust;
}
if (is_positive(_batt_voltage_filt.get())) {
throttle_ratio = ((thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * _lift_max * thrust)) / (2.0f * thrust_curve_expo * _batt_voltage_filt.get());
} else {
throttle_ratio = ((thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * _lift_max * thrust)) / (2.0f * thrust_curve_expo);
return _lift_max * thrust * battery_scale;
}
float throttle_ratio = ((thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * _lift_max * thrust)) / (2.0f * thrust_curve_expo);
return constrain_float(throttle_ratio * battery_scale, 0.0f, 1.0f);
}
return constrain_float(throttle_ratio, 0.0f, 1.0f);
// inverse of above
float AP_MotorsMulticopter::remove_thrust_curve_and_volt_scaling(float throttle) const
{
float battery_scale = 1.0;
if (is_positive(_batt_voltage_filt.get())) {
battery_scale = 1.0 / _batt_voltage_filt.get();
}
// apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
if (is_zero(thrust_curve_expo)) {
// zero expo means linear, avoid floating point exception for small values
return throttle / (_lift_max * battery_scale);
}
float thrust = ((throttle / battery_scale) * (2.0f * thrust_curve_expo)) - (thrust_curve_expo - 1.0f);
thrust = (thrust * thrust) - ((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo));
thrust /= 4.0f * thrust_curve_expo * _lift_max;
return constrain_float(thrust, 0.0f, 1.0f);
}
// update_lift_max from battery voltage - used for voltage compensation
@ -417,12 +434,19 @@ int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
}
// converts desired thrust to linearized actuator output in a range of 0~1
float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in)
float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in) const
{
thrust_in = constrain_float(thrust_in, 0.0f, 1.0f);
return _spin_min + (_spin_max - _spin_min) * apply_thrust_curve_and_volt_scaling(thrust_in);
}
// inverse of above
float AP_MotorsMulticopter::actuator_to_thrust(float actuator) const
{
actuator = (actuator - _spin_min) / (_spin_max - _spin_min);
return constrain_float(remove_thrust_curve_and_volt_scaling(actuator), 0.0f, 1.0f);
}
// adds slew rate limiting to actuator output
void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float input)
{

9
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -83,12 +83,15 @@ public:
// get minimum or maximum pwm value that can be output to motors
int16_t get_pwm_output_min() const;
int16_t get_pwm_output_max() const;
// parameter check for MOT_PWM_MIN/MAX, returns true if parameters are valid
bool check_mot_pwm_params() const;
// converts desired thrust to linearized actuator output in a range of 0~1
float thrust_to_actuator(float thrust_in);
float thrust_to_actuator(float thrust_in) const;
// inverse of above
float actuator_to_thrust(float actuator) const;
// set thrust compensation callback
FUNCTOR_TYPEDEF(thrust_compensation_fn_t, void, float *, uint8_t);
@ -122,6 +125,8 @@ protected:
// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float apply_thrust_curve_and_volt_scaling(float thrust) const;
// inverse of above
float remove_thrust_curve_and_volt_scaling(float throttle) const;
// update_lift_max_from_batt_voltage - used for voltage compensation
void update_lift_max_from_batt_voltage();