Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_GPS: avoid use of mavlink_channel_t when GCS not compiled in #24499

Merged
merged 2 commits into from Aug 8, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 4 additions & 0 deletions libraries/AP_GPS/AP_GPS.cpp
Expand Up @@ -1294,6 +1294,7 @@ void AP_GPS::update_primary(void)
}
#endif // GPS_MAX_RECEIVERS > 1

#if HAL_GCS_ENABLED
void AP_GPS::handle_gps_inject(const mavlink_message_t &msg)
{
mavlink_gps_inject_data_t packet;
Expand Down Expand Up @@ -1331,6 +1332,7 @@ void AP_GPS::handle_msg(const mavlink_message_t &msg)
}
}
}
#endif

#if HAL_MSP_GPS_ENABLED
void AP_GPS::handle_msp(const MSP::msp_gps_data_message_t &pkt)
Expand Down Expand Up @@ -1496,6 +1498,7 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
}
#endif // GPS_MAX_RECEIVERS

#if HAL_GCS_ENABLED
void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst)
{
if (inst >= GPS_MAX_RECEIVERS) {
Expand All @@ -1505,6 +1508,7 @@ void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst)
drivers[inst]->send_mavlink_gps_rtk(chan);
}
}
#endif

bool AP_GPS::first_unconfigured_gps(uint8_t &instance) const
{
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_GPS/AP_GPS_ERB.h
Expand Up @@ -34,7 +34,9 @@ class AP_GPS_ERB : public AP_GPS_Backend

AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }

#if HAL_GCS_ENABLED
bool supports_mavlink_gps_rtk_message() const override { return true; }
#endif

static bool _detect(struct ERB_detect_state &state, uint8_t data);

Expand Down
7 changes: 5 additions & 2 deletions libraries/AP_GPS/AP_GPS_MAV.cpp
Expand Up @@ -16,11 +16,14 @@
//
// MAVLINK GPS driver
//
#include "AP_GPS_MAV.h"
#include <stdint.h>

#include "AP_GPS_config.h"

#if AP_GPS_MAV_ENABLED

#include "AP_GPS_MAV.h"
#include <stdint.h>

// Reading does nothing in this class; we simply return whether or not
// the latest reading has been consumed. By calling this function we assume
// the caller is consuming the new data;
Expand Down
12 changes: 6 additions & 6 deletions libraries/AP_GPS/AP_GPS_MAV.h
Expand Up @@ -19,16 +19,15 @@
//
#pragma once

#include "AP_GPS_config.h"

#if AP_GPS_MAV_ENABLED

#include <AP_HAL/AP_HAL_Boards.h>

#include "AP_GPS.h"
#include "GPS_Backend.h"

#ifndef AP_GPS_MAV_ENABLED
#define AP_GPS_MAV_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED
#endif

#if AP_GPS_MAV_ENABLED
class AP_GPS_MAV : public AP_GPS_Backend {
public:

Expand All @@ -47,4 +46,5 @@ class AP_GPS_MAV : public AP_GPS_Backend {
uint32_t first_week;
JitterCorrection jitter{2000};
};
#endif

#endif // AP_GPS_MAV_ENABLED
2 changes: 2 additions & 0 deletions libraries/AP_GPS/AP_GPS_SBF.h
Expand Up @@ -45,7 +45,9 @@ class AP_GPS_SBF : public AP_GPS_Backend

void broadcast_configuration_failure_reason(void) const override;

#if HAL_GCS_ENABLED
bool supports_mavlink_gps_rtk_message(void) const override { return true; };
#endif

// get the velocity lag, returns true if the driver is confident in the returned value
bool get_lag(float &lag_sec) const override { lag_sec = 0.08f; return true; } ;
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_GPS/AP_GPS_SBP.h
Expand Up @@ -32,7 +32,9 @@ class AP_GPS_SBP : public AP_GPS_Backend

AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }

#if HAL_GCS_ENABLED
bool supports_mavlink_gps_rtk_message() const override { return true; }
#endif

// Methods
bool read() override;
Expand Down
5 changes: 5 additions & 0 deletions libraries/AP_GPS/AP_GPS_config.h
@@ -1,6 +1,7 @@
#pragma once

#include <AP_HAL/AP_HAL_Boards.h>
#include <GCS_MAVLink/GCS_config.h>

#ifndef AP_GPS_ENABLED
#define AP_GPS_ENABLED 1
Expand All @@ -22,6 +23,10 @@
#define AP_GPS_GSOF_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED
#endif

#ifndef AP_GPS_MAV_ENABLED
#define AP_GPS_MAV_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED && HAL_GCS_ENABLED
#endif

#ifndef AP_GPS_NMEA_ENABLED
#define AP_GPS_NMEA_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED
#endif
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_GPS/GPS_Backend.cpp
Expand Up @@ -197,9 +197,9 @@ bool AP_GPS_Backend::should_log() const
}


#if HAL_GCS_ENABLED
void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
{
#if HAL_GCS_ENABLED
const uint8_t instance = state.instance;
// send status
switch (instance) {
Expand Down Expand Up @@ -236,8 +236,8 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
state.rtk_iar_num_hypotheses);
break;
}
#endif
}
#endif


/*
Expand Down
5 changes: 4 additions & 1 deletion libraries/AP_GPS/GPS_Backend.h
Expand Up @@ -19,6 +19,7 @@
#pragma once

#include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS_config.h>
#include <AP_RTC/JitterCorrection.h>
#include "AP_GPS.h"
#include "AP_GPS_config.h"
Expand Down Expand Up @@ -55,13 +56,15 @@ class AP_GPS_Backend

virtual void inject_data(const uint8_t *data, uint16_t len);

#if HAL_GCS_ENABLED
//MAVLink methods
virtual bool supports_mavlink_gps_rtk_message() const { return false; }
virtual void send_mavlink_gps_rtk(mavlink_channel_t chan);
virtual void handle_msg(const mavlink_message_t &msg) { return ; }
#endif

virtual void broadcast_configuration_failure_reason(void) const { return ; }

virtual void handle_msg(const mavlink_message_t &msg) { return ; }
#if HAL_MSP_GPS_ENABLED
virtual void handle_msp(const MSP::msp_gps_data_message_t &pkt) { return; }
#endif
Expand Down