Merge pull request #1939 from Katawann/pr-battery-status

Add battery status message for v1

src/integration_tests/telemetry_async.cpp

@@ -341,7 +341,10 @@ void print_gps_info(Telemetry::GpsInfo gps_info)
 void print_battery(Telemetry::Battery battery)
 {
     std::cout << "Battery " << battery.id << ": " << battery.voltage_v << " v,"
-              << "remaining: " << int(battery.remaining_percent * 1e2f) << " %" << '\n';
+              << "temperature: " << battery.temperature_degc << " degC,"
+              << "current: " << battery.current_battery_a << " A,"
+              << "capacity: " << battery.capacity_consumed_ah << " Ah,"
+              << "remaining: " << int(battery.remaining_percent) << " %" << '\n';
 
     _received_battery = true;
 }

src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h

@@ -373,9 +373,16 @@ class Telemetry : public PluginBase {
      */
     struct Battery {
         uint32_t id{0}; /**< @brief Battery ID, for systems with multiple batteries */
+        float temperature_degc{float(NAN)}; /**< @brief Temperature of the battery in degrees
+                                               Celsius. NAN for unknown temperature */
         float voltage_v{float(NAN)}; /**< @brief Voltage in volts */
+        float current_battery_a{float(NAN)}; /**< @brief Battery current in Amps, NAN if autopilot
+                                                does not measure the current */
+        float capacity_consumed_ah{
+            float(NAN)}; /**< @brief Consumed charge in Amp hours, NAN if autopilot does not provide
+                            consumption estimate */
         float remaining_percent{
-            float(NAN)}; /**< @brief Estimated battery remaining (range: 0.0 to 1.0) */
+            float(NAN)}; /**< @brief Estimated battery remaining (range: 0 to 100) */
     };
 
     /**

src/mavsdk/plugins/telemetry/telemetry.cpp

@@ -999,8 +999,14 @@ std::ostream& operator<<(std::ostream& str, Telemetry::RawGps const& raw_gps)
 bool operator==(const Telemetry::Battery& lhs, const Telemetry::Battery& rhs)
 {
     return (rhs.id == lhs.id) &&
+           ((std::isnan(rhs.temperature_degc) && std::isnan(lhs.temperature_degc)) ||
+            rhs.temperature_degc == lhs.temperature_degc) &&
            ((std::isnan(rhs.voltage_v) && std::isnan(lhs.voltage_v)) ||
             rhs.voltage_v == lhs.voltage_v) &&
+           ((std::isnan(rhs.current_battery_a) && std::isnan(lhs.current_battery_a)) ||
+            rhs.current_battery_a == lhs.current_battery_a) &&
+           ((std::isnan(rhs.capacity_consumed_ah) && std::isnan(lhs.capacity_consumed_ah)) ||
+            rhs.capacity_consumed_ah == lhs.capacity_consumed_ah) &&
            ((std::isnan(rhs.remaining_percent) && std::isnan(lhs.remaining_percent)) ||
             rhs.remaining_percent == lhs.remaining_percent);
 }
@@ -1010,7 +1016,10 @@ std::ostream& operator<<(std::ostream& str, Telemetry::Battery const& battery)
     str << std::setprecision(15);
     str << "battery:" << '\n' << "{\n";
     str << "    id: " << battery.id << '\n';
+    str << "    temperature_degc: " << battery.temperature_degc << '\n';
     str << "    voltage_v: " << battery.voltage_v << '\n';
+    str << "    current_battery_a: " << battery.current_battery_a << '\n';
+    str << "    capacity_consumed_ah: " << battery.capacity_consumed_ah << '\n';
     str << "    remaining_percent: " << battery.remaining_percent << '\n';
     str << '}';
     return str;

src/mavsdk/plugins/telemetry/telemetry_impl.cpp

@@ -1121,8 +1121,7 @@ void TelemetryImpl::process_sys_status(const mavlink_message_t& message)
     if (!_has_bat_status) {
         Telemetry::Battery new_battery;
         new_battery.voltage_v = sys_status.voltage_battery * 1e-3f;
-        // FIXME: it is strange calling it percent when the range goes from 0 to 1.
-        new_battery.remaining_percent = sys_status.battery_remaining * 1e-2f;
+        new_battery.remaining_percent = sys_status.battery_remaining;
 
         set_battery(new_battery);
 
@@ -1213,14 +1212,22 @@ void TelemetryImpl::process_battery_status(const mavlink_message_t& message)
 
     Telemetry::Battery new_battery;
     new_battery.id = bat_status.id;
+    new_battery.temperature_degc = (bat_status.temperature == std::numeric_limits<int16_t>::max()) ?
+                                       static_cast<float>(NAN) :
+                                       bat_status.temperature * 1e-2f; // cdegC to degC
     new_battery.voltage_v = 0.0f;
     for (int i = 0; i < 255; i++) {
         if (bat_status.voltages[i] == std::numeric_limits<uint16_t>::max())
             break;
         new_battery.voltage_v += static_cast<float>(bat_status.voltages[i]) * 1e-3f;
     }
-    // FIXME: it is strange calling it percent when the range goes from 0 to 1.
-    new_battery.remaining_percent = bat_status.battery_remaining * 1e-2f;
+    new_battery.remaining_percent = bat_status.battery_remaining;
+    new_battery.current_battery_a = (bat_status.current_battery == -1) ?
+                                        static_cast<float>(NAN) :
+                                        bat_status.current_battery * 1e-2f; // cA to A
+    new_battery.capacity_consumed_ah = (bat_status.current_consumed == -1) ?
+                                           static_cast<float>(NAN) :
+                                           bat_status.current_consumed * 1e-3f; // mAh to Ah
 
     set_battery(new_battery);