From 039ade421ef46c2e4b893f4de38595c119b5ad50 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 18 Dec 2018 13:06:40 +1100 Subject: [PATCH] GCS_MAVLink: split SYS_STATUS and POWER_STATUS onto separate ap_messages --- libraries/GCS_MAVLink/GCS.h | 5 ++++- libraries/GCS_MAVLink/GCS_Common.cpp | 12 +++++++++++- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 652dd6fea0..10c58ed1c5 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -41,7 +41,8 @@ enum ap_message : uint8_t { MSG_HEARTBEAT, MSG_ATTITUDE, MSG_LOCATION, - MSG_EXTENDED_STATUS1, + MSG_SYS_STATUS, + MSG_POWER_STATUS, MSG_MEMINFO, MSG_NAV_CONTROLLER_OUTPUT, MSG_CURRENT_WAYPOINT, @@ -407,6 +408,8 @@ protected: void send_hwstatus(); void handle_data_packet(mavlink_message_t *msg); + virtual bool vehicle_initialised() const { return true; } + // these two methods are called after current_loc is updated: virtual int32_t global_position_int_alt() const; virtual int32_t global_position_int_relative_alt() const; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e8d370de31..7bfb54c16b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -194,6 +194,10 @@ void GCS_MAVLINK::send_meminfo(void) // report power supply status void GCS_MAVLINK::send_power_status(void) { + if (!vehicle_initialised()) { + // avoid unnecessary errors being reported to user + return; + } mavlink_msg_power_status_send(chan, hal.analogin->board_voltage() * 1000, hal.analogin->servorail_voltage() * 1000, @@ -860,7 +864,8 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_HEARTBEAT, MSG_HEARTBEAT}, { MAVLINK_MSG_ID_ATTITUDE, MSG_ATTITUDE}, { MAVLINK_MSG_ID_GLOBAL_POSITION_INT, MSG_LOCATION}, - { MAVLINK_MSG_ID_SYS_STATUS, MSG_EXTENDED_STATUS1}, + { MAVLINK_MSG_ID_SYS_STATUS, MSG_SYS_STATUS}, + { MAVLINK_MSG_ID_POWER_STATUS, MSG_POWER_STATUS}, { MAVLINK_MSG_ID_MEMINFO, MSG_MEMINFO}, { MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, MSG_NAV_CONTROLLER_OUTPUT}, { MAVLINK_MSG_ID_MISSION_CURRENT, MSG_CURRENT_WAYPOINT}, @@ -3761,6 +3766,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_position_target_global_int(); break; + case MSG_POWER_STATUS: + CHECK_PAYLOAD_SIZE(POWER_STATUS); + send_power_status(); + break; + case MSG_RADIO_IN: CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); send_radio_in();