diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index e68e6aad67..b453a03a13 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -162,8 +162,9 @@ public: void send_heartbeat(void) const; void send_meminfo(void); void send_power_status(void); - void send_battery_status(const AP_BattMonitor &battery, const uint8_t instance) const; - bool send_battery_status(const AP_BattMonitor &battery) const; + void send_battery_status(const AP_BattMonitor &battery, + const uint8_t instance) const; + bool send_battery_status() const; void send_distance_sensor(const AP_RangeFinder_Backend *sensor) const; bool send_distance_sensor(const RangeFinder &rangefinder) const; void send_distance_sensor_downward(const RangeFinder &rangefinder) const; @@ -176,7 +177,7 @@ public: void send_scaled_pressure(); void send_sensor_offsets(const AP_InertialSensor &ins, const Compass &compass); void send_ahrs(); - void send_battery2(const AP_BattMonitor &battery); + void send_battery2(); #if AP_AHRS_NAVEKF_AVAILABLE void send_opticalflow(const OpticalFlow &optflow); #endif diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 8b90195326..9d3d064c88 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -188,7 +188,8 @@ void GCS_MAVLINK::send_power_status(void) hal.analogin->power_status_flags()); } -void GCS_MAVLINK::send_battery_status(const AP_BattMonitor &battery, const uint8_t instance) const +void GCS_MAVLINK::send_battery_status(const AP_BattMonitor &battery, + const uint8_t instance) const { // catch the battery backend not supporting the required number of cells static_assert(sizeof(AP_BattMonitor::cells) >= (sizeof(uint16_t) * MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN), @@ -209,8 +210,10 @@ void GCS_MAVLINK::send_battery_status(const AP_BattMonitor &battery, const uint8 } // returns true if all battery instances were reported -bool GCS_MAVLINK::send_battery_status(const AP_BattMonitor &battery) const +bool GCS_MAVLINK::send_battery_status() const { + const AP_BattMonitor &battery = AP::battery(); + for(uint8_t i = 0; i < battery.num_instances(); i++) { CHECK_PAYLOAD_SIZE(BATTERY_STATUS); send_battery_status(battery, i); @@ -1322,8 +1325,10 @@ void GCS::setup_uarts(AP_SerialManager &serial_manager) } // report battery2 state -void GCS_MAVLINK::send_battery2(const AP_BattMonitor &battery) +void GCS_MAVLINK::send_battery2() { + const AP_BattMonitor &battery = AP::battery(); + if (battery.num_instances() > 1) { int16_t current; if (battery.has_current(1)) { @@ -2654,6 +2659,15 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) ret = try_send_compass_message(id); break; + case MSG_BATTERY_STATUS: + send_battery_status(); + break; + + case MSG_BATTERY2: + CHECK_PAYLOAD_SIZE(BATTERY2); + send_battery2(); + break; + case MSG_EXTENDED_STATUS2: CHECK_PAYLOAD_SIZE(MEMINFO); send_meminfo();