From 71a64d50465df90148d37383572c4e8a6c045ccd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Dec 2023 16:39:09 +1100 Subject: [PATCH] GCS_MAVLink: correct compilation when compass not enabled --- libraries/GCS_MAVLink/GCS_Common.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a81f3d5a38..377c083041 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -22,6 +22,7 @@ #include "GCS.h" #include +#include #include #include #include @@ -38,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -68,6 +70,8 @@ #include "MissionItemProtocol_Rally.h" #include "MissionItemProtocol_Fence.h" +#include + #include #if HAL_RCINPUT_WITH_AP_RADIO @@ -2031,16 +2035,16 @@ void GCS_MAVLINK::send_raw_imu() { #if AP_INERTIALSENSOR_ENABLED 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; +#if AP_COMPASS_ENABLED + const Compass &compass = AP::compass(); if (compass.get_count() >= 1) { mag = compass.get_field(0); - } else { - mag.zero(); } +#endif mavlink_msg_raw_imu_send( chan, @@ -2063,7 +2067,6 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan { #if AP_INERTIALSENSOR_ENABLED const AP_InertialSensor &ins = AP::ins(); - const Compass &compass = AP::compass(); int16_t _temperature = 0; bool have_data = false; @@ -2078,11 +2081,14 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan gyro = ins.get_gyro(instance); have_data = true; } - Vector3f mag{}; + Vector3f mag; +#if AP_COMPASS_ENABLED + const Compass &compass = AP::compass(); if (compass.get_count() > instance) { mag = compass.get_field(instance); have_data = true; } +#endif if (!have_data) { return; }