altos: Report battery voltage instead of S at startup

[fw/altos] / src / kernel / ao_report.c

diff --git a/src/kernel/ao_report.c b/src/kernel/ao_report.c
index 1104cd82532ff8173334591ee06c534b968e1f42..1a0e9e166f959a576378c7eb82ced20e159e4319 100644 (file)
--- a/src/kernel/ao_report.c
+++ b/src/kernel/ao_report.c
@@ -87,19 +87,18 @@ ao_report_digit(uint8_t digit) __reentrant
 }
 
 static void
-ao_report_altitude(void)
+ao_report_number(int16_t n)
 {
-       __pdata int16_t agl = ao_max_height;
        __xdata uint8_t digits[10];
        __pdata uint8_t ndigits, i;
 
-       if (agl < 0)
-               agl = 0;
+       if (n < 0)
+               n = 0;
        ndigits = 0;
        do {
-               digits[ndigits++] = agl % 10;
-               agl /= 10;
-       } while (agl);
+               digits[ndigits++] = n % 10;
+               n /= 10;
+       } while (n);
 
        i = ndigits;
        do
@@ -107,6 +106,27 @@ ao_report_altitude(void)
        while (i != 0);
 }
 
+static void
+ao_report_altitude(void)
+{
+       ao_report_number(ao_max_height);
+}
+
+#if HAS_BATTERY_REPORT
+static void
+ao_report_battery(void)
+{
+       __xdata struct ao_data packet;
+       for (;;) {
+               ao_data_get(&packet);
+               if (packet.adc.v_batt != 0)
+                       break;
+               ao_sleep(DATA_TO_XDATA(&ao_sample_data));
+       }
+       ao_report_number(ao_battery_decivolt(packet.adc.v_batt));
+}
+#endif
+
 #if HAS_IGNITE_REPORT
 static uint8_t
 ao_report_igniter_ready(enum ao_igniter igniter)
@@ -163,7 +183,12 @@ ao_report(void)
 {
        ao_report_state = ao_flight_state;
        for(;;) {
-               ao_report_beep();
+#if HAS_BATTERY_REPORT
+               if (ao_flight_state == ao_flight_startup)
+                       ao_report_battery();
+               else
+#endif
+                       ao_report_beep();
                if (ao_flight_state == ao_flight_landed) {
                        ao_report_altitude();
 #if HAS_FLIGHT