Skip to content

Commit

Permalink
GCS_MAVLink: correct compilation when compass not enabled
Browse files Browse the repository at this point in the history
  • Loading branch information
peterbarker committed Dec 13, 2023
1 parent fc9e6de commit 71a64d5
Showing 1 changed file with 11 additions and 5 deletions.
16 changes: 11 additions & 5 deletions libraries/GCS_MAVLink/GCS_Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include "GCS.h"

#include <AC_Fence/AC_Fence.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_ADSB/AP_ADSB.h>
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
#include <AP_AHRS/AP_AHRS.h>
Expand All @@ -38,6 +39,7 @@
#include <AP_Gripper/AP_Gripper.h>
#include <AC_Sprayer/AC_Sprayer.h>
#include <AP_BLHeli/AP_BLHeli.h>
#include <AP_Relay/AP_Relay.h>
#include <AP_RSSI/AP_RSSI.h>
#include <AP_RTC/AP_RTC.h>
#include <AP_Scheduler/AP_Scheduler.h>
Expand Down Expand Up @@ -68,6 +70,8 @@
#include "MissionItemProtocol_Rally.h"
#include "MissionItemProtocol_Fence.h"

#include <AP_Notify/AP_Notify.h>

#include <stdio.h>

#if HAL_RCINPUT_WITH_AP_RADIO
Expand Down Expand Up @@ -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,
Expand All @@ -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;
Expand All @@ -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;
}
Expand Down

0 comments on commit 71a64d5

Please sign in to comment.