Tracker: add support for SYS_STATUS sending, battery

AntennaTracker/AntennaTracker.cpp

@@ -37,6 +37,7 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
     SCHED_TASK(update_tracking,        50,   1000),
     SCHED_TASK(update_GPS,             10,   4000),
     SCHED_TASK(update_compass,         10,   1500),
+    SCHED_TASK(update_battery,         10,   1500),
     SCHED_TASK(update_barometer,       10,   1500),
     SCHED_TASK(gcs_update,             50,   1700),
     SCHED_TASK(gcs_data_stream_send,   50,   3000),

AntennaTracker/GCS_Mavlink.cpp

@@ -78,6 +78,30 @@ void Tracker::send_attitude(mavlink_channel_t chan)
 }
 
 
+void Tracker::send_extended_status1(mavlink_channel_t chan)
+{
+    int16_t battery_current = -1;
+    int8_t battery_remaining = -1;
+
+    if (battery.has_current() && battery.healthy()) {
+        battery_remaining = battery.capacity_remaining_pct();
+        battery_current = battery.current_amps() * 100;
+    }
+
+    mavlink_msg_sys_status_send(
+        chan,
+        0,
+        0,
+        0,
+        static_cast<uint16_t>(scheduler.load_average() * 1000),
+        battery.voltage() * 1000,  // mV
+        battery_current,        // in 10mA units
+        battery_remaining,      // in %
+        0,  // comm drops %,
+        0,  // comm drops in pkts,
+        0, 0, 0, 0);
+}
+
 void Tracker::send_location(mavlink_channel_t chan)
 {
     uint32_t fix_time;
@@ -201,6 +225,11 @@ bool GCS_MAVLINK_Tracker::try_send_message(enum ap_message id)
         tracker.send_simstate(chan);
         break;
 
+    case MSG_EXTENDED_STATUS1:
+        CHECK_PAYLOAD_SIZE(SYS_STATUS);
+        tracker.send_extended_status1(chan);
+        break;
+
     default:
         return GCS_MAVLINK::try_send_message(id);
     }

AntennaTracker/Tracker.h

@@ -150,6 +150,9 @@ private:
     AP_BoardConfig_CAN BoardConfig_CAN = AP_BoardConfig_CAN::create();
 #endif
 
+    // Battery Sensors
+    AP_BattMonitor battery = AP_BattMonitor::create();
+
     struct Location current_loc;
 
     enum ControlMode control_mode  = INITIALISING;
@@ -201,6 +204,7 @@ private:
     void ten_hz_logging_loop();
     void send_heartbeat(mavlink_channel_t chan);
     void send_attitude(mavlink_channel_t chan);
+    void send_extended_status1(mavlink_channel_t chan);
     void send_location(mavlink_channel_t chan);
     void send_nav_controller_output(mavlink_channel_t chan);
     void send_simstate(mavlink_channel_t chan);
@@ -222,6 +226,7 @@ private:
     void update_barometer(void);
     void update_ahrs();
     void update_compass(void);
+    void update_battery(void);
     void compass_accumulate(void);
     void accel_cal_update(void);
     void barometer_accumulate(void);

AntennaTracker/sensors.cpp

@@ -46,6 +46,11 @@ void Tracker::update_compass(void)
     }
 }
 
+void Tracker::update_battery()
+{
+    battery.read();
+}
+
 /*
   if the compass is enabled then try to accumulate a reading
  */