diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 28a223122a..4ec3df2a6f 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -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) && diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 33cb8b9592..c434fd993a 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -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 #include #include +#include #endif // __AP_GPS_H__ diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp new file mode 100644 index 0000000000..d63f31312b --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_SBP.cpp @@ -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 . + */ + +// +// Swift Navigation GPS driver for ArduPilot +// Origin code by Niels Joubert njoubert.com +// +#include +#include "AP_GPS_SBP.h" +#include + +#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 diff --git a/libraries/AP_GPS/AP_GPS_SBP.h b/libraries/AP_GPS/AP_GPS_SBP.h new file mode 100644 index 0000000000..f2884ff693 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_SBP.h @@ -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 . + */ + +// +// 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 + + +//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__ diff --git a/libraries/AP_GPS/GPS_detect_state.h b/libraries/AP_GPS/GPS_detect_state.h index 8875211e63..5f0214c4ed 100644 --- a/libraries/AP_GPS/GPS_detect_state.h +++ b/libraries/AP_GPS/GPS_detect_state.h @@ -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; +};