Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

GPS: Official Swift Binary Protocol GPS Driver v3 for Piksi GPS

Browse Source 
DRIVER FEATURES:
- All logic for RTK vs Normal now lives inside Piksi
- Supports observation uplink through telem radio
- Supports full SBP packet logging
- Reports high-rate green blinking to indicate RTK lock.
- Switchable to accept only Float or Integer RTK locks.

THIS REQUIRES PIKSI FIRMWARE v0.14 OR HIGHER
- Uses Piksi's new Pseudo-Absolute-Positioning mode
- Onboard Piksi must have Pseudo-Absolute mode enabled
- Ground Station Piksi must have a surveyed location in settings
- Ground Station Piksi must send its location to Onboard Piksi.

NEXT STEPS REQUIRED:
- EKF needs to take higher accuracy GPS into account
- EKF needs to take GPS RTK height into account
- GCS needs to support sending SBP observation packets
--- MAVProxy patch forthcoming
This commit is contained in:
Niels Joubert 2015-04-22 16:10:46 -07:00 committed by Andrew Tridgell
parent 0480867de2
commit a5beef00d1
5 changed files with 313 additions and 868 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
libraries/AP_GPS/AP_GPS.cpp
View File

@ -79,6 +79,25 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("MIN_ELEV", 6, AP_GPS, _min_elevation, -100),
#if GPS_MAX_INSTANCES > 1
// @Param: INJECT_TO
// @DisplayName: Destination for GPS_INJECT_DATA MAVLink packets
// @Description: The GGS can send raw serial packets to inject data to multiple GPSes.
// @Values: 0,1: send to specified instance. 127: broadcast
AP_GROUPINFO("INJECT_TO", 7, AP_GPS, _inject_to, 127),
#endif
#if GPS_RTK_AVAILABLE
// @Param: SBP_LOGMASK
// @DisplayName: Swift Binary Protocol Logging Mask
// @Description: Masked with the SBP msg_type field to determine whether SBR1/SBR2 data is logged
// @Values: 0x0000 for none, 0xFFFF for all, 0xFF00 for external only.
// @User: Advanced
AP_GROUPINFO("SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, 0xFF00),
#endif
AP_GROUPEND
};
@ -251,34 +270,6 @@ found_gps:
}
}
bool
AP_GPS::can_calculate_base_pos(void)
{
#if GPS_RTK_AVAILABLE
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
if (drivers[i] != NULL && drivers[i]->can_calculate_base_pos()) {
return true;
}
}
#endif
return false;
}
/*
Tells the underlying GPS drivers to capture its current position as home.
*/
void
AP_GPS::calculate_base_pos(void)
{
#if GPS_RTK_AVAILABLE
for (uint8_t i = 0; i<GPS_MAX_INSTANCES; i++) {
if (drivers[i] != NULL && drivers[i]->can_calculate_base_pos()) {
drivers[i]->calculate_base_pos();
}
}
#endif
}
AP_GPS::GPS_Status
AP_GPS::highest_supported_status(uint8_t instance) const
{
@ -441,6 +432,7 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
void
AP_GPS::lock_port(uint8_t instance, bool lock)
{
if (instance >= GPS_MAX_INSTANCES) {
return;
}
@ -451,6 +443,35 @@ AP_GPS::lock_port(uint8_t instance, bool lock)
}
}
//Inject a packet of raw binary to a GPS
void
AP_GPS::inject_data(uint8_t *data, uint8_t len)
{
#if GPS_MAX_INSTANCES > 1
//Support broadcasting to all GPSes.
if (_inject_to == 127) {
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
inject_data(i, data, len);
}
} else {
inject_data(_inject_to, data, len);
}
#else
inject_data(0,data,len);
#endif
}
void
AP_GPS::inject_data(uint8_t instance, uint8_t *data, uint8_t len)
{
if (instance < GPS_MAX_INSTANCES && drivers[instance] != NULL)
drivers[instance]->inject_data(data, len);
}
void
AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
{

14
libraries/AP_GPS/AP_GPS.h
View File

@ -74,14 +74,6 @@ public:
/// more) to process incoming data.
void update(void);
//True if any of the underlying GPS Drivers are ready to enter
//a dgps-based fix beyond 3D lock, such as RTK mode.
bool can_calculate_base_pos(void);
//Allows the underlying GPS Drivers to enter a differential lock
//Might cause a position jump, thus only do this on the ground.
void calculate_base_pos(void);
// GPS driver types
enum GPS_Type {
GPS_TYPE_NONE = 0,
@ -336,6 +328,8 @@ public:
#if GPS_MAX_INSTANCES > 1
AP_Int8 _auto_switch;
AP_Int8 _min_dgps;
AP_Int16 _sbp_logmask;
AP_Int8 _inject_to;
#endif
AP_Int8 _sbas_mode;
AP_Int8 _min_elevation;
@ -347,6 +341,10 @@ public:
// lock out a GPS port, allowing another application to use the port
void lock_port(uint8_t instance, bool locked);
//Inject a packet of raw binary to a GPS
void inject_data(uint8_t *data, uint8_t len);
void inject_data(uint8_t instance, uint8_t *data, uint8_t len);
//MAVLink Status Sending
void send_mavlink_gps_raw(mavlink_channel_t chan);
#if GPS_MAX_INSTANCES > 1

864
libraries/AP_GPS/AP_GPS_SBP.cpp
View File

File diff suppressed because it is too large Load Diff

169
libraries/AP_GPS/AP_GPS_SBP.h
View File

@ -18,56 +18,28 @@
// Swift Navigation SBP GPS driver for ArduPilot.
// Code by Niels Joubert
//
// Swift Binary Protocol format: http://docs.swift-nav.com/libswiftnav
// Swift Binary Protocol format: http://docs.swift-nav.com/
//
#ifndef __AP_GPS_SBP_H__
#define __AP_GPS_SBP_H__
#if GPS_RTK_AVAILABLE
#include <AP_GPS.h>
//OVERALL DESIGN NOTES.
// Niels Joubert, April 2014
//
//
// REQUIREMENTS:
// 1) We need to update the entire state structure "atomically",
// which is indicated by returning "true" from read().
//
// - We use sticky bits to track when each part is updated
// and return true when all the sticky bits are set
//
// 2) We want to minimize memory usage in the detection routine
//
// - We use a stripped-down version of the sbp_parser_state_t struct
// that does not bother to decode the message, it just tracks the CRC.
//
class AP_GPS_SBP : public AP_GPS_Backend
{
public:
AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
bool can_calculate_base_pos(void);
void calculate_base_pos(void);
void invalidate_base_pos(void);
AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_RTK; }
// Methods
bool read();
void inject_data(uint8_t *data, uint8_t len);
static bool _detect(struct SBP_detect_state &state, uint8_t data);
virtual void send_mavlink_gps_rtk(mavlink_channel_t chan);
#if GPS_MAX_INSTANCES > 1
virtual void send_mavlink_gps2_rtk(mavlink_channel_t chan);
#endif
private:
// ************************************************************************
@ -107,13 +79,14 @@ private:
static const uint16_t SBP_TRACKING_STATE_MSGTYPE = 0x0016;
static const uint16_t SBP_IAR_STATE_MSGTYPE = 0x0019;
// GPS Time
struct PACKED sbp_gps_time_t {
uint16_t wn; //< GPS week number (unit: weeks)
uint32_t tow; //< GPS Time of Week rounded to the nearest ms (unit: ms)
int32_t ns; //< Nanosecond remainder of rounded tow (unit: ns)
uint8_t flags; //< Status flags (reserved)
};
}; // 11 bytes
// Dilution of Precision
struct PACKED sbp_dops_t {
@ -123,18 +96,7 @@ private:
uint16_t tdop; //< Time Dilution of Precision (unit: 0.01)
uint16_t hdop; //< Horizontal Dilution of Precision (unit: 0.01)
uint16_t vdop; //< Vertical Dilution of Precision (unit: 0.01)
};
// Position solution in absolute Earth Centered Earth Fixed (ECEF) coordinates.
struct PACKED sbp_pos_ecef_t {
uint32_t tow; //< GPS Time of Week (unit: ms)
double x; //< ECEF X coordinate (unit: meters)
double y; //< ECEF Y coordinate (unit: meters)
double z; //< ECEF Z coordinate (unit: meters)
uint16_t accuracy; //< Position accuracy estimate (unit: mm)
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags
};
}; // 14 bytes
// Geodetic position solution.
struct PACKED sbp_pos_llh_t {
@ -146,41 +108,7 @@ private:
uint16_t v_accuracy; //< Vertical position accuracy estimate (unit: mm)
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags
};
// Baseline in Earth Centered Earth Fixed (ECEF) coordinates.
struct PACKED sbp_baseline_ecef_t {
uint32_t tow; //< GPS Time of Week (unit: ms)
int32_t x; //< Baseline ECEF X coordinate (unit: mm)
int32_t y; //< Baseline ECEF Y coordinate (unit: mm)
int32_t z; //< Baseline ECEF Z coordinate (unit: mm)
uint16_t accuracy; //< Position accuracy estimate (unit: mm)
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags (reserved)
};
// Baseline in local North East Down (NED) coordinates.
struct PACKED sbp_baseline_ned_t {
uint32_t tow; //< GPS Time of Week (unit: ms)
int32_t n; //< Baseline North coordinate (unit: mm)
int32_t e; //< Baseline East coordinate (unit: mm)
int32_t d; //< Baseline Down coordinate (unit: mm)
uint16_t h_accuracy; //< Horizontal position accuracy estimate (unit: mm)
uint16_t v_accuracy; //< Vertical position accuracy estimate (unit: mm)
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags (reserved)
};
//Velocity in Earth Centered Earth Fixed (ECEF) coordinates.
struct PACKED sbp_vel_ecef_t {
uint32_t tow; //< GPS Time of Week (unit: ms)
int32_t x; //< Velocity ECEF X coordinate (unit: mm/s)
int32_t y; //< Velocity ECEF Y coordinate (unit: mm/s)
int32_t z; //< Velocity ECEF Z coordinate (unit: mm/s)
uint16_t accuracy; //< Velocity accuracy estimate (unit: mm/s)
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags (reserved)
};
}; // 34 bytes
// Velocity in NED Velocity in local North East Down (NED) coordinates.
struct PACKED sbp_vel_ned_t {
@ -192,7 +120,7 @@ private:
uint16_t v_accuracy; //< Vertical velocity accuracy estimate (unit: mm/s)
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags (reserved)
};
}; // 22 bytes
// Activity and Signal-to-Noise data of a tracking channel on the GPS.
struct PACKED sbp_tracking_state_t {
@ -206,74 +134,31 @@ private:
uint32_t num_hypotheses;
};
void _sbp_process();
void _sbp_process_message();
bool _attempt_state_update();
// ************************************************************************
// Swift Navigation SBP protocol parsing and processing
// Internal Received Messages State
// ************************************************************************
//Pulls data from the port, dispatches messages to processing functions
//Returns true if a new message was successfully decoded.
bool sbp_process();
bool update_state(bool has_new_message);
void update_state_velocity(void);
//Processes individual messages
//When a message is received, it sets a sticky bit that it has updated
//itself. This is used to track when a full update of GPS_State has occurred
void sbp_process_heartbeat(uint8_t* msg);
void sbp_process_gpstime(uint8_t* msg);
void sbp_process_dops(uint8_t* msg);
void sbp_process_pos_ecef(uint8_t* msg);
void sbp_process_pos_llh(uint8_t* msg);
void sbp_process_baseline_ecef(uint8_t* msg);
void sbp_process_baseline_ned(uint8_t* msg);
void sbp_process_vel_ecef(uint8_t* msg);
void sbp_process_vel_ned(uint8_t* msg);
void sbp_process_tracking_state(uint8_t* msg, uint8_t len);
void sbp_process_iar_state(uint8_t* msg);
void sbp_process_startup(uint8_t* msg);
//Internal last-received-messages
sbp_pos_llh_t last_sbp_pos_llh_msg;
sbp_vel_ned_t last_sbp_vel_ned_msg;
sbp_baseline_ecef_t last_sbp_baseline_ecef_msg;
sbp_baseline_ned_t last_sbp_baseline_ned_msg;
sbp_tracking_state_t last_sbp_tracking_state_msg;
uint8_t last_sbp_tracking_state_msg_num;
//Tracking GPS health and received time-of-week
uint32_t last_baseline_received_ms;
uint32_t last_heatbeat_received_ms;
uint32_t last_tracking_state_ms;
int32_t iar_num_hypotheses;
uint8_t baseline_recv_rate; //in hertz * 10
uint32_t last_injected_data_ms;
//Sticky bits to track updating of state
bool dgps_corrections_incoming:1;
bool rtk_corrections_incoming:1;
struct sbp_gps_time_t last_gps_time;
struct sbp_dops_t last_dops;
struct sbp_pos_llh_t last_pos_llh_spp;
struct sbp_pos_llh_t last_pos_llh_rtk;
struct sbp_vel_ned_t last_vel_ned;
uint32_t last_iar_num_hypotheses;
bool has_new_pos_llh:1;
bool has_new_vel_ned:1;
bool has_new_baseline_ecef:1;
//RTK-specific relative-to-absolute positioning
bool has_rtk_base_pos:1;
Vector3d base_pos_ecef;
uint32_t last_full_update_tow;
uint32_t last_full_update_cpu_ms;
// ************************************************************************
// Monitoring and Performance Counting
// ************************************************************************
uint8_t pos_msg_counter;
uint8_t vel_msg_counter;
uint8_t baseline_msg_counter;
uint8_t full_update_counter;
uint32_t crc_error_counter;
uint32_t last_healthcheck_millis;
// ************************************************************************
// Logging to DataFlash
@ -283,14 +168,10 @@ private:
static bool logging_started;
void logging_write_headers();
void logging_log_full_update();
void logging_log_raw_sbp(uint16_t msg_type, uint16_t sender_id, uint8_t msg_len, uint8_t *msg_buff);
void logging_log_health(float pos_msg_hz, float vel_msg_hz, float baseline_msg_hz, float full_update_hz);
void logging_log_llh(struct sbp_pos_llh_t* p);
void logging_log_baseline_ecef(struct sbp_baseline_ecef_t*);
void logging_log_tracking_state(struct sbp_tracking_state_t*, uint8_t num);
};
#endif // GPS_RTK_AVAILABLE
#endif // __AP_GPS_SBP_H__

11
libraries/AP_GPS/GPS_Backend.h
View File

@ -37,16 +37,9 @@ public:
// valid packet from the GPS.
virtual bool read() = 0;
virtual void inject_data(uint8_t *data, uint8_t len) { return; }
#if GPS_RTK_AVAILABLE
// true once an RTK GPS Driver has a converged baseline vector and
// absolute single point solution to enter into an RTK Fix.
virtual bool can_calculate_base_pos(void) { return false; };
// tells a RTK GPS Driver to capture the current single-point solution
// and baseline solution as the current home data.
virtual void calculate_base_pos(void) {};
// Highest status supported by this GPS.
// Allows external system to identify type of receiver connected.
virtual AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D; }