Improving Altitude Hold Mode

[matthew]

According to this page in the wiki: http://wiki.bitcraze.se/projects:crazyf ... itude_hold, the integral (I) part of the altitude hold PID controller is used to compensate for decreasing motor voltage as the battery discharges, and it only resets when the battery is charging. This setup seems kind of fragile to me. There is a lot that can go wrong to throw off the voltage compensation. I've noticed weird behavior of the altitude hold function as the battery level drops, almost to the point where it can't be used after the battery hits 50%. I was wondering if it would make more sense to compensate for battery voltage fluctuations by doing the PID thrust calculations in RPM and then converting the RPM to a PWM percentage based on the measured battery voltage (make a motorsSetSpeed function instead of using motorsSetRatio). Obviously, the max RPM would go down as the battery voltage goes down, but I don't think that would affect the PID controller too much.

Any thoughts or advice before I try to implement it? Is there some big factor I'm overlooking that makes this idea unfeasible?

[tobias]

The altitude hold can be improved, I'm pretty sure of that and I think you are on to something . The issue is that we don't know the RPM. I don't know if you read the RPM measurement posts on our blog but there you will find some useful information.

Since we know the battery voltage it should be possible to do a PWM output function that isn't affected so much by the voltage.

Next step would be to use a physical model as base together with a feed-forward PID. This would probably have to be calibrated for every Crazyflie which is a drawback but could probably improve it a lot in the end.

[matthew]

Here's my first stab at making a motor function that compensates for battery voltage. I used thrust measurement data from the wiki (http://wiki.bitcraze.se/misc:investigat ... easurement) to find a function for converting volts to grams of thrust. I then used the voltage output to calculate a pwm duty cycle based on the current battery voltage.

Code: Select all

void motorsSetThrust(int id, float thrust)
{
	float volts = -0.0006239 * thrust * thrust + 0.088 * thrust + 0.069;
	float supply_voltage = pmGetBatteryVoltage();
	float percentage = volts / supply_voltage;
	percentage = percentage > 1.0 ? 1.0 : percentage;
	uint16_t ratio = percentage * UINT16_MAX;

  switch(id)
  {
    case MOTOR_M1:
      M1_TIM_SETCOMPARE(MOTORS_TIM_M1, C_16_TO_BITS(ratio));
      break;
    case MOTOR_M2:
      M2_TIM_SETCOMPARE(MOTORS_TIM_M2, C_16_TO_BITS(ratio));
      break;
    case MOTOR_M3:
      M3_TIM_SETCOMPARE(MOTORS_TIM_M3, C_16_TO_BITS(ratio));
      break;
    case MOTOR_M4:
      M4_TIM_SETCOMPARE(MOTORS_TIM_M4, C_16_TO_BITS(ratio));
      break;
  }
}

I have been able to successfully use this function to fly my CF, and it seems to compensate well for decreasing battery voltage. Because the calculations are done in grams of thrust instead of % duty cycle, the flight controls need to be re-tuned to achieve the previous level of sensitivity. There also seems to be an issue with stabilizer oscillation as the battery voltage goes below 25%. I believe this is due to the battery's limited ability to source current at low charges, resulting in a lower voltage whenever the motors experience a load. To avoid this issue, am planning to filter the battery voltage before using it in motor calculations.

Because this function is able to compensate for decreasing battery voltage, the altitude hold PID controller doesn't need to rely on the I component to track change in battery voltage. This means that it doesn't have keep track of I state between altHold mode sessions. I modified the code to reset I along with the other PID variables each time altHold is started, and I feel like the results are much more consistent over time.

I guess the next step would be adding feed-forward control to improve the altitude hold system, which should be easier now that I have a functional thrust estimator.
Thoughts?