diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 6ba8077d08..c80aa1f3e1 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -296,7 +296,7 @@ private: void handle_mission_clear_all(AP_Mission &mission, mavlink_message_t *msg); void handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg); - void handle_request_data_stream(mavlink_message_t *msg); + void handle_request_data_stream(mavlink_message_t *msg, bool save); void handle_param_request_list(mavlink_message_t *msg); void handle_param_request_read(mavlink_message_t *msg); void handle_param_set(mavlink_message_t *msg, DataFlash_Class &DataFlash); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 03985f4508..043c2994ee 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -388,7 +388,13 @@ bool GCS_MAVLINK::have_flow_control(void) } -void GCS_MAVLINK::handle_request_data_stream(mavlink_message_t *msg) +/* + handle a request to change stream rate. Note that copter passes in + save==false, as sending mavlink messages on copter on APM2 costs + enough that it can cause flight issues, 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) { mavlink_request_data_stream_t packet; mavlink_msg_request_data_stream_decode(msg, &packet); @@ -405,38 +411,51 @@ void GCS_MAVLINK::handle_request_data_stream(mavlink_message_t *msg) else return; + AP_Int16 *rate = NULL; switch (packet.req_stream_id) { 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); + } + } } void GCS_MAVLINK::handle_param_request_list(mavlink_message_t *msg)