Configurable battery compensation

[knmcguire]

So based on the discussion of PR #931 we looked at the motor battery compensation variables looking a bit hardcoded and very specific for the crazyflie:

crazyflie-firmware/src/drivers/src/motors.c

Lines 109 to 173 in b0c72f2

	// We have data that maps PWM to thrust at different supply voltage levels.
	// However, it is not the PWM that drives the motors but the voltage and
	// amps (= power). With the PWM it is possible to simulate different
	// voltage levels. The assumption is that the voltage used will be an
	// procentage of the supply voltage, we assume that 50% PWM will result in
	// 50% voltage.
	//
	// Thrust (g) Supply Voltage PWM (%) Voltage needed
	// 0.0 4.01 0 0
	// 1.6 3.98 6.25 0.24875
	// 4.8 3.95 12.25 0.49375
	// 7.9 3.82 18.75 0.735
	// 10.9 3.88 25 0.97
	// 13.9 3.84 31.25 1.2
	// 17.3 3.80 37.5 1.425
	// 21.0 3.76 43.25 1.6262
	// 24.4 3.71 50 1.855
	// 28.6 3.67 56.25 2.06438
	// 32.8 3.65 62.5 2.28125
	// 37.3 3.62 68.75 2.48875
	// 41.7 3.56 75 2.67
	// 46.0 3.48 81.25 2.8275
	// 51.9 3.40 87.5 2.975
	// 57.9 3.30 93.75 3.09375
	//
	// To get Voltage needed from wanted thrust we can get the quadratic
	// polyfit coefficients using GNU octave:
	//
	// thrust = [0.0 1.6 4.8 7.9 10.9 13.9 17.3 21.0 ...
	// 24.4 28.6 32.8 37.3 41.7 46.0 51.9 57.9]
	//
	// volts = [0.0 0.24875 0.49375 0.735 0.97 1.2 1.425 1.6262 1.855 ...
	// 2.064375 2.28125 2.48875 2.67 2.8275 2.975 3.09375]
	//
	// p = polyfit(thrust, volts, 2)
	//
	// => p = -0.00062390 0.08835522 0.06865956
	//
	// We will not use the contant term, since we want zero thrust to equal
	// zero PWM.
	//
	// And to get the PWM as a percentage we would need to divide the
	// Voltage needed with the Supply voltage.
	static uint16_t motorsCompensateBatteryVoltage(uint16_t ithrust)
	{
	float supply_voltage = pmGetBatteryVoltage();
	/*
	* A LiPo battery is supposed to be 4.2V charged, 3.7V mid-charge and 3V
	* discharged.
	*
	* A suiteble sanity check for disabiling the voltage compensation would be
	* under 2V. That would suggest a damaged battery. This protects against
	* rushing the motors on bugs and invalid voltage levels.
	*/
	if (supply_voltage < 2.0f)
	{
	return ithrust;
	}
	float thrust = ((float) ithrust / 65536.0f) * 60;
	float volts = -0.0006239f * thrust * thrust + 0.088f * thrust;
	float percentage = volts / supply_voltage;
	percentage = percentage > 1.0f ? 1.0f : percentage;
	return percentage * UINT16_MAX;
	}

In history this functionality has been mostly used to achieve more stable hovering when using only Baro (or none at all) for attitude, so that the motors will give the same thrust while the battery is depleting. Now is the question if you want to keep this same functionality for other platforms as the Bolt.

However, now we live in the age of positioning, and technically this function would no longer be necessary with the z-ranger, flowdeck or any other deck that gives altitude positioning. If it happens to decline in height because of battery ▶️ the state estimator catches that ▶️ the controller compensates for it ▶️ the motors start spinning faster.

This issue is about discussing about the functionality..... is it obsolete and should we remove it in the future? Or would this actually be useful for the Bolt or CF2.1/BQ deck combo and should we make the polynomial values persistent parameters with different default values.

[whoenig]

TLDR: This is important functionality, that should be updated and kept.

This functionality is extremely important, especially when moving to a proper system ID. The goal for CF (and Bolt) in my mind is to have somewhere parameters that we can specify (inertia matrix, mass, arm-length, etc.). Controllers should output desired thrust and moment in SI units (or normalized SI units, e.g., 0..1 like in PX4). Then, a user can pre-tune the system in simulation.

The system ID in the current firmware is unfortunately not very good. What is missing is an estimation of the total possible thrust (a function of the battery state actually). In my experiments, it also didn't match reality well - perhaps because it was measured a long time ago manually. The system-ID board that Tobi developed provides much more accurate results. The data from that is also what we currently use in the lab, with another controller that uses SI units, see IMRCLab/crazyflie-firmware@master...IMRCLab:rai-crazyswarm2-demo for the firmware. Using SI units has the advantage that its much easier to go between simulation and real drone. For Bolt, it might be nicer to switch to some sort of normalized SI units.

[knmcguire]

Yes you have a good point 😄 but it seemed to be based on the voltage of the battery already is it not?

But this is also talking about the location of this system id currently. Currently this is part of the motor.c, so a bit adhoc after the controller has done it's work already. Then it should be more intergrated into the controller framework to make it more logical.

[whoenig]

In my experiments, it not only depends on the voltage of the battery, but also the current PWM (low battery voltage can be because the battery is old, because it has little capacity left, or because the motor load is very high).

Agreed that there should be some more central place where we define the system ID (mass, inertia matrix, arm length, etc.). Other parts can then simply include that file if they need parts of the system ID.

[knmcguire]

Yes agreed. I've changed the title of the issue so we will be reminded to look at this eventually. Since we wanted to convert parameters to persistent, we noticed these hardcoded values here, so at one point we need to find a solution for it. One of the reason why I started the discussion in the first place.

[knmcguire]

Mind that this battery compensation no longer holds for the thrust upgrade kit.