Display battery data from the drone's telemetry, when WITH_MAVLINK

main/disp_oled.cpp

@@ -446,7 +446,12 @@ static uint8_t BattCapacity(uint16_t mVolt)
   return (mVolt-3600+2)/5; }
 
 void OLED_DrawBattery(u8g2_t *OLED, GPS_Position *GPS) // draw battery status page
-{ uint8_t Cap=BattCapacity(BatteryVoltage>>8);         // est. battery capacity based on the voltage readout
+{ 
+#ifdef WITH_MAVLINK
+  uint8_t Cap=MAVLINK_BattCap;                         // [%] from the drone's telemetry
+#else
+  uint8_t Cap=BattCapacity(BatteryVoltage>>8);         // [%] est. battery capacity based on the voltage readout
+#endif
   // u8g2_SetFont(OLED, u8g2_font_battery19_tn);
   // u8g2_DrawGlyph(OLED, 120, 60, '0'+(Cap+10)/20);
 
@@ -466,11 +471,20 @@ void OLED_DrawBattery(u8g2_t *OLED, GPS_Position *GPS) // draw battery status pa
   u8g2_DrawBox  (OLED,  8, 23,  4,  8);                // and the battery tip
 
   strcpy(Line, " .   V");
+#ifdef WITH_MAVLINK
+  Format_UnsDec(Line, MAVLINK_BattVolt, 4, 3);
+#else
   Format_UnsDec(Line, BatteryVoltage>>8, 4, 3);        // print the battery voltage readout
+#endif
   u8g2_DrawStr(OLED, 0, 48, Line);
 
+#ifdef WITH_MAVLINK
+  strcpy(Line, "  .  A");
+  Format_UnsDec(Line, MAVLINK_BattCurr, 4, 2);
+#else
   strcpy(Line, "   . mV/min ");                        // print the battery voltage rate
   Format_SignDec(Line, (600*BatteryVoltageRate+128)>>8, 3, 1);
+#endif
   u8g2_DrawStr(OLED, 0, 60, Line);
 
 #ifdef WITH_BQ
@@ -505,7 +519,11 @@ void OLED_DrawBattery(u8g2_t *OLED, GPS_Position *GPS) // draw battery status pa
 
 void OLED_DrawStatusBar(u8g2_t *OLED, GPS_Position *GPS)
 { static bool Odd=0;
-  uint8_t Cap = BattCapacity(BatteryVoltage>>8);           // est. battery capacity
+#ifdef WITH_MAVLINK
+  uint8_t Cap = MAVLINK_BattCap;                           // [%]
+#else
+  uint8_t Cap = BattCapacity(BatteryVoltage>>8);           // [%] est. battery capacity
+#endif
   uint8_t BattLev = (Cap+10)/20;                           // [0..5] convert to display scale
   uint8_t Charging = 0;                                    // charging or not changing ?
 #ifdef WITH_BQ

main/gps.cpp

@@ -88,10 +88,16 @@ uint32_t GPS_nextBaudRate(void) { BaudRateIdx++; if(BaudRateIdx>=BaudRates) Baud
 const uint32_t GPS_TargetBaudRate = 57600; // BaudRate[4]; // [bps] must be one of the baud rates known by the autbaud
 // const uint8_t  GPS_TargetDynModel =      7; // for UBX GPS's: 6 = airborne with >1g, 7 = with >2g
 
-static char GPS_Cmd[64];
+#ifdef WITH_MAVLINK
+uint16_t MAVLINK_BattVolt = 0;   // [mV]
+uint16_t MAVLINK_BattCurr = 0;   // [10mA]
+uint8_t  MAVLINK_BattCap  = 0;   // [%]
+#endif
 
 // ----------------------------------------------------------------------------
 
+static char GPS_Cmd[64];
+
 static uint16_t SatSNRsum = 0;
 static uint8_t  SatSNRcount = 0;
 
@@ -789,6 +795,9 @@ static void GPS_MAV(void)                                                  // wh
 #endif
   } else if(MsgID==MAV_ID_SYS_STATUS)
   { const MAV_SYS_STATUS *Status = (const MAV_SYS_STATUS *)MAV.getPayload();
+    MAVLINK_BattVolt = Status->battery_voltage;   // [mV]
+    MAVLINK_BattCurr = Status->battery_current;   // [10mA]
+    MAVLINK_BattCap  = Status->battery_remaining; // [%]
 #ifdef DEBUG_PRINT
     xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
     Format_String(CONS_UART_Write, "MAV_SYS_STATUS: ");

main/gps.h

@@ -50,6 +50,12 @@ GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, in
 
 int16_t GPS_AverageSpeed(void);             // [0.1m/s] calc. average speed based on most recent GPS positions
 
+#ifdef WITH_MAVLINK
+extern uint16_t MAVLINK_BattVolt;   // [mV]
+extern uint16_t MAVLINK_BattCurr;   // [10mA]
+extern uint8_t  MAVLINK_BattCap;    // [%]
+#endif
+
 #ifdef __cplusplus
   extern "C"
 #endif