From b51ec1d346e0bc190143fd5018e765b01ea5ee43 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 3 May 2018 11:31:47 +1000 Subject: [PATCH] GCS_MAVLink: move try_send_message of RAW_IMU up to GCS_MAVLINK --- libraries/GCS_MAVLink/GCS.h | 2 +- libraries/GCS_MAVLink/GCS_Common.cpp | 10 +++++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index a692b59453..cf50b37f09 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -172,7 +172,7 @@ public: void send_ahrs2(); void send_system_time(); void send_radio_in(); - void send_raw_imu(const AP_InertialSensor &ins, const Compass &compass); + void send_raw_imu(); void send_scaled_pressure(); void send_sensor_offsets(const AP_InertialSensor &ins, const Compass &compass); void send_ahrs(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index c220c96dca..9dbebbd01d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1030,8 +1030,11 @@ void GCS_MAVLINK::send_radio_in() receiver_rssi); } -void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &compass) +void GCS_MAVLINK::send_raw_imu() { + const AP_InertialSensor &ins = AP::ins(); + const Compass &compass = AP::compass(); + const Vector3f &accel = ins.get_accel(0); const Vector3f &gyro = ins.get_gyro(0); Vector3f mag; @@ -2765,6 +2768,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_radio_in(); break; + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + send_raw_imu(); + break; + case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); send_ahrs();