pbio/battery: Improve numerical accuracy.
When using millivolts, a moving average divisor of N means that the battery voltage must change by at least N mV to change the average.
This scales the voltage to (approximately) microvolts to avoid this problem.
Also reduce update time to make the average slightly less slow.
#include <pbdrv/motor.h>
// Slow moving average battery voltage.
-static int32_t battery_voltage_avg;
+static int32_t battery_voltage_avg_scaled;
+
+// The average battery value is scaled up numerically
+// to reduce rounding errors in the moving average.
+#define SCALE (1024)
// Initializes average to first measurement.
pbio_error_t pbio_battery_init(void) {
// Get battery voltage.
- uint16_t battery_voltage_now;
- pbio_error_t err = pbdrv_battery_get_voltage_now(&battery_voltage_now);
+ uint16_t battery_voltage_now_mv;
+ pbio_error_t err = pbdrv_battery_get_voltage_now(&battery_voltage_now_mv);
if (err != PBIO_SUCCESS) {
return err;
}
// Initialize average voltage.
- battery_voltage_avg = battery_voltage_now;
+ battery_voltage_avg_scaled = (int32_t) battery_voltage_now_mv * SCALE;
return PBIO_SUCCESS;
}
@@ -33,31 +37,31 @@ pbio_error_t pbio_battery_init(void) {
pbio_error_t pbio_battery_update(void) {
// Get battery voltage.
- uint16_t battery_voltage_now;
- pbio_error_t err = pbdrv_battery_get_voltage_now(&battery_voltage_now);
+ uint16_t battery_voltage_now_mv;
+ pbio_error_t err = pbdrv_battery_get_voltage_now(&battery_voltage_now_mv);
if (err != PBIO_SUCCESS) {
return err;
}
// Update moving average.
- battery_voltage_avg = (battery_voltage_avg * 1023 + battery_voltage_now) / 1024;
+ battery_voltage_avg_scaled = (battery_voltage_avg_scaled * 127 + ((int32_t) battery_voltage_now_mv) * SCALE) / 128;
return PBIO_SUCCESS;
}
// Gets the moving average value.
int32_t pbio_battery_get_average_voltage(void) {
- return battery_voltage_avg;
+ return battery_voltage_avg_scaled / SCALE;
}
// Gets the duty cycle required to output the desired voltage.
int32_t pbio_battery_get_duty_from_voltage(int32_t voltage) {
- return voltage * PBDRV_MAX_DUTY / battery_voltage_avg;
+ return voltage * PBDRV_MAX_DUTY / (battery_voltage_avg_scaled / SCALE);
}
// Gets the voltage resulting from the given duty cycle.
int32_t pbio_battery_get_voltage_from_duty(int32_t duty) {
- return duty * battery_voltage_avg / PBDRV_MAX_DUTY;
+ return duty * (battery_voltage_avg_scaled / SCALE) / PBDRV_MAX_DUTY;
}
#endif // PBIO_CONFIG_DCMOTOR
