Copter: Voltage compensation to throttle/pwm is used in many places in motor output function, is it used repeatedly?

luweiagi commented 1 year ago

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 =  battery_scale * sqrt(_lift_max * thrust)

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 =  battery_scale * sqrt(_lift_max * thrust)
= battery_scale * sqrt(_lift_max * thrust_raw * compensation_gain)
= battery_scale * sqrt(_lift_max * thrust_raw * (1 / _lift_max))
= battery_scale * sqrt(thrust_raw)
= 1 / (batt_voltage_resting / _batt_voltage_max)  *  sqrt(thrust_raw)

So, you see, there is no need to create variable _lift_max and no need to compensite in output_armed_stabilizing like *`throttle_thrust = get_throttle() compensation_gain;`. Just using battery_scale** is enough.

I wonder to know if votage is linear to pwm/rpm? or votage is linear to force/thrust? if votage is linear to pwm/rpm, is below situation right？ suppose votage_full = 10v, and votage_now = 8v, and we need pwm = sqrt(thrust) = 1000, but for the season of votage_now is low, so the actual pwm is 1000(8/10)=800? so we must let pwm go to 1000/(8/10)=1250, and the actual pwm is 1250(8/10)=1000? If my guess is right, then I think it is right that putting voltage compensation(battery_scale) outside the sqrt(thrust), it is no problem. But _lift_max is redundant, because _lift_max * (1 / _lift_max ) = 1 inside sqrt(thrust).

by the way, I noticed that in compensation_gain there is air density compensation, This is ok and nessasary. So, pwm can be

pwm =  battery_scale * sqrt(thrust_raw * compensation_gain)
= battery_scale * sqrt(thrust_raw * (1 / _air_density_ratio))

To summarize， I think there is no need to reserve _lift_max.

IamPete1 commented 1 year ago

Sorry, this is a little tricky to follow, could you open a PR with the simplifications you suggest? We have some nice tests for motors now so we should be able to prove its the same. I think the idea is that the result would be the same?

andyp1per commented 1 year ago

Also please review https://github.com/ArduPilot/ardupilot/pull/23237 this might change your maths so there is an option being introduced

lthall commented 1 year ago

Hi @luweiagi,

I always like seeing your code reviews! I have to be equally carful with my replies so it will take me a little time.

The motor mixer was a difficult thing to get right (or at least right enough). I had a couple confusing aspects to deal with:

The maximum thrust of each actuator changed over time meaning the saturation limits reduced throughout the flight,
The code using this needed to know when saturation happened and in what way,
The actuators are not linear and not consistently non-linear from aircraft to aircraft.

I found it easier to write develop the mixing and limiting code in a strict -1 to 1 or 0 to 1 range and then scale the input and output back to the instantaneous actuator range. This is why we scale everything by a gain at the beginning and again before we convert to the pwm output. I figured when I got it right I could play with the code to improve readability, speed, or simplicity once I got the math working correctly.

I say all this before reading your issue in detail, I would not be surprised if you are highlighting aspects of this. Your opening statement for example:

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.

In (1), I am confused why you wrote this:

_lift_max = _batt_voltage_resting_estimate / _batt_voltage_max

This is not correct and I wonder if your statements below this assume this.

I wonder to know if voltage is linear to pwm/rpm? or voltage is linear to force/thrust?

Neither, rpm = kv * (v - iR)

So as the load builds up the rpm drops below the perfect no-load linear voltage to rpm relationship defined by kv. So basically the more losses in the system the more non-linear the relationship is. The current is proportional to torque so the propeller drag increases the losses and rpm. We assume thrust is aproximatly proportional to rpm^2 but these losses reduce the rpm. So we have an approximation that fits this reasonably well before propeller stall. This is where the parameter thrust_curve_expo is used. We assume the thrust curve is somewhere between a linear rpm and rpm^2 relationship, where it moves towards the linear end as losses increase. This is because losses become more significant as rpm increases. This is why our maximum thrust varies with battery voltage:

_lift_max = batt_voltage_filt * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt * batt_voltage_filt;

I think there is no need to reserve _lift_max.

Yes, I am sure this is true but it is also a useful value to keep in mind to understand what is going on. We command thrust between zero and _lift_max on each actuator, that is also where we start seeing saturation. _lift_max at full battery is what we consider 100% thrust. I have not sat down and went through a simplification process on this code yet so I am very open to the idea that it may be easier to understand without this variable.

Thanks for reading through this. There is a lot of potential to clean up the Motor Mixer code. Pete has done some great work but there are a lot of places we can make it easier to understand and read.

Not relevant just yet but we probably shouldn't have the air density here but instead part of lift max.

float AP_MotorsMulticopter::get_compensation_gain() const
{
    // avoid divide by zero
    if (_lift_max <= 0.0f) {
        return 1.0f;
    }

    float ret = 1.0f / _lift_max;

#if AP_MOTORS_DENSITY_COMP == 1
    // air density ratio is increasing in density / decreasing in altitude
    if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) {
        ret *= 1.0f / constrain_float(_air_density_ratio, 0.5f, 1.25f);
    }
#endif
    return ret;
}

luweiagi commented 1 year ago

Hi @luweiagi,

I always like seeing your code reviews! I have to be equally carful with my replies so it will take me a little time.

Thanks @lthall ,

Before I see the detail of your reply and think deeper, I want to make sure what the R refer to?

rpm = kv * (v - iR)

Does R refer to the internal resistance of the battery?

================================== the following is irrelevant to the questio, I just display at here: When I seek wich function calulates the _batt_voltage_resting_estimate, I found this function update_resistance_estimate(), and I feel it is clever and interesting. Below is my understand (simplified and remove filter)

another point I found, witch is also irrelevant to the question, I just mention it by the way:

In fact, althoug I draw this picture, but I still confused why we must add y=x? I think it shoud be totally y=sqrt(x)... how could it be: ? I feel this(y=x) is unreasonable... the reason I guess has two points:

in Using “measured” MOT_THST_EXPO: What improvement can one expect? said:

From what I have seen from T-Motor ESC’s they appear to do some thrust linearisation on the ESC

namely, we must apply the thrust linearisation, so we add y=x. But I think it is not the real reason.

just for control convenient. like sqrt_controller, if we only use y=sqrt(x), when x is smal, y changes very drastically because its slope is infinite, so it should be linearized near zero, so we mix y=x and y=sqrt(x) to get more linearisation near zero to let its slope is finite? This also I think is not true reason...

lthall commented 1 year ago

rpm = kv * (v - iR)

is the basic motor model. So you can see that when you have zero torque (zero current) you do get a linear rpm to voltage drive relationship. The pwm command to the esc is equal to the voltage drive. The maximum voltage drive is the battery voltage. R is the internal resistance of the motor, not the battery.

I think you summarised the battery resistance estimation stuff well.

I still confused why we must add y=x?

Because when a power system becomes very lossy, linear becomes a better approximation, and then starts going the other way. I have actually had a DC motor copter with a negative expo! What is basically happening is the losses are building up faster than the thrust :)

The other thing to keep in mind is we don't start at zero thrust. The higher we set the minimum PWM the more linear the rest of the curve is because we assume zero thrust request is really zero thrust.

The one thing I can say about my work here is I can justify every single decision I made. I am happy to explain it, it helps me review those decisions and the code that was supposed to represent them :)

luweiagi commented 1 year ago

rpm = kv * (v - iR)

is the basic motor model.

Hi, @lthall Yes, I didn't learned the knowledge of motor before. Yesterday, I learned the knowledge of motor . This is the code flowchart of thrust_curve_expo. You can open it in new tab if not clear. You can see the blue star and red star. Let's see the red star.(see the blue star later). The red star show the relation of thrust and pwm. namely: However, I think the relation between thrust and pwm showed in prev picture may be not correctly. I will prove my point next:

The following diagram is the equivalent circuit diagram of a DC motor: So, I proved that the relation of PWM(or Voltage) and Thrust(Lift) is previous equition, not the follow equition as in code:

I do not know if I am right, but the result of my formula is like this...

lthall commented 1 year ago

Sorry @luweiagi, I don't understand what you are trying to tell me.

luweiagi commented 1 year ago

Sorry @luweiagi, I don't understand what you are trying to tell me.

@lthall Sorry for my unclear description. Let me explain my purpose in doing the previsous reply. From the following code , we can get pwm=f(thrust): So, we can infer in reverse thrust = g(pwm):

I do not know if pwm=f(thrust) above is an empirical formula (just for the convenience of data fitting, based on intuition of the scene, without any theory support) or derived from some theory (i.e. has theory surport)?

(1) If it is just an empirical formula, I will no longer try to seek a theoretical explanations and just try to understand the scene. (2) If it is derived from some theory, what is the theory it relies on? I want to find the theory which can derive pwm=f(thrust). The last reply is my derivation. But I don't known whether it is correct.

lthall commented 1 year ago

@luweiagi I love how much effort you got to, I really appreciate it!!

(1) If it is just an empirical formula, I will no longer try to seek a theoretical explanations and just try to understand the scene.

It is an empirical formula.

As your equations above show, when the Cl/Cd is high the PWM to thrust curve is approximately Thrust ~ PWM ^2 and becomes more linear as the Cl/Cd drops. As you point out the expo equation is not derived from the ideal propeller/motor model however:

the ideal propeller / motor model is only an approximation of the physics
the expo must be derived from measurements of the thrust curve and are generally only an approximation of the aircraft
the minimum pwm output to the motor is not zero thrust
we don't know exactly what the pwm to voltage relationship is and it varies from esc to esc

I chose it because the exponential curve is:

simple,
familiar to people
would fit the thrust curve reasonably well

I was able to get my hands on approximately 110 thrust curves of various propellers/motor combinations, ranging from 5 inch to 30 inch. I used that data to ensure I was able to get a reasonable fit using the simple expo equation.

I want to find the theory which can derive pwm=f(thrust). https://github.com/ArduPilot/ardupilot/issues/23575#issuecomment-1523326630 is my derivation. But I don't known whether it is correct.

I think it is pretty good. I always struggle a little with the propeller equations as I have no formal aerodynamics education and from what I can see the Cl and Cd vary a lot based on inflow and rpm.

I have not had the time to go back through the motors libary to see how much easier I can make it to understand and read so I suspect there is a lot we can do to improve this code! Looking at your results above I also wonder if we can get slightly better results using a curve based on your equation above as the shape at the low rpm's vary a little from what we are using now.

I would like to have a chat sometime @luweiagi, ping me on discord or discuss if you comfortable with that.

luweiagi commented 1 year ago

@luweiagi I love how much effort you got to, I really appreciate it!!

Hi, @lthall I'm sorry for not replying you in time because I have a long holiday.

(1) Thank you for your patient communication and explanation. Now I think I have almost completely understand the moter output function.

(2) I found two places which can be modified in the ArduPilot Motor Thrust Fit

when I download from google doc, the content has change and excel not souport the sql code. I have changed it to excel function which google doc also surport:

from

=QUERY(A12:B1000,"select B where A >= "&G5&" order by B limit 1",0)

to

=INDEX(B12:B1000, MATCH(SMALL(A12:A1000, COUNTIF(A12:A1000, "<"&$G$5)+1), A12:A1000, 0), 1)

I have momdified this file and save to new ArduPilot Motor Thrust Fit

The name Thrust at MOT SPIN_MAX seems not right for its content. So I think it can be called AVG of Rescaled Thrust.

(3) I think it is interesting to optimize the motor output function with possible correct cognition： Maybe we can optimize this function if time is enough, and I am glad to participate in it.

(4) If you have any poblem or want to call me, just send me a email: luweiagi@163.com and I will reply once I see it.

I would like to have a chat sometime @luweiagi, ping me on discord or discuss if you comfortable with that.

No problem! I will go to the discord and learn how to use it. I am glad to

ArduPilot / ardupilot

Copter: Voltage compensation to throttle/pwm is used in many places in motor output function, is it used repeatedly? #23575