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

AP_GPS: Swift Binary Protocol SINGLE-POINT-POSITIONING Driver

Browse Source 
This is the SBP driver for Piksi, Lodestar and other forthcoming Swift Navigation GPSes.
This driver currently implements three things:
- Implements a lightweight SBP protocol detection system
- Implements Piksi as a single-point-positioning GPS (same as ublox/others)
- Implements hardware logging of GPS health and baseline messages

Forthcoming iin future updates:
- Need to implement the RTK functionality
This commit is contained in:
Niels Joubert 2014-04-04 16:05:54 -07:00 committed by Andrew Tridgell
parent e32b73f075
commit 33576dfdd9
5 changed files with 749 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_GPS/AP_GPS.cpp
View File

@ -27,14 +27,14 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
// @Param: TYPE
// @DisplayName: GPS type
// @Description: GPS type
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftBinaryProtocol
AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], 1),
#if GPS_MAX_INSTANCES > 1
// @Param: TYPE2
// @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftBinaryProtocol
AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0),
#endif
@ -60,7 +60,7 @@ void AP_GPS::init(DataFlash_Class *dataflash)
}
// baudrates to try to detect GPSes with
const uint16_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 57600U, 9600U};
const uint32_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 115200U, 57600U, 9600U};
// initialisation blobs to send to the GPS to try to get it into the
// right mode
@ -132,8 +132,9 @@ AP_GPS::detect_instance(uint8_t instance)
if (dstate->last_baud == sizeof(_baudrates) / sizeof(_baudrates[0])) {
dstate->last_baud = 0;
}
uint16_t baudrate = pgm_read_word(&_baudrates[dstate->last_baud]);
uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]);
port->begin(baudrate, 256, 16);
Debug("Switching to GPS Baudrate %d", baudrate);
dstate->last_baud_change_ms = now;
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
@ -150,7 +151,7 @@ AP_GPS::detect_instance(uint8_t instance)
for.
*/
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) &&
pgm_read_word(&_baudrates[dstate->last_baud]) >= 38400 &&
pgm_read_dword(&_baudrates[dstate->last_baud]) >= 38400 &&
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
hal.console->print_P(PSTR(" ublox "));
new_gps = new AP_GPS_UBLOX(*this, state[instance], port);
@ -165,6 +166,11 @@ AP_GPS::detect_instance(uint8_t instance)
hal.console->print_P(PSTR(" MTK "));
new_gps = new AP_GPS_MTK(*this, state[instance], port);
}
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
hal.console->print_P(PSTR(" SBP "));
new_gps = new AP_GPS_SBP(*this, state[instance], port);
}
#if !defined( __AVR_ATmega1280__ )
// save a bit of code space on a 1280
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&

14
libraries/AP_GPS/AP_GPS.h
View File

@ -35,6 +35,13 @@
#define GPS_MAX_INSTANCES 1
#endif
#define GPS_DEBUGGING 0
#if GPS_DEBUGGING
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
#else
# define Debug(fmt, args ...)
#endif
class DataFlash_Class;
class AP_GPS_Backend;
@ -65,7 +72,8 @@ public:
GPS_TYPE_MTK19 = 4,
GPS_TYPE_NMEA = 5,
GPS_TYPE_SIRF = 6,
GPS_TYPE_HIL = 7
GPS_TYPE_HIL = 7,
GPS_TYPE_SBP = 8
};
/// GPS status codes
@ -294,6 +302,7 @@ private:
struct MTK19_detect_state mtk19_detect_state;
struct SIRF_detect_state sirf_detect_state;
struct NMEA_detect_state nmea_detect_state;
struct SBP_detect_state sbp_detect_state;
} detect_state[GPS_MAX_INSTANCES];
struct {
@ -301,7 +310,7 @@ private:
uint16_t remaining;
} initblob_state[GPS_MAX_INSTANCES];
static const uint16_t _baudrates[];
static const uint32_t _baudrates[];
static const prog_char _initialisation_blob[];
void detect_instance(uint8_t instance);
@ -314,5 +323,6 @@ private:
#include <AP_GPS_MTK19.h>
#include <AP_GPS_NMEA.h>
#include <AP_GPS_SIRF.h>
#include <AP_GPS_SBP.h>
#endif // __AP_GPS_H__

485
libraries/AP_GPS/AP_GPS_SBP.cpp Normal file
View File

@ -0,0 +1,485 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
//
// Swift Navigation GPS driver for ArduPilot
// Origin code by Niels Joubert njoubert.com
//
#include <AP_GPS.h>
#include "AP_GPS_SBP.h"
#include <DataFlash.h>
#define SBP_DEBUGGING 0
#define SBP_FAKE_3DLOCK 0
extern const AP_HAL::HAL& hal;
#define SBP_MILLIS_BETWEEN_HEALTHCHECKS 1500
#define SBP_DEBUGGING 0
#if SBP_DEBUGGING
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
#else
# define Debug(fmt, args ...)
#endif
/*
only do detailed hardware logging on boards likely to have more log
storage space
*/
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define SBP_HW_LOGGING 1
#else
#define SBP_HW_LOGGING 0
#endif
AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port),
has_updated_pos(false),
has_updated_vel(false),
logging_started(false),
pos_msg_counter(0),
vel_msg_counter(0),
dops_msg_counter(0),
baseline_msg_counter(0),
crc_error_counter(0),
last_healthcheck_millis(0)
{
Debug("Initializing SBP Driver");
port->begin(115200, 256, 16);
port->flush();
parser_state.state = sbp_parser_state_t::WAITING;
state.status = AP_GPS::NO_FIX;
state.have_vertical_velocity = true;
}
bool
AP_GPS_SBP::read(void)
{
//First we process all data waiting for the queue.
sbp_process();
uint32_t now = hal.scheduler->millis();
uint32_t elapsed = now - last_healthcheck_millis;
if (elapsed > SBP_MILLIS_BETWEEN_HEALTHCHECKS) {
last_healthcheck_millis = now;
#if SBP_DEBUGGING || SBP_HW_LOGGING
float pos_msg_hz = pos_msg_counter / (float) elapsed * 1000.0;
float vel_msg_hz = vel_msg_counter / (float) elapsed * 1000.0;
float dops_msg_hz = dops_msg_counter / (float) elapsed * 1000.0;
float baseline_msg_hz = baseline_msg_counter / (float) elapsed * 1000.0;
float crc_error_hz = crc_error_counter / (float) elapsed * 1000.0;
pos_msg_counter = 0;
vel_msg_counter = 0;
dops_msg_counter = 0;
baseline_msg_counter = 0;
crc_error_counter = 0;
Debug("SBP GPS perf: CRC=(%.2fHz) Pos=(%.2fHz) Vel=(%.2fHz) Dops=(%.2fHz) Baseline=(%.2fHz)\n",
crc_error_hz,
pos_msg_hz,
vel_msg_hz,
dops_msg_hz,
baseline_msg_hz);
#if SBP_HW_LOGGING
logging_log_health(pos_msg_hz,
vel_msg_hz,
dops_msg_hz,
baseline_msg_hz,
crc_error_hz);
#endif
#endif
}
//Now we check whether we've done a full update - is all the sticky bits set?
if (has_updated_pos && has_updated_vel) {
state.status = AP_GPS::GPS_OK_FIX_3D;
has_updated_pos = false;
has_updated_vel = false;
return true;
}
return false;
}
//This attempts to read all the SBP messages from the incoming port.
void
AP_GPS_SBP::sbp_process()
{
while (port->available() > 0) {
uint8_t temp = port->read();
uint16_t crc;
//This switch reads one character at a time,
//parsing it into buffers until a full message is dispatched
switch(parser_state.state) {
case sbp_parser_state_t::WAITING:
if (temp == SBP_PREAMBLE) {
parser_state.n_read = 0;
parser_state.state = sbp_parser_state_t::GET_TYPE;
}
break;
case sbp_parser_state_t::GET_TYPE:
*((uint8_t*)&(parser_state.msg_type) + parser_state.n_read) = temp;
parser_state.n_read += 1;
if (parser_state.n_read >= 2) {
parser_state.n_read = 0;
parser_state.state = sbp_parser_state_t::GET_SENDER;
}
break;
case sbp_parser_state_t::GET_SENDER:
*((uint8_t*)&(parser_state.sender_id) + parser_state.n_read) = temp;
parser_state.n_read += 1;
if (parser_state.n_read >= 2) {
parser_state.n_read = 0;
parser_state.state = sbp_parser_state_t::GET_LEN;
}
break;
case sbp_parser_state_t::GET_LEN:
parser_state.msg_len = temp;
parser_state.n_read = 0;
parser_state.state = sbp_parser_state_t::GET_MSG;
break;
case sbp_parser_state_t::GET_MSG:
*((uint8_t*)&(parser_state.msg_buff) + parser_state.n_read) = temp;
parser_state.n_read += 1;
if (parser_state.n_read >= parser_state.msg_len) {
parser_state.n_read = 0;
parser_state.state = sbp_parser_state_t::GET_CRC;
}
break;
case sbp_parser_state_t::GET_CRC:
*((uint8_t*)&(parser_state.crc) + parser_state.n_read) = temp;
parser_state.n_read += 1;
if (parser_state.n_read >= 2) {
parser_state.state = sbp_parser_state_t::WAITING;
crc = crc16_ccitt((uint8_t*)&(parser_state.msg_type), 2, 0);
crc = crc16_ccitt((uint8_t*)&(parser_state.sender_id), 2, crc);
crc = crc16_ccitt(&(parser_state.msg_len), 1, crc);
crc = crc16_ccitt(parser_state.msg_buff, parser_state.msg_len, crc);
if (parser_state.crc == crc) {
//OK, we have a valid message. Dispatch the appropriate function:
switch(parser_state.msg_type) {
case SBP_GPS_TIME_MSGTYPE:
sbp_process_gpstime(parser_state.msg_buff);
break;
case SBP_DOPS_MSGTYPE:
sbp_process_dops(parser_state.msg_buff);
break;
case SBP_POS_ECEF_MSGTYPE:
sbp_process_pos_ecef(parser_state.msg_buff);
break;
case SBP_POS_LLH_MSGTYPE:
sbp_process_pos_llh(parser_state.msg_buff);
break;
case SBP_BASELINE_ECEF_MSGTYPE:
sbp_process_baseline_ecef(parser_state.msg_buff);
break;
case SBP_BASELINE_NED_MSGTYPE:
sbp_process_baseline_ned(parser_state.msg_buff);
break;
case SBP_VEL_ECEF_MSGTYPE:
sbp_process_vel_ecef(parser_state.msg_buff);
break;
case SBP_VEL_NED_MSGTYPE:
sbp_process_vel_ned(parser_state.msg_buff);
break;
default:
Debug("Unknown message received: msg_type=0x%x", parser_state.msg_type);
}
} else {
Debug("CRC Error Occurred!\n");
crc_error_counter += 1;
}
}
break;
default:
parser_state.state = sbp_parser_state_t::WAITING;
break;
}
}
//We have parsed all the waiting messages
return;
}
void
AP_GPS_SBP::sbp_process_gpstime(uint8_t* msg)
{
struct sbp_gps_time_t* t = (struct sbp_gps_time_t*)msg;
state.time_week = t->wn;
state.time_week_ms = t->tow;
state.last_gps_time_ms = hal.scheduler->millis();
}
void
AP_GPS_SBP::sbp_process_dops(uint8_t* msg)
{
struct sbp_dops_t* d = (struct sbp_dops_t*) msg;
state.time_week_ms = d->tow;
state.last_gps_time_ms = hal.scheduler->millis();
state.hdop = d->hdop;
dops_msg_counter += 1;
}
void
AP_GPS_SBP::sbp_process_pos_ecef(uint8_t* msg)
{
//Ideally we'd like this data in LLH format, not ECEF
}
void
AP_GPS_SBP::sbp_process_pos_llh(uint8_t* msg)
{
struct sbp_pos_llh_t* pos = (struct sbp_pos_llh_t*)msg;
state.time_week_ms = pos->tow;
state.last_gps_time_ms = hal.scheduler->millis();
state.location.lat = (int32_t) (pos->lat*1e7);
state.location.lng = (int32_t) (pos->lon*1e7);
state.location.alt = (int32_t) (pos->height*1e2);
state.num_sats = pos->n_sats;
pos_msg_counter += 1;
has_updated_pos = true;
}
void
AP_GPS_SBP::sbp_process_baseline_ecef(uint8_t* msg)
{
struct sbp_baseline_ecef_t* b = (struct sbp_baseline_ecef_t*)msg;
baseline_msg_counter += 1;
#if SBP_HW_LOGGING
logging_log_baseline(b);
#endif
}
void
AP_GPS_SBP::sbp_process_baseline_ned(uint8_t* msg)
{
//Ideally we'd like this data in ECEF format, not NED
}
void
AP_GPS_SBP::sbp_process_vel_ecef(uint8_t* msg)
{
//Ideally we'd like this data in NED format, not ECEF
}
void
AP_GPS_SBP::sbp_process_vel_ned(uint8_t* msg)
{
struct sbp_vel_ned_t* vel = (struct sbp_vel_ned_t*)msg;
state.time_week_ms = vel->tow;
state.last_gps_time_ms = hal.scheduler->millis();
state.velocity[0] = (float)vel->n / 1000.0;
state.velocity[1] = (float)vel->e / 1000.0;
state.velocity[2] = (float)vel->d / 1000.0;
state.num_sats = vel->n_sats;
float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
state.ground_speed = safe_sqrt(ground_vector_sq);
state.ground_course_cd = (int32_t) 100*ToDeg(atan2f(state.velocity[1], state.velocity[0]));
if (state.ground_course_cd < 0) {
state.ground_course_cd += 36000;
}
vel_msg_counter += 1;
has_updated_vel = true;
}
bool
AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data)
{
uint16_t crc;
//This switch reads one character at a time,
//if we find something that looks like our preamble
//we'll try to read the full message length, calculating the CRC.
//If the CRC matches, we have a SBP GPS!
switch(state.state) {
case SBP_detect_state::WAITING:
if (data == SBP_PREAMBLE) {
state.n_read = 0;
state.crc_so_far = 0;
state.state = SBP_detect_state::GET_TYPE;
}
break;
case SBP_detect_state::GET_TYPE:
state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
state.n_read += 1;
if (state.n_read >= 2) {
state.n_read = 0;
state.state = SBP_detect_state::GET_SENDER;
}
break;
case SBP_detect_state::GET_SENDER:
state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
state.n_read += 1;
if (state.n_read >= 2) {
state.n_read = 0;
state.state = SBP_detect_state::GET_LEN;
}
break;
case SBP_detect_state::GET_LEN:
state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
state.msg_len = data;
state.n_read = 0;
state.state = SBP_detect_state::GET_MSG;
break;
case SBP_detect_state::GET_MSG:
state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
state.n_read += 1;
if (state.n_read >= state.msg_len) {
state.n_read = 0;
state.state = SBP_detect_state::GET_CRC;
}
break;
case SBP_detect_state::GET_CRC:
*((uint8_t*)&(state.crc) + state.n_read) = data;
state.n_read += 1;
if (state.n_read >= 2) {
state.state = SBP_detect_state::WAITING;
return state.crc == state.crc_so_far;
}
break;
default:
state.state = SBP_detect_state::WAITING;
break;
}
return false;
}
#if SBP_HW_LOGGING
#define LOG_MSG_SBPHEALTH 202
#define LOG_MSG_SBPBASELINE 203
struct PACKED log_SbpHealth {
LOG_PACKET_HEADER;
uint32_t timestamp;
float pos_msg_hz;
float vel_msg_hz;
float dops_msg_hz;
float baseline_msg_hz;
float crc_error_hz;
};
struct PACKED log_SbpBaseline {
LOG_PACKET_HEADER;
uint32_t timestamp;
uint32_t tow;
int32_t baseline_x;
int32_t baseline_y;
int32_t baseline_z;
uint16_t baseline_accuracy;
uint8_t num_sats;
uint8_t flags;
};
static const struct LogStructure sbp_log_structures[] PROGMEM = {
{ LOG_MSG_SBPHEALTH, sizeof(log_SbpHealth),
"SBPH", "Ifffff", "TimeMS,PosHz,VelHz,DopsHz,BaseHz,CrcHz" },
{ LOG_MSG_SBPBASELINE, sizeof(log_SbpBaseline),
"SBPB", "IIiiiHBB", "TimeMS,tow,bx,by,bz,bacc,num_sats,flags" }
};
void AP_GPS_SBP::logging_write_headers(void)
{
if (!logging_started) {
logging_started = true;
gps._DataFlash->AddLogFormats(sbp_log_structures, sizeof(sbp_log_structures) / sizeof(LogStructure));
}
}
void AP_GPS_SBP::logging_log_health(float pos_msg_hz, float vel_msg_hz, float dops_msg_hz, float baseline_msg_hz, float crc_error_hz)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
logging_write_headers();
struct log_SbpHealth pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH),
timestamp : hal.scheduler->millis(),
pos_msg_hz : pos_msg_hz,
vel_msg_hz : vel_msg_hz,
dops_msg_hz : dops_msg_hz,
baseline_msg_hz : baseline_msg_hz,
crc_error_hz : crc_error_hz
};
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
void AP_GPS_SBP::logging_log_baseline(struct sbp_baseline_ecef_t* b)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
logging_write_headers();
struct log_SbpBaseline pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPBASELINE),
timestamp : hal.scheduler->millis(),
tow : b->tow,
baseline_x : b->x,
baseline_y : b->y,
baseline_z : b->z,
baseline_accuracy : b->accuracy,
num_sats : b->n_sats,
flags : b->flags
};
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
#endif // SBP_HW_LOGGING

226
libraries/AP_GPS/AP_GPS_SBP.h Normal file
View File

@ -0,0 +1,226 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
//
// Swift Navigation SBP GPS driver for ArduPilot.
// Code by Niels Joubert
//
// Swift Binary Protocol format: http://docs.swift-nav.com/libswiftnav
#ifndef __AP_GPS_SBP_H__
#define __AP_GPS_SBP_H__
#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);
// Methods
bool read();
static bool _detect(struct SBP_detect_state &state, uint8_t data);
private:
// ************************************************************************
// Swift Navigation SBP protocol types and definitions
// ************************************************************************
struct sbp_parser_state_t {
enum {
WAITING = 0,
GET_TYPE = 1,
GET_SENDER = 2,
GET_LEN = 3,
GET_MSG = 4,
GET_CRC = 5
} state:8;
uint16_t msg_type;
uint16_t sender_id;
uint16_t crc;
uint8_t msg_len;
uint8_t n_read;
uint8_t msg_buff[256];
} parser_state;
static const uint8_t SBP_PREAMBLE = 0x55;
//Message types supported by this driver
static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100;
static const uint16_t SBP_DOPS_MSGTYPE = 0x0206;
static const uint16_t SBP_POS_ECEF_MSGTYPE = 0x0200;
static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201;
static const uint16_t SBP_BASELINE_ECEF_MSGTYPE = 0x0202;
static const uint16_t SBP_BASELINE_NED_MSGTYPE = 0x0203;
static const uint16_t SBP_VEL_ECEF_MSGTYPE = 0x0204;
static const uint16_t SBP_VEL_NED_MSGTYPE = 0x0205;
// 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)
};
// Dilution of Precision
struct PACKED sbp_dops_t {
uint32_t tow; //< GPS Time of Week (unit: ms)
uint16_t gdop; //< Geometric Dilution of Precision (unit: 0.01)
uint16_t pdop; //< Position Dilution of Precision (unit: 0.01)
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
};
// Geodetic position solution.
struct PACKED sbp_pos_llh_t {
uint32_t tow; //< GPS Time of Week (unit: ms)
double lat; //< Latitude (unit: degrees)
double lon; //< Longitude (unit: degrees)
double height; //< Height (unit: meters)
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
};
// 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)
};
// Velocity in NED Velocity in local North East Down (NED) coordinates.
struct PACKED sbp_vel_ned_t {
uint32_t tow; //< GPS Time of Week (unit: ms)
int32_t n; //< Velocity North coordinate (unit: mm/s)
int32_t e; //< Velocity East coordinate (unit: mm/s)
int32_t d; //< Velocity Down coordinate (unit: mm/s)
uint16_t h_accuracy; //< Horizontal velocity accuracy estimate (unit: mm/s)
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)
};
// ************************************************************************
// Swift Navigation SBP protocol parsing and processing
// ************************************************************************
//Pulls data from the port, dispatches messages to processing functions
void sbp_process();
//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_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);
//Sticky bits to track updating of state
bool has_updated_pos:1;
bool has_updated_vel:1;
// ************************************************************************
// Monitoring and Performance Counting
// ************************************************************************
uint8_t pos_msg_counter;
uint8_t vel_msg_counter;
uint8_t dops_msg_counter;
uint8_t baseline_msg_counter;
uint16_t crc_error_counter;
uint32_t last_healthcheck_millis;
// ************************************************************************
// Logging to DataFlash
// ************************************************************************
// have we written the logging headers to DataFlash?
bool logging_started:1;
void logging_write_headers();
void logging_log_health(float pos_msg_hz, float vel_msg_hz, float dops_msg_hz, float baseline_msg_hz, float crc_error_hz);
void logging_log_baseline(struct sbp_baseline_ecef_t*);
};
#endif // __AP_GPS_SBP_H__

15
libraries/AP_GPS/GPS_detect_state.h
View File

@ -53,3 +53,18 @@ struct UBLOX_detect_state {
uint8_t step;
uint8_t ck_a, ck_b;
};
struct SBP_detect_state {
enum {
WAITING = 0,
GET_TYPE = 1,
GET_SENDER = 2,
GET_LEN = 3,
GET_MSG = 4,
GET_CRC = 5
} state:8;
uint8_t n_read;
uint8_t msg_len;
uint16_t crc_so_far;
uint16_t crc;
};