From b76fa57108489d266e03edc73bcfa2b9658b5ca2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 2 May 2018 20:18:08 +1000 Subject: [PATCH] GCS_MAVLink: move try_send_message handling of RC_CHANNELS_RAW up --- libraries/GCS_MAVLink/GCS.h | 2 +- libraries/GCS_MAVLink/GCS_Common.cpp | 13 ++++++++++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 215658a0da..479a04a8ea 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -162,7 +162,7 @@ public: bool send_proximity(const AP_Proximity &proximity) const; void send_ahrs2(); void send_system_time(); - void send_radio_in(uint8_t receiver_rssi); + void send_radio_in(); void send_raw_imu(const AP_InertialSensor &ins, const Compass &compass); void send_scaled_pressure(); void send_sensor_offsets(const AP_InertialSensor &ins, const Compass &compass); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index df922a3e3e..8ada9f3877 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -951,8 +951,14 @@ void GCS_MAVLINK::send_system_time() /* send RC_CHANNELS messages */ -void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi) +void GCS_MAVLINK::send_radio_in() { + AP_RSSI *rssi = AP::rssi(); + uint8_t receiver_rssi = 0; + if (rssi != nullptr) { + receiver_rssi = rssi->read_receiver_rssi_uint8(); + } + uint32_t now = AP_HAL::millis(); mavlink_status_t *status = mavlink_get_channel_status(chan); @@ -2674,6 +2680,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_local_position(); break; + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + send_radio_in(); + break; + case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); send_ahrs();