From 0881ccbee0c6153c4132675ec5c9d2087dcc4a0f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 16 Aug 2014 15:27:03 +0900 Subject: [PATCH] GCS_MAVLink: send radio_in using rcmap --- libraries/GCS_MAVLink/GCS.h | 3 ++- libraries/GCS_MAVLink/GCS_Common.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 3652c20431..6a20cf11b2 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -13,6 +13,7 @@ #include #include #include "../AP_BattMonitor/AP_BattMonitor.h" +#include #include // GCS Message ID's @@ -192,7 +193,7 @@ public: void send_ahrs2(AP_AHRS &ahrs); bool send_gps_raw(AP_GPS &gps); void send_system_time(AP_GPS &gps); - void send_radio_in(uint8_t receiver_rssi); + void send_radio_in(const RCMapper &rcmap, uint8_t receiver_rssi); void send_raw_imu(const AP_InertialSensor &ins, const Compass &compass); void send_scaled_pressure(AP_Baro &barometer); void send_sensor_offsets(const AP_InertialSensor &ins, const Compass &compass, AP_Baro &barometer); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 45464586b0..67bb8487a7 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -981,7 +981,7 @@ void GCS_MAVLINK::send_system_time(AP_GPS &gps) /* send RC_CHANNELS_RAW, and RC_CHANNELS messages */ -void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi) +void GCS_MAVLINK::send_radio_in(const RCMapper &rcmap, uint8_t receiver_rssi) { uint32_t now = hal.scheduler->millis(); @@ -993,10 +993,10 @@ void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi) chan, now, 0, // port - values[0], - values[1], - values[2], - values[3], + values[rcmap.roll()-1], + values[rcmap.pitch()-1], + values[rcmap.throttle()-1], + values[rcmap.yaw()-1], values[4], values[5], values[6],