Hi,
voltage compensation to throttle/pwm is used in many places in motor output function. I think it is used repeatedly, there is no need to use it so many times.
The current situation is as follows:
voltage compensation is used in three functions in output():
void AP_MotorsMulticopter::output()
{
update_lift_max_from_batt_voltage();
output_armed_stabilizing();
output_to_motors();
}
(1) the first function:
_lift_max = _batt_voltage_resting_estimate / _batt_voltage_max
void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
{
float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate / _batt_voltage_max, _dt);
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
_lift_max = batt_voltage_filt * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt * batt_voltage_filt;
}
(2) the second function:
compensation_gain = 1 / _lift_max
and
throttle_thrust = get_throttle() * compensation_gain;
void AP_MotorsMatrix::output_armed_stabilizing()
{
const float compensation_gain = get_compensation_gain();
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
(3) the third function:
use _lift_max and battery_scale = 1 / batt_voltage_filt
void AP_MotorsMatrix::output_to_motors()
{
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
set_actuator_with_slew(_actuator[i], thrust_to_actuator(_thrust_rpyt_out[i]));
}
--->
float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in) const
{
return _spin_min + (_spin_max - _spin_min) * apply_thrust_curve_and_volt_scaling(thrust_in);
}
--->
float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) const
{
float battery_scale = 1.0 / _batt_voltage_filt.get();
if (is_zero(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);
}
we use simplified throttle->pwm as the key formula for convenience and simple:
$$
pwm = \sqrt{_lift_max * thrust} * battery_scale
$$
In above formula, voltage compensation used in several times:
(1) thrust:
void AP_MotorsMatrix::output_armed_stabilizing() {
throttle_thrust = get_throttle() * compensation_gain;
}
we can get
thrust = thrust_raw * compensation_gain = thrust_raw * (1 / _lift_max)
= thrust_raw / (batt_voltage_resting / _batt_voltage_max)
(2) _lift_max:
_lift_max = batt_voltage_resting / _batt_voltage_max
(3) battery_scale:
battery_scale = 1.0 / _batt_voltage_filt = 1 / (batt_voltage_resting / _batt_voltage_max)
so,
pwm_normalized = _lift_max * thrust * battery_scale
= (batt_voltage_resting / _batt_voltage_max) *
thrust_raw / (batt_voltage_resting / _batt_voltage_max) *
1 / (batt_voltage_resting / _batt_voltage_max)
= thrust_raw / (batt_voltage_resting / _batt_voltage_max)
= thrust_raw * battery_scale
not end… I have to sleep now!