From f8a05564e889fb892f0a3e41ea0faf72bb81631e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 18 Dec 2018 13:08:11 +1100 Subject: [PATCH] Copter: split SYS_STATUS and POWER_STATUS onto separate ap_messages --- ArduCopter/GCS_Mavlink.cpp | 22 ++++++++++++---------- ArduCopter/GCS_Mavlink.h | 2 ++ 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index b6e14ec28e..806f029bfe 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -254,6 +254,10 @@ uint32_t GCS_MAVLINK_Copter::telem_delay() const return (uint32_t)(copter.g.telem_delay); } +bool GCS_MAVLINK_Copter::vehicle_initialised() const { + return copter.ap.initialised; +} + // try to send a message, return false if it wasn't sent bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) { @@ -277,17 +281,14 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) switch(id) { - case MSG_EXTENDED_STATUS1: + case MSG_SYS_STATUS: // send extended status only once vehicle has been initialised // to avoid unnecessary errors being reported to user - if (copter.ap.initialised) { - if (PAYLOAD_SIZE(chan, SYS_STATUS) + - PAYLOAD_SIZE(chan, POWER_STATUS) > comm_get_txspace(chan)) { - return false; - } - copter.send_sys_status(chan); - send_power_status(); + if (!vehicle_initialised()) { + return true; } + CHECK_PAYLOAD_SIZE(SYS_STATUS); + copter.send_sys_status(chan); break; case MSG_NAV_CONTROLLER_OUTPUT: @@ -445,7 +446,8 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = { MSG_SENSOR_OFFSETS }; static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { - MSG_EXTENDED_STATUS1, // SYS_STATUS, POWER_STATUS + MSG_SYS_STATUS, + MSG_POWER_STATUS, MSG_MEMINFO, MSG_CURRENT_WAYPOINT, // MISSION_CURRENT MSG_GPS_RAW, @@ -1443,7 +1445,7 @@ void Copter::mavlink_delay_cb() if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs_send_heartbeat(); - gcs().send_message(MSG_EXTENDED_STATUS1); + gcs().send_message(MSG_SYS_STATUS); } if (tnow - last_50hz > 20) { last_50hz = tnow; diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 4dfe139fa6..c7c181598c 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -47,6 +47,8 @@ private: void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; bool try_send_message(enum ap_message id) override; + bool vehicle_initialised() const override; + void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override;