Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-02 14:13:42 -04:00
Code Issues Packages Projects Releases Wiki Activity
e8e59014ad
ardupilot/libraries/AP_GPS/AP_GPS.h
Andrew Tridgell f1cbfb3e46 AP_GPS: change handling of moving baseline yaw 
this changes yaw handling in a few ways:

 - GPS yaw now has a timestamp associated with the yaw separate from
   the timestamp associated with the GPS fix

 - we no longer force the primary to change to the UBLOX MB rover when
   it has a GPS yaw. This means we don't change GPS primary due to GPS
   loss, which keeps the GPS more stable. It also increases accuracy
   as the rover is always less accurate in position and velocity than
   the base

 - now we force the primary to be the MB base if the other GPS is a
   rover and the base has GPS lock
2021-07-23 10:19:46 +09:00

705 lines
26 KiB
C++
Raw Blame History

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <inttypes.h>
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_Param/AP_Param.h>
#include "GPS_detect_state.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_MSP/msp.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
/**
maximum number of GPS instances available on this platform. If more
than 1 then redundant sensors may be available
*/
#ifndef GPS_MAX_RECEIVERS
#define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data
#endif
#ifndef GPS_MAX_INSTANCES
#define GPS_MAX_INSTANCES (GPS_MAX_RECEIVERS + 1) // maximum number of GPS instances including the 'virtual' GPS created by blending receiver data
#endif
#if GPS_MAX_INSTANCES > GPS_MAX_RECEIVERS
#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS // the virtual blended GPS is always the highest instance (2)
#endif
#define GPS_UNKNOWN_DOP UINT16_MAX // set unknown DOP's to maximum value, which is also correct for MAVLink
// the number of GPS leap seconds
#define GPS_LEAPSECONDS_MILLIS 18000ULL
#define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * AP_MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS)
#ifndef GPS_MOVING_BASELINE
#define GPS_MOVING_BASELINE !HAL_MINIMIZE_FEATURES && GPS_MAX_RECEIVERS>1
#endif
#ifndef HAL_MSP_GPS_ENABLED
#define HAL_MSP_GPS_ENABLED HAL_MSP_SENSORS_ENABLED
#endif
#if GPS_MOVING_BASELINE
#include "MovingBase.h"
#endif // GPS_MOVING_BASELINE
class AP_GPS_Backend;
/// @class AP_GPS
/// GPS driver main class
class AP_GPS
{
friend class AP_GPS_ERB;
friend class AP_GPS_GSOF;
friend class AP_GPS_MAV;
friend class AP_GPS_MSP;
friend class AP_GPS_ExternalAHRS;
friend class AP_GPS_MTK;
friend class AP_GPS_MTK19;
friend class AP_GPS_NMEA;
friend class AP_GPS_NOVA;
friend class AP_GPS_PX4;
friend class AP_GPS_SBF;
friend class AP_GPS_SBP;
friend class AP_GPS_SBP2;
friend class AP_GPS_SIRF;
friend class AP_GPS_UBLOX;
friend class AP_GPS_Backend;
friend class AP_GPS_UAVCAN;
public:
AP_GPS();
/* Do not allow copies */
AP_GPS(const AP_GPS &other) = delete;
AP_GPS &operator=(const AP_GPS&) = delete;
static AP_GPS *get_singleton() {
return _singleton;
}
// allow threads to lock against GPS update
HAL_Semaphore &get_semaphore(void) {
return rsem;
}
// GPS driver types
enum GPS_Type {
GPS_TYPE_NONE = 0,
GPS_TYPE_AUTO = 1,
GPS_TYPE_UBLOX = 2,
GPS_TYPE_MTK = 3,
GPS_TYPE_MTK19 = 4,
GPS_TYPE_NMEA = 5,
GPS_TYPE_SIRF = 6,
GPS_TYPE_HIL = 7,
GPS_TYPE_SBP = 8,
GPS_TYPE_UAVCAN = 9,
GPS_TYPE_SBF = 10,
GPS_TYPE_GSOF = 11,
GPS_TYPE_ERB = 13,
GPS_TYPE_MAV = 14,
GPS_TYPE_NOVA = 15,
GPS_TYPE_HEMI = 16, // hemisphere NMEA
GPS_TYPE_UBLOX_RTK_BASE = 17,
GPS_TYPE_UBLOX_RTK_ROVER = 18,
GPS_TYPE_MSP = 19,
GPS_TYPE_ALLYSTAR = 20, // AllyStar NMEA
GPS_TYPE_EXTERNAL_AHRS = 21,
};
/// GPS status codes
enum GPS_Status {
NO_GPS = GPS_FIX_TYPE_NO_GPS, ///< No GPS connected/detected
NO_FIX = GPS_FIX_TYPE_NO_FIX, ///< Receiving valid GPS messages but no lock
GPS_OK_FIX_2D = GPS_FIX_TYPE_2D_FIX, ///< Receiving valid messages and 2D lock
GPS_OK_FIX_3D = GPS_FIX_TYPE_3D_FIX, ///< Receiving valid messages and 3D lock
GPS_OK_FIX_3D_DGPS = GPS_FIX_TYPE_DGPS, ///< Receiving valid messages and 3D lock with differential improvements
GPS_OK_FIX_3D_RTK_FLOAT = GPS_FIX_TYPE_RTK_FLOAT, ///< Receiving valid messages and 3D RTK Float
GPS_OK_FIX_3D_RTK_FIXED = GPS_FIX_TYPE_RTK_FIXED, ///< Receiving valid messages and 3D RTK Fixed
};
// GPS navigation engine settings. Not all GPS receivers support
// this
enum GPS_Engine_Setting {
GPS_ENGINE_NONE = -1,
GPS_ENGINE_PORTABLE = 0,
GPS_ENGINE_STATIONARY = 2,
GPS_ENGINE_PEDESTRIAN = 3,
GPS_ENGINE_AUTOMOTIVE = 4,
GPS_ENGINE_SEA = 5,
GPS_ENGINE_AIRBORNE_1G = 6,
GPS_ENGINE_AIRBORNE_2G = 7,
GPS_ENGINE_AIRBORNE_4G = 8
};
// role for auto-config
enum GPS_Role {
GPS_ROLE_NORMAL,
GPS_ROLE_MB_BASE,
GPS_ROLE_MB_ROVER,
};
/*
The GPS_State structure is filled in by the backend driver as it
parses each message from the GPS.
*/
struct GPS_State {
uint8_t instance; // the instance number of this GPS
// all the following fields must all be filled by the backend driver
GPS_Status status; ///< driver fix status
uint32_t time_week_ms; ///< GPS time (milliseconds from start of GPS week)
uint16_t time_week; ///< GPS week number
Location location; ///< last fix location
float ground_speed; ///< ground speed in m/sec
float ground_course; ///< ground course in degrees
float gps_yaw; ///< GPS derived yaw information, if available (degrees)
uint32_t gps_yaw_time_ms; ///< timestamp of last GPS yaw reading
bool gps_yaw_configured; ///< GPS is configured to provide yaw
uint16_t hdop; ///< horizontal dilution of precision in cm
uint16_t vdop; ///< vertical dilution of precision in cm
uint8_t num_sats; ///< Number of visible satellites
Vector3f velocity; ///< 3D velocity in m/s, in NED format
float speed_accuracy; ///< 3D velocity RMS accuracy estimate in m/s
float horizontal_accuracy; ///< horizontal RMS accuracy estimate in m
float vertical_accuracy; ///< vertical RMS accuracy estimate in m
float gps_yaw_accuracy; ///< heading accuracy of the GPS in degrees
bool have_vertical_velocity; ///< does GPS give vertical velocity? Set to true only once available.
bool have_speed_accuracy; ///< does GPS give speed accuracy? Set to true only once available.
bool have_horizontal_accuracy; ///< does GPS give horizontal position accuracy? Set to true only once available.
bool have_vertical_accuracy; ///< does GPS give vertical position accuracy? Set to true only once available.
bool have_gps_yaw; ///< does GPS give yaw? Set to true only once available.
bool have_gps_yaw_accuracy; ///< does the GPS give a heading accuracy estimate? Set to true only once available
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
uint32_t uart_timestamp_ms; ///< optional timestamp from set_uart_timestamp()
uint32_t lagged_sample_count; ///< number of samples with 50ms more lag than expected
// all the following fields must only all be filled by RTK capable backend drivers
uint32_t rtk_time_week_ms; ///< GPS Time of Week of last baseline in milliseconds
uint16_t rtk_week_number; ///< GPS Week Number of last baseline
uint32_t rtk_age_ms; ///< GPS age of last baseline correction in milliseconds (0 when no corrections, 0xFFFFFFFF indicates overflow)
uint8_t rtk_num_sats; ///< Current number of satellites used for RTK calculation
uint8_t rtk_baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED
int32_t rtk_baseline_x_mm; ///< Current baseline in ECEF x or NED north component in mm
int32_t rtk_baseline_y_mm; ///< Current baseline in ECEF y or NED east component in mm
int32_t rtk_baseline_z_mm; ///< Current baseline in ECEF z or NED down component in mm
uint32_t rtk_accuracy; ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999)
int32_t rtk_iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses
};
/// Startup initialisation.
void init(const AP_SerialManager& serial_manager);
/// Update GPS state based on possible bytes received from the module.
/// This routine must be called periodically (typically at 10Hz or
/// more) to process incoming data.
void update(void);
// Pass mavlink data to message handlers (for MAV type)
void handle_msg(const mavlink_message_t &msg);
#if HAL_MSP_GPS_ENABLED
void handle_msp(const MSP::msp_gps_data_message_t &pkt);
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
void handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt);
#endif
// Accessor functions
// return number of active GPS sensors. Note that if the first GPS
// is not present but the 2nd is then we return 2. Note that a blended
// GPS solution is treated as an additional sensor.
uint8_t num_sensors(void) const;
// Return the index of the primary sensor which is the index of the sensor contributing to
// the output. A blended solution is available as an additional instance
uint8_t primary_sensor(void) const {
return primary_instance;
}
/// Query GPS status
GPS_Status status(uint8_t instance) const {
if (_force_disable_gps && state[instance].status > NO_FIX) {
return NO_FIX;
}
return state[instance].status;
}
GPS_Status status(void) const {
return status(primary_instance);
}
// return a single human-presentable character representing the
// fix type. For space-constrained human-readable displays
char status_onechar(void) const {
switch (status()) {
case AP_GPS::NO_GPS:
return ' ';
case AP_GPS::NO_FIX:
return '-';
case AP_GPS::GPS_OK_FIX_2D:
return '2';
case AP_GPS::GPS_OK_FIX_3D:
return '3';
case AP_GPS::GPS_OK_FIX_3D_DGPS:
return '4';
case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT:
return '5';
case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED:
return '6';
}
// should never reach here; compiler flags guarantees this.
return '?';
}
// Query the highest status this GPS supports (always reports GPS_OK_FIX_3D for the blended GPS)
GPS_Status highest_supported_status(uint8_t instance) const;
// location of last fix
const Location &location(uint8_t instance) const {
return state[instance].location;
}
const Location &location() const {
return location(primary_instance);
}
// report speed accuracy
bool speed_accuracy(uint8_t instance, float &sacc) const;
bool speed_accuracy(float &sacc) const {
return speed_accuracy(primary_instance, sacc);
}
bool horizontal_accuracy(uint8_t instance, float &hacc) const;
bool horizontal_accuracy(float &hacc) const {
return horizontal_accuracy(primary_instance, hacc);
}
bool vertical_accuracy(uint8_t instance, float &vacc) const;
bool vertical_accuracy(float &vacc) const {
return vertical_accuracy(primary_instance, vacc);
}
// 3D velocity in NED format
const Vector3f &velocity(uint8_t instance) const {
return state[instance].velocity;
}
const Vector3f &velocity() const {
return velocity(primary_instance);
}
// ground speed in m/s
float ground_speed(uint8_t instance) const {
return state[instance].ground_speed;
}
float ground_speed() const {
return ground_speed(primary_instance);
}
// ground speed in cm/s
uint32_t ground_speed_cm(void) const {
return ground_speed() * 100;
}
// ground course in degrees
float ground_course(uint8_t instance) const {
return state[instance].ground_course;
}
float ground_course() const {
return ground_course(primary_instance);
}
// ground course in centi-degrees
int32_t ground_course_cd(uint8_t instance) const {
return ground_course(instance) * 100;
}
int32_t ground_course_cd() const {
return ground_course_cd(primary_instance);
}
// yaw in degrees if available
bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const;
bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const {
return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg, time_ms);
}
// number of locked satellites
uint8_t num_sats(uint8_t instance) const {
return state[instance].num_sats;
}
uint8_t num_sats() const {
return num_sats(primary_instance);
}
// GPS time of week in milliseconds
uint32_t time_week_ms(uint8_t instance) const {
return state[instance].time_week_ms;
}
uint32_t time_week_ms() const {
return time_week_ms(primary_instance);
}
// GPS week
uint16_t time_week(uint8_t instance) const {
return state[instance].time_week;
}
uint16_t time_week() const {
return time_week(primary_instance);
}
// horizontal dilution of precision
uint16_t get_hdop(uint8_t instance) const {
return state[instance].hdop;
}
uint16_t get_hdop() const {
return get_hdop(primary_instance);
}
// vertical dilution of precision
uint16_t get_vdop(uint8_t instance) const {
return state[instance].vdop;
}
uint16_t get_vdop() const {
return get_vdop(primary_instance);
}
// the time we got our last fix in system milliseconds. This is
// used when calculating how far we might have moved since that fix
uint32_t last_fix_time_ms(uint8_t instance) const {
return timing[instance].last_fix_time_ms;
}
uint32_t last_fix_time_ms(void) const {
return last_fix_time_ms(primary_instance);
}
// the time we last processed a message in milliseconds. This is
// used to indicate that we have new GPS data to process
uint32_t last_message_time_ms(uint8_t instance) const {
return timing[instance].last_message_time_ms;
}
uint32_t last_message_time_ms(void) const {
return last_message_time_ms(primary_instance);
}
// system time delta between the last two reported positions
uint16_t last_message_delta_time_ms(uint8_t instance) const {
return timing[instance].delta_time_ms;
}
uint16_t last_message_delta_time_ms(void) const {
return last_message_delta_time_ms(primary_instance);
}
// return true if the GPS supports vertical velocity values
bool have_vertical_velocity(uint8_t instance) const {
return state[instance].have_vertical_velocity;
}
bool have_vertical_velocity(void) const {
return have_vertical_velocity(primary_instance);
}
// return true if the GPS currently has yaw available
bool have_gps_yaw(uint8_t instance) const {
return !_force_disable_gps_yaw && state[instance].have_gps_yaw;
}
bool have_gps_yaw(void) const {
return have_gps_yaw(primary_instance);
}
// return true if the GPS is configured to provide yaw. This will
// be true if we expect the GPS to provide yaw, even if it
// currently is not able to provide yaw
bool have_gps_yaw_configured(uint8_t instance) const {
return state[instance].gps_yaw_configured;
}
// the expected lag (in seconds) in the position and velocity readings from the gps
// return true if the GPS hardware configuration is known or the lag parameter has been set manually
bool get_lag(uint8_t instance, float &lag_sec) const;
bool get_lag(float &lag_sec) const {
return get_lag(primary_instance, lag_sec);
}
// return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin
const Vector3f &get_antenna_offset(uint8_t instance) const;
// lock out a GPS port, allowing another application to use the port
void lock_port(uint8_t instance, bool locked);
//MAVLink Status Sending
void send_mavlink_gps_raw(mavlink_channel_t chan);
void send_mavlink_gps2_raw(mavlink_channel_t chan);
void send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst);
// Returns true if there is an unconfigured GPS, and provides the instance number of the first non configured GPS
bool first_unconfigured_gps(uint8_t &instance) const WARN_IF_UNUSED;
void broadcast_first_configuration_failure_reason(void) const;
// pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned
bool all_consistent(float &distance) const;
// pre-arm check of GPS blending. False if blending is unhealthy, True if healthy or blending is not being used
bool blend_health_check() const;
// handle sending of initialisation strings to the GPS - only used by backends
void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
void send_blob_update(uint8_t instance);
// return last fix time since the 1/1/1970 in microseconds
uint64_t time_epoch_usec(uint8_t instance) const;
uint64_t time_epoch_usec(void) const {
return time_epoch_usec(primary_instance);
}
// convert GPS week and millis to unix epoch in ms
static uint64_t time_epoch_convert(uint16_t gps_week, uint32_t gps_ms);
static const struct AP_Param::GroupInfo var_info[];
void Write_AP_Logger_Log_Startup_messages();
// indicate which bit in LOG_BITMASK indicates gps logging enabled
void set_log_gps_bit(uint32_t bit) { _log_gps_bit = bit; }
// report if the gps is healthy (this is defined as existing, an update at a rate greater than 4Hz,
// as well as any driver specific behaviour)
bool is_healthy(uint8_t instance) const;
bool is_healthy(void) const { return is_healthy(primary_instance); }
// returns true if all GPS instances have passed all final arming checks/state changes
bool prepare_for_arming(void);
// returns true if all GPS backend drivers haven't seen any failure
// this is for backends to be able to spout pre arm error messages
bool backends_healthy(char failure_msg[], uint16_t failure_msg_len);
// returns false if any GPS drivers are not performing their logging appropriately
bool logging_failed(void) const;
bool logging_present(void) const { return _raw_data != 0; }
bool logging_enabled(void) const { return _raw_data != 0; }
// used to disable GPS for GPS failure testing in flight
void force_disable(bool disable) {
_force_disable_gps = disable;
}
// used to disable GPS yaw for GPS failure testing in flight
void set_force_disable_yaw(bool disable) {
_force_disable_gps_yaw = disable;
}
// handle possibly fragmented RTCM injection data
void handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len);
// get configured type by instance
GPS_Type get_type(uint8_t instance) const {
return instance>=GPS_MAX_RECEIVERS? GPS_Type::GPS_TYPE_NONE : GPS_Type(_type[instance].get());
}
// get iTOW, if supported, zero otherwie
uint32_t get_itow(uint8_t instance) const;
bool get_error_codes(uint8_t instance, uint32_t &error_codes) const;
bool get_error_codes(uint32_t &error_codes) const { return get_error_codes(primary_instance, error_codes); }
enum class SBAS_Mode : int8_t {
Disabled = 0,
Enabled = 1,
DoNotChange = 2,
};
protected:
// configuration parameters
AP_Int8 _type[GPS_MAX_RECEIVERS];
AP_Int8 _navfilter;
AP_Int8 _auto_switch;
AP_Int8 _min_dgps;
AP_Int16 _sbp_logmask;
AP_Int8 _inject_to;
uint32_t _last_instance_swap_ms;
AP_Enum<SBAS_Mode> _sbas_mode;
AP_Int8 _min_elevation;
AP_Int8 _raw_data;
AP_Int8 _gnss_mode[GPS_MAX_RECEIVERS];
AP_Int16 _rate_ms[GPS_MAX_RECEIVERS]; // this parameter should always be accessed using get_rate_ms()
AP_Int8 _save_config;
AP_Int8 _auto_config;
AP_Vector3f _antenna_offset[GPS_MAX_RECEIVERS];
AP_Int16 _delay_ms[GPS_MAX_RECEIVERS];
AP_Int8 _com_port[GPS_MAX_RECEIVERS];
AP_Int8 _blend_mask;
AP_Float _blend_tc;
AP_Int16 _driver_options;
AP_Int8 _primary;
#if GPS_MAX_RECEIVERS > 1 && HAL_ENABLE_LIBUAVCAN_DRIVERS
AP_Int32 _node_id[GPS_MAX_RECEIVERS];
AP_Int32 _override_node_id[GPS_MAX_RECEIVERS];
#endif
#if GPS_MOVING_BASELINE
MovingBase mb_params[GPS_MAX_RECEIVERS];
#endif // GPS_MOVING_BASELINE
uint32_t _log_gps_bit = -1;
private:
static AP_GPS *_singleton;
HAL_Semaphore rsem;
// returns the desired gps update rate in milliseconds
// this does not provide any guarantee that the GPS is updating at the requested
// rate it is simply a helper for use in the backends for determining what rate
// they should be configuring the GPS to run at
uint16_t get_rate_ms(uint8_t instance) const;
struct GPS_timing {
// the time we got our last fix in system milliseconds
uint32_t last_fix_time_ms;
// the time we got our last message in system milliseconds
uint32_t last_message_time_ms;
// delta time between the last pair of GPS updates in system milliseconds
uint16_t delta_time_ms;
// count of delayed frames
uint8_t delayed_count;
// the average time delta
float average_delta_ms;
};
// Note allowance for an additional instance to contain blended data
GPS_timing timing[GPS_MAX_INSTANCES];
GPS_State state[GPS_MAX_INSTANCES];
AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS];
AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS];
/// primary GPS instance
uint8_t primary_instance;
/// number of GPS instances present
uint8_t num_instances;
// which ports are locked
uint8_t locked_ports;
// state of auto-detection process, per instance
struct detect_state {
uint32_t last_baud_change_ms;
uint8_t current_baud;
bool auto_detected_baud;
struct UBLOX_detect_state ublox_detect_state;
struct MTK_detect_state mtk_detect_state;
struct MTK19_detect_state mtk19_detect_state;
struct SIRF_detect_state sirf_detect_state;
struct NMEA_detect_state nmea_detect_state;
struct SBP_detect_state sbp_detect_state;
struct SBP2_detect_state sbp2_detect_state;
struct ERB_detect_state erb_detect_state;
} detect_state[GPS_MAX_RECEIVERS];
struct {
const char *blob;
uint16_t remaining;
} initblob_state[GPS_MAX_RECEIVERS];
static const uint32_t _baudrates[];
static const char _initialisation_blob[];
static const char _initialisation_raw_blob[];
void detect_instance(uint8_t instance);
void update_instance(uint8_t instance);
/*
buffer for re-assembling RTCM data for GPS injection.
The 8 bit flags field in GPS_RTCM_DATA is interpreted as:
1 bit for "is fragmented"
2 bits for fragment number
5 bits for sequence number
The rtcm_buffer is allocated on first use. Once a block of data
is successfully reassembled it is injected into all active GPS
backends. This assumes we don't want more than 4*180=720 bytes
in a RTCM data block
*/
struct rtcm_buffer {
uint8_t fragments_received;
uint8_t sequence;
uint8_t fragment_count;
uint16_t total_length;
uint8_t buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4];
} *rtcm_buffer;
// re-assemble GPS_RTCM_DATA message
void handle_gps_rtcm_data(const mavlink_message_t &msg);
void handle_gps_inject(const mavlink_message_t &msg);
//Inject a packet of raw binary to a GPS
void inject_data(const uint8_t *data, uint16_t len);
void inject_data(uint8_t instance, const uint8_t *data, uint16_t len);
// GPS blending and switching
Vector3f _blended_antenna_offset; // blended antenna offset
float _blended_lag_sec; // blended receiver lag in seconds
float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
float _omega_lpf; // cutoff frequency in rad/sec of LPF applied to position offsets
bool _output_is_blended; // true when a blended GPS solution being output
uint8_t _blend_health_counter; // 0 = perfectly health, 100 = very unhealthy
// calculate the blend weight. Returns true if blend could be calculated, false if not
bool calc_blend_weights(void);
// calculate the blended state
void calc_blended_state(void);
bool should_log() const;
bool needs_uart(GPS_Type type) const;
/// Update primary instance
void update_primary(void);
// helper function for mavlink gps yaw
uint16_t gps_yaw_cdeg(uint8_t instance) const;
// Auto configure types
enum GPS_AUTO_CONFIG {
GPS_AUTO_CONFIG_DISABLE = 0,
GPS_AUTO_CONFIG_ENABLE = 1
};
enum class GPSAutoSwitch {
NONE = 0,
USE_BEST = 1,
BLEND = 2,
//USE_SECOND = 3, deprecated for new primary param
USE_PRIMARY_IF_3D_FIX = 4,
};
// used for flight testing with GPS loss
bool _force_disable_gps;
// used for flight testing with GPS yaw loss
bool _force_disable_gps_yaw;
// logging support
void Write_GPS(uint8_t instance);
};
namespace AP {
AP_GPS &gps();
};
Reference in New Issue View Git Blame Copy Permalink