ArduPilot开源代码之AP_GPS
AP_GPS//应用高层抽象业务AP_GPS_Backend//驱动抽象方法AP_GPS_XXX//具体协议解析其中XXX代表：ERB/GSOF/MAV/MSP/ExternalAHRS/NMEA/NOVA/SBF/SBP/SBP2/SIRF/UBLOX/DroneCAN。
·
ArduPilot开源代码之AP_GPS
1. 源由
GPS模块这里不能狭义的理解为全球定位。从广义的角度，是一个飞控位置处理模块。
换句话说，只要通过位置接口协议提供三维坐标位置，那么飞控就能应用内部算法，进行一些列的路点巡航，定位等位置相关的飞行操作。
因此，了解该模块，对于飞控应用来说举足轻重。接下来，分析和研读该位置处理模块。
2. AP_GPS类
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
支持的GPS类型种类繁多，通常我们常听说的应该是 NMEA/UBLOX/MSP，除此之外ArduPilot还支持以下类型：
// 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状态对于高精度定位来说很重要，通常我们看到的是GPS_OK_FIX_3D，而当我们卫星数量在20颗以上的时候会出现更高精度的GPS_OK_FIX_3D_DGPS。
如果有幸使用RTK的GPS模块，就会得到RTK的cm级别精度，这个对于精确定位来说是非常重要的，毕竟米级别的精度在低空复杂区域巡航的角度来说是非常危险的。
/// 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类方法
3.1 AP_GPS类初始化
应用类的初始化总是有个init函数处理，启动过程可参考：ArduPilot飞控启动&运行过程简介。
AP_Vehicle::setup
└──> init_ardupilot
└──> AP_GPS::init
对数据融合和状态进行初始化。
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任务
SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200, 9),
定时以50Hz频率定期更新GPS信息（通常GPS是5Hz或者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动态维护
GPS实例维护（初始化，数据更新，状态监测）。
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实例初始化
GPS实例运行检测步骤。如果找到了GPS，那么将new一个[实例]并更改[实例]状态，status从NO_GPS到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实例发现
GPS实例运行检测，如果这找到了GPS，那么它将返回一个实例，否则为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. 实例理解
鉴于目前穿越机上常用的GPS模块采用UBLOX协议，这里就该实例进行解释。
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
检查报文是否满足UBlox协议规范，如果满足则表示是UBlox设备。
/*
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
由于UART主要是字符流设备，因此驱动通过串口一个字节一个字节读入，并逐字进行解析。
当满足一包有效数据时，进行_parse_gps报文解析。
// 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
整个函数非常长，但是“万变不离其宗”，主要目的就是对UBlox协议报文进行解析。从函数可以分析支持一下UBlox报文：
- 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_VALGET
- 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. 总结
ArduPilot通过以下几个应用类框架，将GPS整体业务进行了封装：
- AP_GPS //应用高层抽象业务
- AP_GPS_Backend //驱动抽象方法
- AP_GPS_XXX //具体协议解析
其中XXX代表：ERB/GSOF/MAV/MSP/ExternalAHRS/NMEA/NOVA/SBF/SBP/SBP2/SIRF/UBLOX/DroneCAN。
6. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot飞控之ubuntu22.04-SITL安装
【4】ArduPilot飞控之ubuntu22.04-Gazebo模拟
【5】ArduPilot飞控之Mission Planner模拟
【6】ArduPilot飞控AOCODARC-H7DUAL固件编译
【7】ArduPilot之开源代码Library&Sketches设计
【8】ArduPilot之开源代码Sensor Drivers设计
【9】ArduPilot之开源代码基础知识&Threading概念
【10】ArduPilot之开源代码UARTs and the Console使用
【11】ArduPilot飞控启动&运行过程简介
【11】ArduPilot之开源代码Task介绍
【12】ArduPilot开源代码之AP_Param
【13】ArduPilot开源代码之AP_Scheduler
【14】ArduPilot开源代码之AP_VideoTX
【15】ArduPilot开源代码之AP_InertialSensor_Backend
【16】ArduPilot开源代码之AP_InertialSensor
【17】ArduPilot开源代码之AP_Logger
瓜分20万奖金 获得内推名额 丰厚实物奖励 易参与易上手
更多推荐
0
0- 0
已为社区贡献14条内容
所有评论(0)