From 2998176c20704d3862a227bbb90c75b92d1cdb54 Mon Sep 17 00:00:00 2001
From: Gus Grubba <gus@auterion.com>
Date: Sat, 27 Jul 2019 13:56:33 -0400
Subject: [PATCH] Make Mock Link send battery info

---
 src/comm/MockLink.cc | 24 ++++++++++++++++++++++++
 src/comm/MockLink.h  |  4 ++++
 2 files changed, 28 insertions(+)

diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc
index f75e8ade4..c5dfffd89 100644
--- a/src/comm/MockLink.cc
+++ b/src/comm/MockLink.cc
@@ -104,6 +104,7 @@ MockLink::MockLink(SharedLinkConfigurationPointer& config)
 
     _adsbVehicleCoordinate = QGeoCoordinate(_vehicleLatitude, _vehicleLongitude).atDistanceAndAzimuth(1000, _adsbAngle);
     _adsbVehicleCoordinate.setAltitude(100);
+    _runningTime.start();
 }
 
 MockLink::~MockLink(void)
@@ -176,6 +177,7 @@ void MockLink::_run1HzTasks(void)
             _sendHighLatency2();
         } else {
             _sendVibration();
+            _sendSysStatus();
             _sendADSBVehicles();
             if (!qgcApp()->runningUnitTests()) {
                 // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
@@ -357,6 +359,28 @@ void MockLink::_sendHighLatency2(void)
     respondWithMavlinkMessage(msg);
 }
 
+void MockLink::_sendSysStatus(void)
+{
+    if(_batteryRemaining > 50) {
+        _batteryRemaining = static_cast<int8_t>(100 - (_runningTime.elapsed() / 1000));
+    }
+    mavlink_message_t   msg;
+    mavlink_msg_sys_status_pack_chan(
+        _vehicleSystemId,
+        _vehicleComponentId,
+        static_cast<uint8_t>(_mavlinkChannel),
+        &msg,
+        0,          // onboard_control_sensors_present
+        0,          // onboard_control_sensors_enabled
+        0,          // onboard_control_sensors_health
+        250,        // load
+        4200 * 4,   // voltage_battery
+        8000,       // current_battery
+        _batteryRemaining, // battery_remaining
+        0,0,0,0,0,0);
+    respondWithMavlinkMessage(msg);
+}
+
 void MockLink::_sendVibration(void)
 {
     mavlink_message_t   msg;
diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h
index 0d3bee323..a8186866d 100644
--- a/src/comm/MockLink.h
+++ b/src/comm/MockLink.h
@@ -196,6 +196,7 @@ private:
     void _sendHomePosition(void);
     void _sendGpsRawInt(void);
     void _sendVibration(void);
+    void _sendSysStatus(void);
     void _sendStatusTextMessages(void);
     void _respondWithAutopilotVersion(void);
     void _sendRCChannels(void);
@@ -226,6 +227,9 @@ private:
     uint32_t    _mavCustomMode;
     uint8_t     _mavState;
 
+    QTime       _runningTime;
+    int8_t      _batteryRemaining = 100;
+
     MAV_AUTOPILOT       _firmwareType;
     MAV_TYPE            _vehicleType;
     double              _vehicleLatitude;
-- 
2.22.0