From d8871078ffdded97036c65c092b3d064c0283f56 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 21 May 2018 22:34:22 +1000 Subject: [PATCH] GCS_MAVLink: create persist_streamrates() callback to indicate persistence Removes the catching of the message in each vehicle separately. --- libraries/GCS_MAVLink/GCS.h | 3 ++- libraries/GCS_MAVLink/GCS_Common.cpp | 4 ++++ libraries/GCS_MAVLink/GCS_Param.cpp | 6 +++--- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 4c897b4d6e..e87dd34b68 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -278,7 +278,8 @@ protected: // saveable rate of each stream AP_Int16 streamRates[NUM_STREAMS]; - void handle_request_data_stream(mavlink_message_t *msg, bool save); + virtual bool persist_streamrates() const { return false; } + void handle_request_data_stream(mavlink_message_t *msg); virtual void handle_command_ack(const mavlink_message_t* msg); void handle_set_mode(mavlink_message_t* msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 6ef9f131c0..ba6abae538 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2349,6 +2349,10 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg) handle_common_rally_message(msg); break; + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + handle_request_data_stream(msg); + break; + case MAVLINK_MSG_ID_DATA96: handle_data_packet(msg); break; diff --git a/libraries/GCS_MAVLink/GCS_Param.cpp b/libraries/GCS_MAVLink/GCS_Param.cpp index b54103d26a..884b3c0311 100644 --- a/libraries/GCS_MAVLink/GCS_Param.cpp +++ b/libraries/GCS_MAVLink/GCS_Param.cpp @@ -124,7 +124,7 @@ bool GCS_MAVLINK::have_flow_control(void) save==false so we don't want the save to happen when the user connects the ground station. */ -void GCS_MAVLINK::handle_request_data_stream(mavlink_message_t *msg, bool save) +void GCS_MAVLINK::handle_request_data_stream(mavlink_message_t *msg) { mavlink_request_data_stream_t packet; mavlink_msg_request_data_stream_decode(msg, &packet); @@ -143,7 +143,7 @@ void GCS_MAVLINK::handle_request_data_stream(mavlink_message_t *msg, bool save) case MAV_DATA_STREAM_ALL: // note that we don't set STREAM_PARAMS - that is internal only for (uint8_t i=0; iset_and_save_ifchanged(freq); } else { rate->set(freq);