AP_GPS of ArduPilot open source code
1. Origin
The GPS module here cannot be understood as global positioning in a narrow sense. From a broad perspective, it is a flight control position processing module.
In other words, as long as the three-dimensional coordinate position is provided through the position interface protocol, the flight controller can apply internal algorithms to perform a series of position-related flight operations such as waypoint cruise and positioning.
Therefore, understanding this module is very important for flight control applications. Next, analyze and study the location processing module.
2. AP_GPS 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_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_DroneCAN;
public:
AP_GPS();
/* Do not allow copies */
CLASS_NO_COPY(AP_GPS);
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, // driver removed
// GPS_TYPE_MTK19 = 4, // driver removed
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_TYPE_UAVCAN_RTK_BASE = 22,
GPS_TYPE_UAVCAN_RTK_ROVER = 23,
GPS_TYPE_UNICORE_NMEA = 24,
GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25,
#if HAL_SIM_GPS_ENABLED
GPS_TYPE_SITL = 100,
#endif
};
/// GPS status codes. These are kept aligned with MAVLink by
/// static_assert in AP_GPS.cpp
enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock
GPS_OK_FIX_2D = 2, ///< Receiving valid messages and 2D lock
GPS_OK_FIX_3D = 3, ///< Receiving valid messages and 3D lock
GPS_OK_FIX_3D_DGPS = 4, ///< Receiving valid messages and 3D lock with differential improvements
GPS_OK_FIX_3D_RTK_FLOAT = 5, ///< Receiving valid messages and 3D RTK Float
GPS_OK_FIX_3D_RTK_FIXED = 6, ///< 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,
};
// GPS Covariance Types matching ROS2 sensor_msgs/msg/NavSatFix
enum class CovarianceType : uint8_t {
UNKNOWN = 0, ///< The GPS does not support any accuracy metrics
APPROXIMATED = 1, ///< The accuracy is approximated through metrics such as HDOP/VDOP
DIAGONAL_KNOWN = 2, ///< The diagonal (east, north, up) components of covariance are reported by the GPS
KNOWN = 3, ///< The full covariance array is reported by the GPS
};
/*
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/s
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
float undulation; //<height that WGS84 is above AMSL at the current location
bool have_undulation; ///<do we have a value for the undulation
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
uint64_t last_corrected_gps_time_us;///< the system time we got the last corrected GPS timestamp, microseconds
bool corrected_timestamp_updated; ///< true if the corrected timestamp has been updated
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
// UBX Relative Position and Heading message information
float relPosHeading; ///< Reported Heading in degrees
float relPosLength; ///< Reported Position horizontal distance in meters
float relPosD; ///< Reported Vertical distance in meters
float accHeading; ///< Reported Heading Accuracy in degrees
uint32_t relposheading_ts; ///< True if new data has been received since last time it was false
};
/// Startup initialisation.
void init(const class 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 WARN_IF_UNUSED;
// location of last fix
const Location &location(uint8_t instance) const {
return state[instance].location;
}
const Location &location() const {
return location(primary_instance);
}
// get the difference between WGS84 and AMSL. A positive value means
// the AMSL height is higher than WGS84 ellipsoid height
bool get_undulation(uint8_t instance, float &undulation) const;
// get the difference between WGS84 and AMSL. A positive value means
// the AMSL height is higher than WGS84 ellipsoid height
bool get_undulation(float &undulation) const {
return get_undulation(primary_instance, undulation);
}
// 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);
}
CovarianceType position_covariance(const uint8_t instance, Matrix3f& cov) const WARN_IF_UNUSED;
// 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);
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);
}
uint64_t last_message_epoch_usec(uint8_t instance) const;
uint64_t last_message_epoch_usec() const {
return last_message_epoch_usec(primary_instance);
}
// convert GPS week and millis to unix epoch in ms
static uint64_t istate_time_to_epoch_ms(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,
};
#if GPS_MOVING_BASELINE
// methods used by UAVCAN GPS driver and AP_Periph for moving baseline
void inject_MBL_data(uint8_t* data, uint16_t length);
void get_RelPosHeading(uint32_t ×tamp, float &relPosHeading, float &relPosLength, float &relPosD, float &accHeading);
bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len);
void clear_RTCMV3();
#endif // GPS_MOVING_BASELINE
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 HAL_ENABLE_DRONECAN_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;
enum DriverOptions : int16_t {
UBX_MBUseUart2 = (1U << 0U),
SBF_UseBaseForYaw = (1U << 1U),
UBX_Use115200 = (1U << 2U),
UAVCAN_MBUseDedicatedBus = (1 << 3U),
HeightEllipsoid = (1U << 4),
};
// check if an option is set
bool option_set(const DriverOptions option) const {
return (uint8_t(_driver_options.get()) & uint8_t(option)) != 0;
}
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 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);
// run detection step for one GPS instance. If this finds a GPS then it
// will return it - otherwise nullptr
AP_GPS_Backend *_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;
struct {
uint16_t fragments_used;
uint16_t fragments_discarded;
} rtcm_stats;
// 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;
#if GPS_MAX_RECEIVERS > 1
/// Update primary instance
void update_primary(void);
#endif
// 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_SERIAL_ONLY = 1,
GPS_AUTO_CONFIG_ENABLE_ALL = 2,
};
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);
}
2.1 GPS_Type
There are many types of GPS supported, usually we often hear that it should be NMEA/UBLOX/MSP, in addition ArduPilot also supports the following types:
// GPS driver types
enum GPS_Type {
GPS_TYPE_NONE = 0,
GPS_TYPE_AUTO = 1,
GPS_TYPE_UBLOX = 2,
// GPS_TYPE_MTK = 3, // driver removed
// GPS_TYPE_MTK19 = 4, // driver removed
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_TYPE_UAVCAN_RTK_BASE = 22,
GPS_TYPE_UAVCAN_RTK_ROVER = 23,
GPS_TYPE_UNICORE_NMEA = 24,
GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25,
#if HAL_SIM_GPS_ENABLED
GPS_TYPE_SITL = 100,
#endif
};
2.2 GPS_Status
GPS status is very important for high-precision positioning. Usually, what we see is GPS_OK_FIX_3Dthat when the number of our satellites is more than 20, higher precision will appear GPS_OK_FIX_3D_DGPS.
If you are lucky enough to use the RTK GPS module, you will get the cm-level accuracy of RTK, which is very important for precise positioning. After all, the meter-level accuracy is very dangerous for cruising in low-altitude complex areas.
/// GPS status codes. These are kept aligned with MAVLink by
/// static_assert in AP_GPS.cpp
enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock
GPS_OK_FIX_2D = 2, ///< Receiving valid messages and 2D lock
GPS_OK_FIX_3D = 3, ///< Receiving valid messages and 3D lock
GPS_OK_FIX_3D_DGPS = 4, ///< Receiving valid messages and 3D lock with differential improvements
GPS_OK_FIX_3D_RTK_FLOAT = 5, ///< Receiving valid messages and 3D RTK Float
GPS_OK_FIX_3D_RTK_FIXED = 6, ///< Receiving valid messages and 3D RTK Fixed
};
3. AP_GPS class method
3.1 AP_GPS class initialization
The initialization of the application class is always handled by an init function. For the startup process, please refer to: Introduction to ArduPilot flight control startup & operation process .
AP_Vehicle::setup
└──> init_ardupilot
└──> AP_GPS::init
Initialize data fusion and state.
AP_GPS::init
├──> <(_auto_switch.get() == 3) && !_primary.configured()> // Set new primary param based on old auto_switch use second option
│ ├──> _primary.set_and_save(1)
│ └──> _auto_switch.set_and_save(0)
├──> [search for serial ports with gps protocol]
├──> _omega_lpf = 1.0f / constrain_float(_blend_tc, 5.0f, 30.0f) // Initialise class variables used to do GPS blending
├──> [prep the state instance fields]
└──> [sanity check update rate]
3.2 AP_GPS task
SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200, 9),
Regularly update GPS information at a frequency of 50Hz (usually GPS is at a frequency of 5Hz or 10Hz).
AP_GPS::update
├──> update_instance //将GPS实例进行逐一更新
├──> [calculate number of instances] num_instances
├──> <HAL_LOGGING_ENABLED> <primary_instance != old_primary>
│ └──> AP::logger().Write_Event(LogEvent::GPS_PRIMARY_CHANGED)
└──> <HAL_BUILD_AP_PERIPH> // update notify with gps status. We always base this on the primary_instance
├──> AP_Notify::flags.gps_status = state[primary_instance].status;
└──> AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;
3.3 AP_GPS dynamic maintenance
GPS instance maintenance (initialization, data update, status monitoring).
AP_GPS::update_instance
├──> <_type[instance] == GPS_TYPE_HIL> // in HIL, leave info alone
│ └──> return
├──> <_type[instance] == GPS_TYPE_NONE> // not enabled
│ ├──> state[instance].status = NO_GPS;
│ ├──> state[instance].hdop = GPS_UNKNOWN_DOP;
│ └──> state[instance].vdop = GPS_UNKNOWN_DOP;
├──> <locked_ports & (1U<<instance)> // the port is locked by another driver
│ └──> return
├──> <drivers[instance] == nullptr> // we don't yet know the GPS type of this one, or it has timed out and needs to be re-initialised
│ ├──> detect_instance(instance) // GPS实例如果在总线上发现，则进行实例初始化
│ └──> return
├──> <_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY>
│ └──> send_blob_update(instance)
│
│ // we have an active driver for this instance
├──> bool result = drivers[instance]->read()
├──> uint32_t tnow = AP_HAL::millis()
├──> [if we did not get a message, and the idle timer of 4 seconds has expired, re-initialise the GPS. This will cause GPS detection to run again]
│ ├──> memset((void *)&state[instance], 0, sizeof(state[instance]));
│ ├──> state[instance].instance = instance
│ ├──> state[instance].hdop = GPS_UNKNOWN_DOP
│ ├──> state[instance].vdop = GPS_UNKNOWN_DOP
│ ├──> timing[instance].last_message_time_ms = tnow
│ ├──> timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
│ ├──> <GPS_TYPE_MAV/GPS_TYPE_UAVCAN/GPS_TYPE_UAVCAN_RTK_BASE/GPS_TYPE_UAVCAN_RTK_ROVER> // do not try to detect again if type is MAV or UAVCAN
│ │ └──> state[instance].status = NO_FIX
│ ├──> <GPS_TYPE others> // free the driver before we run the next detection, so we don't end up with two allocated at any time
│ │ ├──> delete drivers[instance]
│ │ ├──> drivers[instance] = nullptr
│ │ └──> state[instance].status = NO_GPS
│ └──> data_should_be_logged = true
├──> [Get GPS data]
│ ├──> <state[instance].corrected_timestamp_updated>
│ │ // set the timestamp for this messages based on
│ │ // set_uart_timestamp() or per specific transport in backend
│ │ // , if available
│ │ ├──> tnow = state[instance].last_corrected_gps_time_us/1000U;
│ │ └──> state[instance].corrected_timestamp_updated = false
│ ├──> [delta will only be correct after parsing two messages]
│ │ ├──> timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms;
│ │ └──> timing[instance].last_message_time_ms = tnow
│ ├──> <state[instance].status >= GPS_OK_FIX_2D && !_force_disable_gps> // if GPS disabled for flight testing then don't update fix timing value
│ │ └──> timing[instance].last_fix_time_ms = tnow
│ └──> data_should_be_logged = true
└──> <GPS_MAX_RECEIVERS > 1> <drivers[instance] && _type[instance] == GPS_TYPE_UBLOX_RTK_BASE)>
└──> <drivers[instance]->get_RTCMV3(rtcm_data, rtcm_len)>
└──> for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
└──> <i != instance && _type[i] == GPS_TYPE_UBLOX_RTK_ROVER> // pass the data to the rover
├──> inject_data(i, rtcm_data, rtcm_len);
├──> drivers[instance]->clear_RTCMV3();
└──> break;
3.4 GPS instance initialization
The GPS instance runs the detection step. If GPS is found, a [instance] will be new and the [instance] status will be changed from NO_GPS to NO_FIX.
AP_GPS::detect_instance
├──> const uint32_t now = AP_HAL::millis()
├──> state[instance].status = NO_GPS
├──> state[instance].hdop = GPS_UNKNOWN_DOP
├──> state[instance].vdop = GPS_UNKNOWN_DOP
├──> AP_GPS_Backend *new_gps = _detect_instance(instance)
├──> <new_gps == nullptr> return
├──> state[instance].status = NO_FIX
├──> drivers[instance] = new_gps
├──> timing[instance].last_message_time_ms = now
├──> timing[instance].delta_time_ms = GPS_TIMEOUT_MS
└──> new_gps->broadcast_gps_type()
3.5 GPS instance discovery
The GPS instance runs the detection, if this found a GPS then it will return an instance, otherwise nullptr.
AP_GPS::_detect_instance
├──> <GPS_TYPE_MAV>
│ ├──> dstate->auto_detected_baud = false; // specified, not detected
│ └──> return new AP_GPS_MAV(*this, state[instance], nullptr);
├──> <GPS_TYPE_UAVCAN> <GPS_TYPE_UAVCAN_RTK_BASE> <GPS_TYPE_UAVCAN_RTK_ROVER>
│ ├──> dstate->auto_detected_baud = false; // specified, not detected
│ └──> return AP_GPS_DroneCAN::probe(*this, state[instance]);
├──> <GPS_TYPE_MSP>
│ ├──> dstate->auto_detected_baud = false; // specified, not detected
│ └──> return new AP_GPS_MSP(*this, state[instance], nullptr);
├──> <GPS_TYPE_EXTERNAL_AHRS>
│ ├──> dstate->auto_detected_baud = false; // specified, not detected
│ └──> return new AP_GPS_ExternalAHRS(*this, state[instance], nullptr);
├──> <_port[instance] == nullptr> return nullptr; // UART not available，以下GPS实例初始化必须依赖串行口
│
│ // all remaining drivers automatically cycle through baud rates to detect
│ // the correct baud rate, and should have the selected baud broadcast
├──> dstate->auto_detected_baud = true;
├──> const uint32_t now = AP_HAL::millis();
├──> [auto try baud rates] // {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U, 460800U}
├──> <_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY>
│ └──> send_blob_update(instance);
├──> <GPS_TYPE_SBF>
│ └──> return new AP_GPS_SBF(*this, state[instance], _port[instance]);
├──> <GPS_TYPE_GSOF>
│ └──> return new AP_GPS_GSOF(*this, state[instance], _port[instance]);
├──> <GPS_TYPE_NOVA>
│ └──> return new AP_GPS_NOVA(*this, state[instance], _port[instance]);
├──> <GPS_TYPE_SITL>
│ └──> return new AP_GPS_SITL(*this, state[instance], _port[instance]);
├──> <initblob_state[instance].remaining != 0> return nullptr;
│
│ // byte serial data detect
├──> uint16_t bytecount = MIN(8192U, _port[instance]->available());
├──> <GPS_TYPE_UBLOX>
│ ├──> AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
│ └──> return new AP_GPS_UBLOX(*this, state[instance], _port[instance], GPS_ROLE_NORMAL);
├──> <GPS_TYPE_UBLOX_RTK_BASE><GPS_TYPE_UBLOX_RTK_ROVER>
│ ├──> AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
│ └──> return new AP_GPS_UBLOX(*this, state[instance], _port[instance], role);
├──> <GPS_TYPE_SBP>
│ ├──> AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) {
│ │ └──> return new AP_GPS_SBP2(*this, state[instance], _port[instance]);
│ ├──> AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
│ └──> return new AP_GPS_SBP(*this, state[instance], _port[instance]);
├──> <GPS_TYPE_SIRF>
│ ├──> AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
│ └──> return new AP_GPS_SIRF(*this, state[instance], _port[instance]);
├──> <GPS_TYPE_ERB>
│ ├──> AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) {
│ └──> return new AP_GPS_ERB(*this, state[instance], _port[instance]);
├──> <GPS_TYPE_NMEA><GPS_TYPE_HEMI><GPS_TYPE_UNICORE_NMEA><GPS_TYPE_UNICORE_MOVINGBASE_NMEA><GPS_TYPE_ALLYSTAR>
│ ├──> AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
│ └──> return new AP_GPS_NMEA(*this, state[instance], _port[instance]);
└──> return nullptr;
4. Example understanding
In view of the fact that the commonly used GPS module on the traversing machine adopts the UBLOX protocol, here is an example to explain it.
class AP_GPS_UBLOX : public AP_GPS_Backend
{
public:
AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role role);
~AP_GPS_UBLOX() override;
// Methods
bool read() override;
AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }
static bool _detect(struct UBLOX_detect_state &state, uint8_t data);
bool is_configured(void) const override {
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
if (!gps._auto_config) {
return true;
} else {
return !_unconfigured_messages;
}
#else
return true;
#endif // CONFIG_HAL_BOARD != HAL_BOARD_SITL
}
bool get_error_codes(uint32_t &error_codes) const override {
error_codes = _unconfigured_messages;
return true;
};
void broadcast_configuration_failure_reason(void) const override;
void Write_AP_Logger_Log_Startup_messages() const override;
// get the velocity lag, returns true if the driver is confident in the returned value
bool get_lag(float &lag_sec) const override;
const char *name() const override { return "u-blox"; }
// support for retrieving RTCMv3 data from a moving baseline base
bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len) override;
void clear_RTCMV3(void) override;
// ublox specific healthy checks
bool is_healthy(void) const override;
private:
// u-blox UBX protocol essentials
struct PACKED ubx_header {
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
};
#if UBLOX_GNSS_SETTINGS
struct PACKED ubx_cfg_gnss {
uint8_t msgVer;
uint8_t numTrkChHw;
uint8_t numTrkChUse;
uint8_t numConfigBlocks;
PACKED struct configBlock {
uint8_t gnssId;
uint8_t resTrkCh;
uint8_t maxTrkCh;
uint8_t reserved1;
uint32_t flags;
} configBlock[UBLOX_MAX_GNSS_CONFIG_BLOCKS];
};
#endif
struct PACKED ubx_cfg_nav_rate {
uint16_t measure_rate_ms;
uint16_t nav_rate;
uint16_t timeref;
};
struct PACKED ubx_cfg_msg {
uint8_t msg_class;
uint8_t msg_id;
};
struct PACKED ubx_cfg_msg_rate {
uint8_t msg_class;
uint8_t msg_id;
uint8_t rate;
};
struct PACKED ubx_cfg_msg_rate_6 {
uint8_t msg_class;
uint8_t msg_id;
uint8_t rates[6];
};
struct PACKED ubx_cfg_nav_settings {
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t res1;
uint32_t res2;
uint32_t res3;
uint32_t res4;
};
struct PACKED ubx_cfg_tp5 {
uint8_t tpIdx;
uint8_t version;
uint8_t reserved1[2];
int16_t antCableDelay;
int16_t rfGroupDelay;
uint32_t freqPeriod;
uint32_t freqPeriodLock;
uint32_t pulseLenRatio;
uint32_t pulseLenRatioLock;
int32_t userConfigDelay;
uint32_t flags;
};
struct PACKED ubx_cfg_prt {
uint8_t portID;
};
struct PACKED ubx_cfg_sbas {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
};
// F9 config keys
enum class ConfigKey : uint32_t {
TMODE_MODE = 0x20030001,
CFG_RATE_MEAS = 0x30210001,
CFG_UART1_BAUDRATE = 0x40520001,
CFG_UART1_ENABLED = 0x10520005,
CFG_UART1INPROT_UBX = 0x10730001,
CFG_UART1INPROT_NMEA = 0x10730002,
CFG_UART1INPROT_RTCM3X = 0x10730004,
CFG_UART1OUTPROT_UBX = 0x10740001,
CFG_UART1OUTPROT_NMEA = 0x10740002,
CFG_UART1OUTPROT_RTCM3X = 0x10740004,
CFG_UART2_BAUDRATE = 0x40530001,
CFG_UART2_ENABLED = 0x10530005,
CFG_UART2INPROT_UBX = 0x10750001,
CFG_UART2INPROT_NMEA = 0x10750002,
CFG_UART2INPROT_RTCM3X = 0x10750004,
CFG_UART2OUTPROT_UBX = 0x10760001,
CFG_UART2OUTPROT_NMEA = 0x10760002,
CFG_UART2OUTPROT_RTCM3X = 0x10760004,
MSGOUT_RTCM_3X_TYPE4072_0_UART1 = 0x209102ff,
MSGOUT_RTCM_3X_TYPE4072_1_UART1 = 0x20910382,
MSGOUT_RTCM_3X_TYPE1077_UART1 = 0x209102cd,
MSGOUT_RTCM_3X_TYPE1087_UART1 = 0x209102d2,
MSGOUT_RTCM_3X_TYPE1097_UART1 = 0x20910319,
MSGOUT_RTCM_3X_TYPE1127_UART1 = 0x209102d7,
MSGOUT_RTCM_3X_TYPE1230_UART1 = 0x20910304,
MSGOUT_UBX_NAV_RELPOSNED_UART1 = 0x2091008e,
MSGOUT_RTCM_3X_TYPE4072_0_UART2 = 0x20910300,
MSGOUT_RTCM_3X_TYPE4072_1_UART2 = 0x20910383,
MSGOUT_RTCM_3X_TYPE1077_UART2 = 0x209102ce,
MSGOUT_RTCM_3X_TYPE1087_UART2 = 0x209102d3,
MSGOUT_RTCM_3X_TYPE1097_UART2 = 0x2091031a,
MSGOUT_RTCM_3X_TYPE1127_UART2 = 0x209102d8,
MSGOUT_RTCM_3X_TYPE1230_UART2 = 0x20910305,
MSGOUT_UBX_NAV_RELPOSNED_UART2 = 0x2091008f,
// enable specific signals and constellations
CFG_SIGNAL_GPS_ENA = 0x1031001f,
CFG_SIGNAL_GPS_L1CA_ENA = 0x10310001,
CFG_SIGNAL_GPS_L2C_ENA = 0x10310003,
CFG_SIGNAL_GPS_L5_ENA = 0x10310004,
CFG_SIGNAL_SBAS_ENA = 0x10310020,
CFG_SIGNAL_SBAS_L1CA_ENA = 0x10310005,
CFG_SIGNAL_GAL_ENA = 0x10310021,
CFG_SIGNAL_GAL_E1_ENA = 0x10310007,
CFG_SIGNAL_GAL_E5A_ENA = 0x10310009,
CFG_SIGNAL_GAL_E5B_ENA = 0x1031000a,
CFG_SIGNAL_BDS_ENA = 0x10310022,
CFG_SIGNAL_BDS_B1_ENA = 0x1031000d,
CFG_SIGNAL_BDS_B1C_ENA = 0x1031000f,
CFG_SIGNAL_BDS_B2_ENA = 0x1031000e,
CFG_SIGNAL_BDS_B2A_ENA = 0x10310028,
CFG_SIGNAL_QZSS_ENA = 0x10310024,
CFG_SIGNAL_QZSS_L1CA_ENA = 0x10310012,
CFG_SIGNAL_QZSS_L1S_ENA = 0x10310014,
CFG_SIGNAL_QZSS_L2C_ENA = 0x10310015,
CFG_SIGNAL_QZSS_L5_ENA = 0x10310017,
CFG_SIGNAL_GLO_ENA = 0x10310025,
CFG_SIGNAL_GLO_L1_ENA = 0x10310018,
CFG_SIGNAL_GLO_L2_ENA = 0x1031001a,
CFG_SIGNAL_NAVIC_ENA = 0x10310026,
CFG_SIGNAL_NAVIC_L5_ENA = 0x1031001d,
// other keys
CFG_NAVSPG_DYNMODEL = 0x20110021,
};
// layers for VALSET
#define UBX_VALSET_LAYER_RAM 0x1U
#define UBX_VALSET_LAYER_BBR 0x2U
#define UBX_VALSET_LAYER_FLASH 0x4U
#define UBX_VALSET_LAYER_ALL 0x7U
struct PACKED ubx_cfg_valset {
uint8_t version;
uint8_t layers;
uint8_t transaction;
uint8_t reserved[1];
uint32_t key;
};
struct PACKED ubx_cfg_valget {
uint8_t version;
uint8_t layers;
uint8_t reserved[2];
// variable length data, check buffer length
};
struct PACKED ubx_nav_posllh {
uint32_t itow; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
};
struct PACKED ubx_nav_status {
uint32_t itow; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
};
struct PACKED ubx_nav_dop {
uint32_t itow; // GPS msToW
uint16_t gDOP;
uint16_t pDOP;
uint16_t tDOP;
uint16_t vDOP;
uint16_t hDOP;
uint16_t nDOP;
uint16_t eDOP;
};
struct PACKED ubx_nav_solution {
uint32_t itow;
int32_t time_nsec;
uint16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
};
struct PACKED ubx_nav_pvt {
uint32_t itow;
uint16_t year;
uint8_t month, day, hour, min, sec;
uint8_t valid;
uint32_t t_acc;
int32_t nano;
uint8_t fix_type;
uint8_t flags;
uint8_t flags2;
uint8_t num_sv;
int32_t lon, lat;
int32_t h_ellipsoid, h_msl;
uint32_t h_acc, v_acc;
int32_t velN, velE, velD, gspeed;
int32_t head_mot;
uint32_t s_acc;
uint32_t head_acc;
uint16_t p_dop;
uint8_t flags3;
uint8_t reserved1[5];
int32_t headVeh;
int16_t magDec;
uint16_t magAcc;
};
struct PACKED ubx_nav_relposned {
uint8_t version;
uint8_t reserved1;
uint16_t refStationId;
uint32_t iTOW;
int32_t relPosN;
int32_t relPosE;
int32_t relPosD;
int32_t relPosLength;
int32_t relPosHeading;
uint8_t reserved2[4];
int8_t relPosHPN;
int8_t relPosHPE;
int8_t relPosHPD;
int8_t relPosHPLength;
uint32_t accN;
uint32_t accE;
uint32_t accD;
uint32_t accLength;
uint32_t accHeading;
uint8_t reserved3[4];
uint32_t flags;
};
struct PACKED ubx_nav_velned {
uint32_t itow; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
};
struct PACKED ubx_nav_timegps {
uint32_t itow;
int32_t ftow;
uint16_t week;
int8_t leapS;
uint8_t valid; // leapsvalid | weekvalid | tow valid;
uint32_t tAcc;
};
// Lea6 uses a 60 byte message
struct PACKED ubx_mon_hw_60 {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[17];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
};
// Neo7 uses a 68 byte message
struct PACKED ubx_mon_hw_68 {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[25];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
};
struct PACKED ubx_mon_hw2 {
int8_t ofsI;
uint8_t magI;
int8_t ofsQ;
uint8_t magQ;
uint8_t cfgSource;
uint8_t reserved0[3];
uint32_t lowLevCfg;
uint32_t reserved1[2];
uint32_t postStatus;
uint32_t reserved2;
};
struct PACKED ubx_mon_ver {
char swVersion[30];
char hwVersion[10];
char extension; // extensions are not enabled
};
struct PACKED ubx_nav_svinfo_header {
uint32_t itow;
uint8_t numCh;
uint8_t globalFlags;
uint16_t reserved;
};
#if UBLOX_RXM_RAW_LOGGING
struct PACKED ubx_rxm_raw {
int32_t iTOW;
int16_t week;
uint8_t numSV;
uint8_t reserved1;
struct ubx_rxm_raw_sv {
double cpMes;
double prMes;
float doMes;
uint8_t sv;
int8_t mesQI;
int8_t cno;
uint8_t lli;
} svinfo[UBLOX_MAX_RXM_RAW_SATS];
};
struct PACKED ubx_rxm_rawx {
double rcvTow;
uint16_t week;
int8_t leapS;
uint8_t numMeas;
uint8_t recStat;
uint8_t reserved1[3];
PACKED struct ubx_rxm_rawx_sv {
double prMes;
double cpMes;
float doMes;
uint8_t gnssId;
uint8_t svId;
uint8_t reserved2;
uint8_t freqId;
uint16_t locktime;
uint8_t cno;
uint8_t prStdev;
uint8_t cpStdev;
uint8_t doStdev;
uint8_t trkStat;
uint8_t reserved3;
} svinfo[UBLOX_MAX_RXM_RAWX_SATS];
};
#endif
struct PACKED ubx_ack_ack {
uint8_t clsID;
uint8_t msgID;
};
struct PACKED ubx_cfg_cfg {
uint32_t clearMask;
uint32_t saveMask;
uint32_t loadMask;
};
struct PACKED ubx_tim_tm2 {
uint8_t ch;
uint8_t flags;
uint16_t count;
uint16_t wnR;
uint16_t wnF;
uint32_t towMsR;
uint32_t towSubMsR;
uint32_t towMsF;
uint32_t towSubMsF;
uint32_t accEst;
};
// Receive buffer
union PACKED {
DEFINE_BYTE_ARRAY_METHODS
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_dop dop;
ubx_nav_solution solution;
ubx_nav_pvt pvt;
ubx_nav_timegps timegps;
ubx_nav_velned velned;
ubx_cfg_msg_rate msg_rate;
ubx_cfg_msg_rate_6 msg_rate_6;
ubx_cfg_nav_settings nav_settings;
ubx_cfg_nav_rate nav_rate;
ubx_cfg_prt prt;
ubx_mon_hw_60 mon_hw_60;
ubx_mon_hw_68 mon_hw_68;
ubx_mon_hw2 mon_hw2;
ubx_mon_ver mon_ver;
ubx_cfg_tp5 nav_tp5;
#if UBLOX_GNSS_SETTINGS
ubx_cfg_gnss gnss;
#endif
ubx_cfg_sbas sbas;
ubx_cfg_valget valget;
ubx_nav_svinfo_header svinfo_header;
ubx_nav_relposned relposned;
#if UBLOX_RXM_RAW_LOGGING
ubx_rxm_raw rxm_raw;
ubx_rxm_rawx rxm_rawx;
#endif
ubx_ack_ack ack;
ubx_tim_tm2 tim_tm2;
} _buffer;
enum class RELPOSNED {
gnssFixOK = 1U << 0,
diffSoln = 1U << 1,
relPosValid = 1U << 2,
carrSolnFloat = 1U << 3,
carrSolnFixed = 1U << 4,
isMoving = 1U << 5,
refPosMiss = 1U << 6,
refObsMiss = 1U << 7,
relPosHeadingValid = 1U << 8,
relPosNormalized = 1U << 9
};
enum ubs_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
CLASS_MON = 0x0A,
CLASS_RXM = 0x02,
CLASS_TIM = 0x0d,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_DOP = 0x4,
MSG_SOL = 0x6,
MSG_PVT = 0x7,
MSG_TIMEGPS = 0x20,
MSG_RELPOSNED = 0x3c,
MSG_VELNED = 0x12,
MSG_CFG_CFG = 0x09,
MSG_CFG_RATE = 0x08,
MSG_CFG_MSG = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24,
MSG_CFG_PRT = 0x00,
MSG_CFG_SBAS = 0x16,
MSG_CFG_GNSS = 0x3E,
MSG_CFG_TP5 = 0x31,
MSG_CFG_VALSET = 0x8A,
MSG_CFG_VALGET = 0x8B,
MSG_MON_HW = 0x09,
MSG_MON_HW2 = 0x0B,
MSG_MON_VER = 0x04,
MSG_NAV_SVINFO = 0x30,
MSG_RXM_RAW = 0x10,
MSG_RXM_RAWX = 0x15,
MSG_TIM_TM2 = 0x03
};
enum ubx_gnss_identifier {
GNSS_GPS = 0x00,
GNSS_SBAS = 0x01,
GNSS_GALILEO = 0x02,
GNSS_BEIDOU = 0x03,
GNSS_IMES = 0x04,
GNSS_QZSS = 0x05,
GNSS_GLONASS = 0x06
};
enum ubs_nav_fix_type {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,
FIX_2D = 2,
FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5
};
enum ubx_nav_status_bits {
NAV_STATUS_FIX_VALID = 1,
NAV_STATUS_DGPS_USED = 2
};
enum ubx_hardware_version {
ANTARIS = 0,
UBLOX_5,
UBLOX_6,
UBLOX_7,
UBLOX_M8,
UBLOX_F9 = 0x80, // comes from MON_VER hwVersion/swVersion strings
UBLOX_M9 = 0x81, // comes from MON_VER hwVersion/swVersion strings
UBLOX_M10 = 0x82,
UBLOX_UNKNOWN_HARDWARE_GENERATION = 0xff // not in the ublox spec used for
// flagging state in the driver
};
enum config_step {
STEP_PVT = 0,
STEP_NAV_RATE, // poll NAV rate
STEP_SOL,
STEP_PORT,
STEP_STATUS,
STEP_POSLLH,
STEP_VELNED,
STEP_TIMEGPS,
STEP_POLL_SVINFO, // poll svinfo
STEP_POLL_SBAS, // poll SBAS
STEP_POLL_NAV, // poll NAV settings
STEP_POLL_GNSS, // poll GNSS
STEP_POLL_TP5, // poll TP5
STEP_TMODE, // set TMODE-MODE
STEP_DOP,
STEP_MON_HW,
STEP_MON_HW2,
STEP_RAW,
STEP_RAWX,
STEP_VERSION,
STEP_RTK_MOVBASE, // setup moving baseline
STEP_TIM_TM2,
STEP_M10,
STEP_LAST
};
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// State machine state
uint8_t _step;
uint8_t _msg_id;
uint16_t _payload_length;
uint16_t _payload_counter;
uint8_t _class;
bool _cfg_saved;
uint32_t _last_vel_time;
uint32_t _last_pos_time;
uint32_t _last_cfg_sent_time;
uint8_t _num_cfg_save_tries;
uint32_t _last_config_time;
uint16_t _delay_time;
uint8_t _next_message;
uint8_t _ublox_port;
bool _have_version;
struct ubx_mon_ver _version;
uint32_t _unconfigured_messages;
uint8_t _hardware_generation;
uint32_t _last_pvt_itow;
uint32_t _last_relposned_itow;
uint32_t _last_relposned_ms;
// the role set from GPS_TYPE
AP_GPS::GPS_Role role;
// do we have new position information?
bool _new_position:1;
// do we have new speed information?
bool _new_speed:1;
uint8_t _disable_counter;
// Buffer parse & GPS state update
bool _parse_gps();
// used to update fix between status and position packets
AP_GPS::GPS_Status next_fix;
bool _cfg_needs_save;
bool noReceivedHdop;
bool havePvtMsg;
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
bool _configure_valset(ConfigKey key, const void *value, uint8_t layers=UBX_VALSET_LAYER_ALL);
bool _configure_valget(ConfigKey key);
void _configure_rate(void);
void _configure_sbas(bool enable);
void _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b);
bool _send_message(uint8_t msg_class, uint8_t msg_id, const void *msg, uint16_t size);
void send_next_rate_update(void);
bool _request_message_rate(uint8_t msg_class, uint8_t msg_id);
void _request_next_config(void);
void _request_port(void);
void _request_version(void);
void _save_cfg(void);
void _verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void _check_new_itow(uint32_t itow);
void unexpected_message(void);
void log_mon_hw(void);
void log_mon_hw2(void);
void log_tim_tm2(void);
void log_rxm_raw(const struct ubx_rxm_raw &raw);
void log_rxm_rawx(const struct ubx_rxm_rawx &raw);
#if GPS_MOVING_BASELINE
// see if we should use uart2 for moving baseline config
bool mb_use_uart2(void) const {
return option_set(AP_GPS::DriverOptions::UBX_MBUseUart2)?true:false;
}
#endif
// structure for list of config key/value pairs for
// specific configurations
struct PACKED config_list {
ConfigKey key;
// support up to 4 byte values, assumes little-endian
uint32_t value;
};
// return size of a config key payload
uint8_t config_key_size(ConfigKey key) const;
// configure a set of config key/value pairs. The unconfig_bit corresponds to
// a bit in _unconfigured_messages
bool _configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit, uint8_t layers=UBX_VALSET_LAYER_ALL);
// find index in active_config list
int8_t find_active_config_index(ConfigKey key) const;
// return true if GPS is capable of F9 config
bool supports_F9_config(void) const;
uint8_t _pps_freq = 1;
#ifdef HAL_GPIO_PPS
void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us);
void set_pps_desired_freq(uint8_t freq) override;
#endif
// status of active configuration for a role
struct {
const config_list *list;
uint8_t count;
uint32_t done_mask;
uint32_t unconfig_bit;
uint8_t layers;
} active_config;
#if GPS_MOVING_BASELINE
// config for moving baseline base
static const config_list config_MB_Base_uart1[];
static const config_list config_MB_Base_uart2[];
// config for moving baseline rover
static const config_list config_MB_Rover_uart1[];
static const config_list config_MB_Rover_uart2[];
// RTCM3 parser for when in moving baseline base mode
RTCM3_Parser *rtcm3_parser;
#endif // GPS_MOVING_BASELINE
static const config_list config_M10[];
};
4.1 AP_GPS_UBLOX::_detect
Check whether the message meets the Ublox protocol specification, and if so, it indicates that it is a Ublox device.
/*
detect a Ublox GPS. Adds one byte, and returns true if the stream
matches a UBlox
*/
bool
AP_GPS_UBLOX::_detect(struct UBLOX_detect_state &state, uint8_t data)
{
reset:
switch (state.step) {
case 1:
if (PREAMBLE2 == data) {
state.step++;
break;
}
state.step = 0;
FALLTHROUGH;
case 0:
if (PREAMBLE1 == data)
state.step++;
break;
case 2:
state.step++;
state.ck_b = state.ck_a = data;
break;
case 3:
state.step++;
state.ck_b += (state.ck_a += data);
break;
case 4:
state.step++;
state.ck_b += (state.ck_a += data);
state.payload_length = data;
break;
case 5:
state.step++;
state.ck_b += (state.ck_a += data);
state.payload_counter = 0;
break;
case 6:
state.ck_b += (state.ck_a += data);
if (++state.payload_counter == state.payload_length)
state.step++;
break;
case 7:
state.step++;
if (state.ck_a != data) {
state.step = 0;
goto reset;
}
break;
case 8:
state.step = 0;
if (state.ck_b == data) {
// a valid UBlox packet
return true;
} else {
goto reset;
}
}
return false;
}
4.2 AP_GPS_UBLOX::read
Since UART is mainly a character stream device, the driver reads in byte by byte through the serial port and parses it word by word.
When a packet of valid data is satisfied, _parse_gpsthe message is parsed.
// Ensure there is enough space for the largest possible outgoing message
// Process bytes available from the stream
//
// The stream is assumed to contain only messages we recognise. If it
// contains other messages, and those messages contain the preamble
// bytes, it is possible for this code to fail to synchronise to the
// stream immediately. Without buffering the entire message and
// re-processing it from the top, this is unavoidable. The parser
// attempts to avoid this when possible.
//
bool
AP_GPS_UBLOX::read(void)
{
bool parsed = false;
uint32_t millis_now = AP_HAL::millis();
// walk through the gps configuration at 1 message per second
if (millis_now - _last_config_time >= _delay_time) {
_request_next_config();
_last_config_time = millis_now;
if (_unconfigured_messages) { // send the updates faster until fully configured
if (!havePvtMsg && (_unconfigured_messages & CONFIG_REQUIRED_INITIAL)) {
_delay_time = 300;
} else {
_delay_time = 750;
}
} else {
_delay_time = 2000;
}
}
if(!_unconfigured_messages && gps._save_config && !_cfg_saved &&
_num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 &&
!hal.util->get_soft_armed()) {
//save the configuration sent until now
if (gps._save_config == 1 ||
(gps._save_config == 2 && _cfg_needs_save)) {
_save_cfg();
}
}
const uint16_t numc = MIN(port->available(), 8192U);
for (uint16_t i = 0; i < numc; i++) { // Process bytes received
// read the next byte
const int16_t rdata = port->read();
if (rdata < 0) {
break;
}
const uint8_t data = rdata;
#if AP_GPS_DEBUG_LOGGING_ENABLED
log_data(&data, 1);
#endif
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
if (rtcm3_parser->read(data)) {
// we've found a RTCMv3 packet. We stop parsing at
// this point and reset u-blox parse state. We need to
// stop parsing to give the higher level driver a
// chance to send the RTCMv3 packet to another (rover)
// GPS
_step = 0;
break;
}
}
#endif
reset:
switch(_step) {
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
Debug("reset %u", __LINE__);
FALLTHROUGH;
case 0:
if(PREAMBLE1 == data)
_step++;
break;
// Message header processing
//
// We sniff the class and message ID to decide whether we
// are going to gather the message bytes or just discard
// them.
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
_class = data;
_ck_b = _ck_a = data; // reset the checksum accumulators
break;
case 3:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
break;
case 4:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)(data<<8);
if (_payload_length > sizeof(_buffer)) {
Debug("large payload %u", (unsigned)_payload_length);
// assume any payload bigger then what we know about is noise
_payload_length = 0;
_step = 0;
goto reset;
}
_payload_counter = 0; // prepare to receive payload
if (_payload_length == 0) {
// bypass payload and go straight to checksum
_step++;
}
break;
// Receive message data
//
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_payload_counter < sizeof(_buffer)) {
_buffer[_payload_counter] = data;
}
if (++_payload_counter == _payload_length)
_step++;
break;
// Checksum and message processing
//
case 7:
_step++;
if (_ck_a != data) {
Debug("bad cka %x should be %x", data, _ck_a);
_step = 0;
goto reset;
}
break;
case 8:
_step = 0;
if (_ck_b != data) {
Debug("bad ckb %x should be %x", data, _ck_b);
break; // bad checksum
}
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
// this is a uBlox packet, discard any partial RTCMv3 state
rtcm3_parser->reset();
}
#endif
if (_parse_gps()) {
parsed = true;
}
break;
}
}
return parsed;
}
4.3 AP_GPS_UBLOX::_parse_gps
The whole function is very long, but "everything remains the same", the main purpose is to analyze the Ublox protocol message. From the function, you can analyze and support the Ublox message:
- CLASS_ACK
- MSG_ACK_ACK
- CLASS_CFG
- MSG_CFG_NAV_SETTINGS
- MSG_CFG_GNSS
- MSG_CFG_SBAS
- MSG_CFG_MSG
- MSG_CFG_PRT
- MSG_CFG_RATE
- MSG_CFG_TP5
- MSG_CFG_SELECTED
- CLASS_MON
- MSG_MON_HW
- MSG_MON_HW2
- MSG_MON_VER
- CLASS_RXM
- MSG_RXM_RAW
- MSG_RXM_RAWX
- CLASS_TIM
- MSG_TIM_TM2
- CLASS_NAV
- MSG_POSLLH
- MSG_STATUS
- MSG_DOP
- MSG_SOL
- MSG_RELPOSNED
- MSG_PVT
- MSG_TIMEGPS
- MSG_VELNED
- MSG_NAV_SVINFO
bool
AP_GPS_UBLOX::_parse_gps(void)
{
if (_class == CLASS_ACK) {
Debug("ACK %u", (unsigned)_msg_id);
if(_msg_id == MSG_ACK_ACK) {
switch(_buffer.ack.clsID) {
case CLASS_CFG:
switch(_buffer.ack.msgID) {
case MSG_CFG_CFG:
_cfg_saved = true;
_cfg_needs_save = false;
break;
case MSG_CFG_GNSS:
_unconfigured_messages &= ~CONFIG_GNSS;
break;
case MSG_CFG_MSG:
// There is no way to know what MSG config was ack'ed, assume it was the last
// one requested. To verify it rerequest the last config we sent. If we miss
// the actual ack we will catch it next time through the poll loop, but that
// will be a good chunk of time later.
break;
case MSG_CFG_NAV_SETTINGS:
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
break;
case MSG_CFG_RATE:
// The GPS will ACK a update rate that is invalid. in order to detect this
// only accept the rate as configured by reading the settings back and
// validating that they all match the target values
break;
case MSG_CFG_SBAS:
_unconfigured_messages &= ~CONFIG_SBAS;
break;
case MSG_CFG_TP5:
_unconfigured_messages &= ~CONFIG_TP5;
break;
}
break;
case CLASS_MON:
switch(_buffer.ack.msgID) {
case MSG_MON_HW:
_unconfigured_messages &= ~CONFIG_RATE_MON_HW;
break;
case MSG_MON_HW2:
_unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
break;
}
}
}
return false;
}
if (_class == CLASS_CFG) {
switch(_msg_id) {
case MSG_CFG_NAV_SETTINGS:
Debug("Got settings %u min_elev %d drLimit %u\n",
(unsigned)_buffer.nav_settings.dynModel,
(int)_buffer.nav_settings.minElev,
(unsigned)_buffer.nav_settings.drLimit);
_buffer.nav_settings.mask = 0;
if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
_buffer.nav_settings.dynModel != gps._navfilter) {
// we've received the current nav settings, change the engine
// settings and send them back
Debug("Changing engine setting from %u to %u\n",
(unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
_buffer.nav_settings.dynModel = gps._navfilter;
_buffer.nav_settings.mask |= 1;
}
if (gps._min_elevation != -100 &&
_buffer.nav_settings.minElev != gps._min_elevation) {
Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
_buffer.nav_settings.minElev = gps._min_elevation;
_buffer.nav_settings.mask |= 2;
}
if (_buffer.nav_settings.mask != 0) {
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
&_buffer.nav_settings,
sizeof(_buffer.nav_settings));
_unconfigured_messages |= CONFIG_NAV_SETTINGS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
}
return false;
#if UBLOX_GNSS_SETTINGS
case MSG_CFG_GNSS:
if (gps._gnss_mode[state.instance] != 0) {
struct ubx_cfg_gnss start_gnss = _buffer.gnss;
uint8_t gnssCount = 0;
Debug("Got GNSS Settings %u %u %u %u:\n",
(unsigned)_buffer.gnss.msgVer,
(unsigned)_buffer.gnss.numTrkChHw,
(unsigned)_buffer.gnss.numTrkChUse,
(unsigned)_buffer.gnss.numConfigBlocks);
#if UBLOX_DEBUGGING
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
Debug(" %u %u %u 0x%08x\n",
(unsigned)_buffer.gnss.configBlock[i].gnssId,
(unsigned)_buffer.gnss.configBlock[i].resTrkCh,
(unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
(unsigned)_buffer.gnss.configBlock[i].flags);
}
#endif
for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
if((gps._gnss_mode[state.instance] & (1 << i)) && i != GNSS_SBAS) {
gnssCount++;
}
}
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
// Reserve an equal portion of channels for all enabled systems that supports it
if(gps._gnss_mode[state.instance] & (1 << _buffer.gnss.configBlock[i].gnssId)) {
if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId && (_hardware_generation > UBLOX_M8 || GNSS_GALILEO !=_buffer.gnss.configBlock[i].gnssId)) {
_buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
_buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
} else {
if(GNSS_SBAS ==_buffer.gnss.configBlock[i].gnssId) {
_buffer.gnss.configBlock[i].resTrkCh = 1;
_buffer.gnss.configBlock[i].maxTrkCh = 3;
}
if(GNSS_GALILEO ==_buffer.gnss.configBlock[i].gnssId) {
_buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
_buffer.gnss.configBlock[i].maxTrkCh = 8; //Per the M8 receiver description UBX-13003221 - R16, 4.1.1.3 it is not recommended to set the number of galileo channels higher then eigh
}
}
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
} else {
_buffer.gnss.configBlock[i].resTrkCh = 0;
_buffer.gnss.configBlock[i].maxTrkCh = 0;
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
}
}
if (memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) {
_send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
_unconfigured_messages |= CONFIG_GNSS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_GNSS;
}
} else {
_unconfigured_messages &= ~CONFIG_GNSS;
}
return false;
#endif
case MSG_CFG_SBAS:
if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n",
(unsigned)_buffer.sbas.mode,
(unsigned)_buffer.sbas.usage,
(unsigned)_buffer.sbas.maxSBAS,
(unsigned)_buffer.sbas.scanmode2,
(unsigned)_buffer.sbas.scanmode1);
if (_buffer.sbas.mode != gps._sbas_mode) {
_buffer.sbas.mode = gps._sbas_mode;
_send_message(CLASS_CFG, MSG_CFG_SBAS,
&_buffer.sbas,
sizeof(_buffer.sbas));
_unconfigured_messages |= CONFIG_SBAS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
}
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
}
return false;
case MSG_CFG_MSG:
if(_payload_length == sizeof(ubx_cfg_msg_rate_6)) {
// can't verify the setting without knowing the port
// request the port again
if(_ublox_port >= UBLOX_MAX_PORTS) {
_request_port();
return false;
}
_verify_rate(_buffer.msg_rate_6.msg_class, _buffer.msg_rate_6.msg_id,
_buffer.msg_rate_6.rates[_ublox_port]);
} else {
_verify_rate(_buffer.msg_rate.msg_class, _buffer.msg_rate.msg_id,
_buffer.msg_rate.rate);
}
return false;
case MSG_CFG_PRT:
_ublox_port = _buffer.prt.portID;
return false;
case MSG_CFG_RATE:
if(_buffer.nav_rate.measure_rate_ms != gps._rate_ms[state.instance] ||
_buffer.nav_rate.nav_rate != 1 ||
_buffer.nav_rate.timeref != 0) {
_configure_rate();
_unconfigured_messages |= CONFIG_RATE_NAV;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_RATE_NAV;
}
return false;
#if CONFIGURE_PPS_PIN
case MSG_CFG_TP5: {
// configure the PPS pin for 1Hz, zero delay
Debug("Got TP5 ver=%u 0x%04x %u\n",
(unsigned)_buffer.nav_tp5.version,
(unsigned)_buffer.nav_tp5.flags,
(unsigned)_buffer.nav_tp5.freqPeriod);
#ifdef HAL_GPIO_PPS
hal.gpio->attach_interrupt(HAL_GPIO_PPS, FUNCTOR_BIND_MEMBER(&AP_GPS_UBLOX::pps_interrupt, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_FALLING);
#endif
const uint16_t desired_flags = 0x003f;
const uint16_t desired_period_hz = _pps_freq;
if (_buffer.nav_tp5.flags != desired_flags ||
_buffer.nav_tp5.freqPeriod != desired_period_hz) {
_buffer.nav_tp5.tpIdx = 0;
_buffer.nav_tp5.reserved1[0] = 0;
_buffer.nav_tp5.reserved1[1] = 0;
_buffer.nav_tp5.antCableDelay = 0;
_buffer.nav_tp5.rfGroupDelay = 0;
_buffer.nav_tp5.freqPeriod = desired_period_hz;
_buffer.nav_tp5.freqPeriodLock = desired_period_hz;
_buffer.nav_tp5.pulseLenRatio = 1;
_buffer.nav_tp5.pulseLenRatioLock = 2;
_buffer.nav_tp5.userConfigDelay = 0;
_buffer.nav_tp5.flags = desired_flags;
_send_message(CLASS_CFG, MSG_CFG_TP5,
&_buffer.nav_tp5,
sizeof(_buffer.nav_tp5));
_unconfigured_messages |= CONFIG_TP5;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_TP5;
}
return false;
}
#endif // CONFIGURE_PPS_PIN
case MSG_CFG_VALGET: {
uint8_t cfg_len = _payload_length - sizeof(ubx_cfg_valget);
const uint8_t *cfg_data = (const uint8_t *)(&_buffer) + sizeof(ubx_cfg_valget);
while (cfg_len >= 5) {
ConfigKey id;
memcpy(&id, cfg_data, sizeof(uint32_t));
cfg_len -= 4;
cfg_data += 4;
switch (id) {
case ConfigKey::TMODE_MODE: {
uint8_t mode = cfg_data[0];
if (mode != 0) {
// ask for mode 0, to disable TIME mode
mode = 0;
_configure_valset(ConfigKey::TMODE_MODE, &mode);
_cfg_needs_save = true;
_unconfigured_messages |= CONFIG_TMODE_MODE;
} else {
_unconfigured_messages &= ~CONFIG_TMODE_MODE;
}
break;
}
default:
break;
}
// see if it is in active config list
int8_t cfg_idx = find_active_config_index(id);
if (cfg_idx >= 0) {
const uint8_t key_size = config_key_size(id);
if (cfg_len < key_size ||
memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) {
_configure_valset(id, &active_config.list[cfg_idx].value, active_config.layers);
_unconfigured_messages |= active_config.unconfig_bit;
active_config.done_mask &= ~(1U << cfg_idx);
_cfg_needs_save = true;
} else {
active_config.done_mask |= (1U << cfg_idx);
Debug("done %u mask=0x%x",
unsigned(cfg_idx),
unsigned(active_config.done_mask));
if (active_config.done_mask == (1U<<active_config.count)-1) {
// all done!
_unconfigured_messages &= ~active_config.unconfig_bit;
}
}
}
// step over the value
uint8_t step_size = config_key_size(id);
if (step_size == 0) {
return false;
}
cfg_len -= step_size;
cfg_data += step_size;
}
}
}
}
if (_class == CLASS_MON) {
switch(_msg_id) {
case MSG_MON_HW:
if (_payload_length == 60 || _payload_length == 68) {
log_mon_hw();
}
break;
case MSG_MON_HW2:
if (_payload_length == 28) {
log_mon_hw2();
}
break;
case MSG_MON_VER:
_have_version = true;
strncpy(_version.hwVersion, _buffer.mon_ver.hwVersion, sizeof(_version.hwVersion));
strncpy(_version.swVersion, _buffer.mon_ver.swVersion, sizeof(_version.swVersion));
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
"u-blox %d HW: %s SW: %s",
state.instance + 1,
_version.hwVersion,
_version.swVersion);
// check for F9 and M9. The F9 does not respond to SVINFO,
// so we need to use MON_VER for hardware generation
if (strncmp(_version.hwVersion, "00190000", 8) == 0) {
if (strncmp(_version.swVersion, "EXT CORE 1", 10) == 0) {
// a F9
if (_hardware_generation != UBLOX_F9) {
// need to ensure time mode is correctly setup on F9
_unconfigured_messages |= CONFIG_TMODE_MODE;
}
_hardware_generation = UBLOX_F9;
}
if (strncmp(_version.swVersion, "EXT CORE 4", 10) == 0) {
// a M9
_hardware_generation = UBLOX_M9;
}
}
// check for M10
if (strncmp(_version.hwVersion, "000A0000", 8) == 0) {
if (strncmp(_version.swVersion, "ROM SPG 5", 9) == 0) {
_hardware_generation = UBLOX_M10;
_unconfigured_messages |= CONFIG_M10;
}
}
break;
default:
unexpected_message();
}
return false;
}
#if UBLOX_RXM_RAW_LOGGING
if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
log_rxm_raw(_buffer.rxm_raw);
return false;
} else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
log_rxm_rawx(_buffer.rxm_rawx);
return false;
}
#endif // UBLOX_RXM_RAW_LOGGING
#if UBLOX_TIM_TM2_LOGGING
if ((_class == CLASS_TIM) && (_msg_id == MSG_TIM_TM2) && (_payload_length == 28)) {
log_tim_tm2();
return false;
}
#endif // UBLOX_TIM_TM2_LOGGING
if (_class != CLASS_NAV) {
unexpected_message();
return false;
}
switch (_msg_id) {
case MSG_POSLLH:
Debug("MSG_POSLLH next_fix=%u", next_fix);
if (havePvtMsg) {
_unconfigured_messages |= CONFIG_RATE_POSLLH;
break;
}
_check_new_itow(_buffer.posllh.itow);
_last_pos_time = _buffer.posllh.itow;
state.location.lng = _buffer.posllh.longitude;
state.location.lat = _buffer.posllh.latitude;
if (option_set(AP_GPS::HeightEllipsoid)) {
state.location.alt = _buffer.posllh.altitude_ellipsoid / 10;
} else {
state.location.alt = _buffer.posllh.altitude_msl / 10;
}
state.have_undulation = true;
state.undulation = (_buffer.posllh.altitude_msl - _buffer.posllh.altitude_ellipsoid) * 0.001;
state.status = next_fix;
_new_position = true;
state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
#if UBLOX_FAKE_3DLOCK
state.location.lng = 1491652300L;
state.location.lat = -353632610L;
state.location.alt = 58400;
state.vertical_accuracy = 0;
state.horizontal_accuracy = 0;
#endif
break;
case MSG_STATUS:
Debug("MSG_STATUS fix_status=%u fix_type=%u",
_buffer.status.fix_status,
_buffer.status.fix_type);
_check_new_itow(_buffer.status.itow);
if (havePvtMsg) {
_unconfigured_messages |= CONFIG_RATE_STATUS;
break;
}
if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
(_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
}else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
next_fix = AP_GPS::GPS_OK_FIX_2D;
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
#if UBLOX_FAKE_3DLOCK
state.status = AP_GPS::GPS_OK_FIX_3D;
next_fix = state.status;
#endif
break;
case MSG_DOP:
Debug("MSG_DOP");
noReceivedHdop = false;
_check_new_itow(_buffer.dop.itow);
state.hdop = _buffer.dop.hDOP;
state.vdop = _buffer.dop.vDOP;
#if UBLOX_FAKE_3DLOCK
state.hdop = 130;
state.hdop = 170;
#endif
break;
case MSG_SOL:
Debug("MSG_SOL fix_status=%u fix_type=%u",
_buffer.solution.fix_status,
_buffer.solution.fix_type);
_check_new_itow(_buffer.solution.itow);
if (havePvtMsg) {
state.time_week = _buffer.solution.week;
break;
}
if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
(_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
}else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
next_fix = AP_GPS::GPS_OK_FIX_2D;
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
if(noReceivedHdop) {
state.hdop = _buffer.solution.position_DOP;
}
state.num_sats = _buffer.solution.satellites;
if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
state.last_gps_time_ms = AP_HAL::millis();
state.time_week_ms = _buffer.solution.itow;
state.time_week = _buffer.solution.week;
}
#if UBLOX_FAKE_3DLOCK
next_fix = state.status;
state.num_sats = 10;
state.time_week = 1721;
state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
state.last_gps_time_ms = AP_HAL::millis();
state.hdop = 130;
#endif
break;
#if GPS_MOVING_BASELINE
case MSG_RELPOSNED:
{
if (role != AP_GPS::GPS_ROLE_MB_ROVER) {
// ignore RELPOSNED if not configured as a rover
break;
}
// note that we require the yaw to come from a fixed solution, not a float solution
// yaw from a float solution would only be acceptable with a very large separation between
// GPS modules
const uint32_t valid_mask = static_cast<uint32_t>(RELPOSNED::relPosHeadingValid) |
static_cast<uint32_t>(RELPOSNED::relPosValid) |
static_cast<uint32_t>(RELPOSNED::gnssFixOK) |
static_cast<uint32_t>(RELPOSNED::isMoving) |
static_cast<uint32_t>(RELPOSNED::carrSolnFixed);
const uint32_t invalid_mask = static_cast<uint32_t>(RELPOSNED::refPosMiss) |
static_cast<uint32_t>(RELPOSNED::refObsMiss) |
static_cast<uint32_t>(RELPOSNED::carrSolnFloat);
_check_new_itow(_buffer.relposned.iTOW);
if (_buffer.relposned.iTOW != _last_relposned_itow+200) {
// useful for looking at packet loss on links
MB_Debug("RELPOSNED ITOW %u %u\n", unsigned(_buffer.relposned.iTOW), unsigned(_last_relposned_itow));
}
_last_relposned_itow = _buffer.relposned.iTOW;
MB_Debug("RELPOSNED flags: %lx valid: %lx invalid: %lx\n", _buffer.relposned.flags, valid_mask, invalid_mask);
if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
((_buffer.relposned.flags & invalid_mask) == 0)) {
if (calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5,
_buffer.relposned.relPosLength * 0.01,
_buffer.relposned.relPosD*0.01)) {
state.have_gps_yaw_accuracy = true;
state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
_last_relposned_ms = AP_HAL::millis();
}
state.relPosHeading = _buffer.relposned.relPosHeading * 1e-5;
state.relPosLength = _buffer.relposned.relPosLength * 0.01;
state.relPosD = _buffer.relposned.relPosD * 0.01;
state.accHeading = _buffer.relposned.accHeading * 1e-5;
state.relposheading_ts = AP_HAL::millis();
} else {
state.have_gps_yaw_accuracy = false;
}
}
break;
#endif // GPS_MOVING_BASELINE
case MSG_PVT:
Debug("MSG_PVT");
havePvtMsg = true;
// position
_check_new_itow(_buffer.pvt.itow);
_last_pvt_itow = _buffer.pvt.itow;
_last_pos_time = _buffer.pvt.itow;
state.location.lng = _buffer.pvt.lon;
state.location.lat = _buffer.pvt.lat;
if (option_set(AP_GPS::HeightEllipsoid)) {
state.location.alt = _buffer.pvt.h_ellipsoid / 10;
} else {
state.location.alt = _buffer.pvt.h_msl / 10;
}
state.have_undulation = true;
state.undulation = (_buffer.pvt.h_msl - _buffer.pvt.h_ellipsoid) * 0.001;
switch (_buffer.pvt.fix_type)
{
case 0:
state.status = AP_GPS::NO_FIX;
break;
case 1:
state.status = AP_GPS::NO_FIX;
break;
case 2:
state.status = AP_GPS::GPS_OK_FIX_2D;
break;
case 3:
state.status = AP_GPS::GPS_OK_FIX_3D;
if (_buffer.pvt.flags & 0b00000010) // diffsoln
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
if (_buffer.pvt.flags & 0b01000000) // carrsoln - float
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
if (_buffer.pvt.flags & 0b10000000) // carrsoln - fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 4:
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
"Unexpected state %d", _buffer.pvt.flags);
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 5:
state.status = AP_GPS::NO_FIX;
break;
default:
state.status = AP_GPS::NO_FIX;
break;
}
next_fix = state.status;
_new_position = true;
state.horizontal_accuracy = _buffer.pvt.h_acc*1.0e-3f;
state.vertical_accuracy = _buffer.pvt.v_acc*1.0e-3f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
// SVs
state.num_sats = _buffer.pvt.num_sv;
// velocity
_last_vel_time = _buffer.pvt.itow;
state.ground_speed = _buffer.pvt.gspeed*0.001f; // m/s
state.ground_course = wrap_360(_buffer.pvt.head_mot * 1.0e-5f); // Heading 2D deg * 100000
state.have_vertical_velocity = true;
state.velocity.x = _buffer.pvt.velN * 0.001f;
state.velocity.y = _buffer.pvt.velE * 0.001f;
state.velocity.z = _buffer.pvt.velD * 0.001f;
state.have_speed_accuracy = true;
state.speed_accuracy = _buffer.pvt.s_acc*0.001f;
_new_speed = true;
// dop
if(noReceivedHdop) {
state.hdop = _buffer.pvt.p_dop;
state.vdop = _buffer.pvt.p_dop;
}
state.last_gps_time_ms = AP_HAL::millis();
// time
state.time_week_ms = _buffer.pvt.itow;
#if UBLOX_FAKE_3DLOCK
state.location.lng = 1491652300L;
state.location.lat = -353632610L;
state.location.alt = 58400;
state.vertical_accuracy = 0;
state.horizontal_accuracy = 0;
state.status = AP_GPS::GPS_OK_FIX_3D;
state.num_sats = 10;
state.time_week = 1721;
state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
state.last_gps_time_ms = AP_HAL::millis();
state.hdop = 130;
state.speed_accuracy = 0;
next_fix = state.status;
#endif
break;
case MSG_TIMEGPS:
Debug("MSG_TIMEGPS");
_check_new_itow(_buffer.timegps.itow);
if (_buffer.timegps.valid & UBX_TIMEGPS_VALID_WEEK_MASK) {
state.time_week = _buffer.timegps.week;
}
break;
case MSG_VELNED:
Debug("MSG_VELNED");
if (havePvtMsg) {
_unconfigured_messages |= CONFIG_RATE_VELNED;
break;
}
_check_new_itow(_buffer.velned.itow);
_last_vel_time = _buffer.velned.itow;
state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
state.ground_course = wrap_360(_buffer.velned.heading_2d * 1.0e-5f); // Heading 2D deg * 100000
state.have_vertical_velocity = true;
state.velocity.x = _buffer.velned.ned_north * 0.01f;
state.velocity.y = _buffer.velned.ned_east * 0.01f;
state.velocity.z = _buffer.velned.ned_down * 0.01f;
velocity_to_speed_course(state);
state.have_speed_accuracy = true;
state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
#if UBLOX_FAKE_3DLOCK
state.speed_accuracy = 0;
#endif
_new_speed = true;
break;
case MSG_NAV_SVINFO:
{
Debug("MSG_NAV_SVINFO\n");
static const uint8_t HardwareGenerationMask = 0x07;
_check_new_itow(_buffer.svinfo_header.itow);
_hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
switch (_hardware_generation) {
case UBLOX_5:
case UBLOX_6:
// only 7 and newer support CONFIG_GNSS
_unconfigured_messages &= ~CONFIG_GNSS;
break;
case UBLOX_7:
case UBLOX_M8:
#if UBLOX_SPEED_CHANGE
port->begin(4000000U);
Debug("Changed speed to 4Mhz for SPI-driven UBlox\n");
#endif
break;
default:
hal.console->printf("Wrong Ublox Hardware Version%u\n", _hardware_generation);
break;
};
_unconfigured_messages &= ~CONFIG_VERSION;
/* We don't need that anymore */
_configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
break;
}
default:
Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
if (++_disable_counter == 0) {
Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
_configure_message_rate(CLASS_NAV, _msg_id, 0);
}
return false;
}
if (state.have_gps_yaw) {
// when we are a rover we want to ensure we have both the new
// PVT and the new RELPOSNED message so that we give a
// consistent view
if (AP_HAL::millis() - _last_relposned_ms > 400) {
// we have stopped receiving valid RELPOSNED messages, disable yaw reporting
state.have_gps_yaw = false;
} else if (_last_relposned_itow != _last_pvt_itow) {
// wait until ITOW matches
return false;
}
}
// we only return true when we get new position and speed data
// this ensures we don't use stale data
if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
_new_speed = _new_position = false;
return true;
}
return false;
}
5. Summary
ArduPilot encapsulates the overall GPS business through the following application frameworks:
- AP_GPS //Application high-level abstract business
- AP_GPS_Backend //driver abstract method
- AP_GPS_XXX //Specific protocol analysis
XXX stands for: ERB/GSOF/MAV/MSP/ExternalAHRS/NMEA/NOVA/SBF/SBP/SBP2/SIRF/UBLOX/DroneCAN.
6. References
【1】A brief introduction to the ArduPilot open source flight control system【
2】ArduPilot 's open source code framework
【3】ArduPilot flight control ubuntu22.04- SITL installation Mission Planner simulation of control [6] ArduPilot flight control AOCODARC-H7DUAL firmware compilation [7] ArduPilot open source code Library&Sketches design [8] ArduPilot open source code Sensor Drivers design [9] ArduPilot open source code basic knowledge & Threading concept [10] ArduPilot The use of open source code UARTs and the Console【11】ArduPilot flight control startup & running process introduction【11 】 ArduPilot open source code Task introduction【12】ArduPilot open source code AP_Param【13】ArduPilot open source code AP_Scheduler【14】ArduPilot open source code AP_VideoTX
[15] AP_InertialSensor_Backend of ArduPilot open source code
[16] AP_InertialSensor of ArduPilot open source code
[17] AP_Logger of ArduPilot open source code