From dc4ad26e9666b028594d7cb3d71ed8a682ef6f31 Mon Sep 17 00:00:00 2001 From: Do Carmo Lucas Date: Wed, 2 Oct 2019 20:57:50 +0100 Subject: [PATCH] GCS_MAVLink: updates to support the latest stable mavlink changes (IMU temperature extension) --- libraries/GCS_MAVLink/GCS.h | 2 +- libraries/GCS_MAVLink/GCS_Common.cpp | 9 ++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 1ac6da6691..e196a1ccb8 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -210,7 +210,7 @@ public: virtual void send_position_target_local_ned() { }; void send_servo_output_raw(); void send_accelcal_vehicle_position(uint32_t position); - void send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)); + void send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature)); void send_sys_status(); void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc); void send_rpm() const; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index b13e302d14..e0be540b02 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1601,10 +1601,12 @@ void GCS_MAVLINK::send_raw_imu() gyro.z * 1000.0f, mag.x, mag.y, - mag.z); + mag.z, + 0, // for now, only instance 0 is sent + 0); // TODO: get the IMU temperature } -void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)) +void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature)) { const AP_InertialSensor &ins = AP::ins(); const Compass &compass = AP::compass(); @@ -1639,7 +1641,8 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan gyro.z * 1000.0f, mag.x, mag.y, - mag.z); + mag.z, + 0); // TODO: get the IMU temperature }