From cce46cf0c5174038d9a1981ae22433a3c86c4b33 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Wed, 2 Sep 2015 17:13:38 +0800 Subject: [PATCH] AP_GPS_SBF: add support for Septentrio gps --- libraries/AP_GPS/AP_GPS.cpp | 6 +- libraries/AP_GPS/AP_GPS.h | 4 +- libraries/AP_GPS/AP_GPS_SBF.cpp | 295 ++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_SBF.h | 133 +++++++++++++ libraries/AP_GPS/GPS_detect_state.h | 1 + 5 files changed, 437 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_GPS/AP_GPS_SBF.cpp create mode 100644 libraries/AP_GPS/AP_GPS_SBF.h diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 0afd5ea2ce..8de465a535 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -145,7 +145,7 @@ const uint32_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 115200U, 57600U, 9 // initialisation blobs to send to the GPS to try to get it into the // right mode -const prog_char AP_GPS::_initialisation_blob[] PROGMEM = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY; +const prog_char AP_GPS::_initialisation_blob[] PROGMEM = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY ; const prog_char AP_GPS::_initialisation_raw_blob[] PROGMEM = UBLOX_SET_BINARY_RAW_BAUD MTK_SET_BINARY SIRF_SET_BINARY; /* @@ -273,6 +273,10 @@ AP_GPS::detect_instance(uint8_t instance) hal.console->print_P(PSTR(" SBP ")); new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]); } + else if (_type[instance] == GPS_TYPE_SBF) { + hal.console->print_P(PSTR(" SBF ")); + new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]); + } #endif // HAL_CPU_CLASS #if !defined(GPS_SKIP_SIRF_NMEA) // save a bit of code space on a 1280 diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 7ab6a46137..ad6e525d82 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -88,7 +88,8 @@ public: GPS_TYPE_SIRF = 6, GPS_TYPE_HIL = 7, GPS_TYPE_SBP = 8, - GPS_TYPE_PX4 = 9 + GPS_TYPE_PX4 = 9, + GPS_TYPE_SBF = 10, }; /// GPS status codes @@ -424,5 +425,6 @@ private: #include "AP_GPS_SIRF.h" #include "AP_GPS_SBP.h" #include "AP_GPS_PX4.h" +#include "AP_GPS_SBF.h" #endif // __AP_GPS_H__ diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp new file mode 100644 index 0000000000..4a8d953e54 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -0,0 +1,295 @@ +// -*- 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 . + */ + +// +// Septentrio GPS driver for ArduPilot. +// Code by Michael Oborne +// + +#include "AP_GPS.h" +#include "AP_GPS_SBF.h" +#include + +#if GPS_RTK_AVAILABLE + +extern const AP_HAL::HAL& hal; + +#define SBF_DEBUGGING 0 + +#if SBF_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 + +AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, + AP_HAL::UARTDriver *_port) : + AP_GPS_Backend(_gps, _state, _port) +{ + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; +} + +// Process all bytes available from the stream +// +bool +AP_GPS_SBF::read(void) +{ + uint32_t now = hal.scheduler->millis(); + + if (_init_blob_index < (sizeof(_initialisation_blob) / sizeof(_initialisation_blob[0]))) { + if (now > _init_blob_time) { + port->write((const uint8_t*)_initialisation_blob[_init_blob_index], strlen(_initialisation_blob[_init_blob_index])); + _init_blob_time = now + 70; + _init_blob_index++; + } + } + + bool ret = false; + while (port->available() > 0) { + uint8_t temp = port->read(); + ret |= parse(temp); + } + + return ret; +} + +bool +AP_GPS_SBF::parse(uint8_t temp) +{ + switch (sbf_msg.sbf_state) + { + default: + case sbf_msg_parser_t::PREAMBLE1: + if (temp == SBF_PREAMBLE1) { + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE2; + sbf_msg.read = 0; + } + break; + case sbf_msg_parser_t::PREAMBLE2: + if (temp == SBF_PREAMBLE2) { + sbf_msg.sbf_state = sbf_msg_parser_t::CRC1; + } + else + { + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; + } + break; + case sbf_msg_parser_t::CRC1: + sbf_msg.crc = temp; + sbf_msg.sbf_state = sbf_msg_parser_t::CRC2; + break; + case sbf_msg_parser_t::CRC2: + sbf_msg.crc += (uint16_t)(temp << 8); + sbf_msg.sbf_state = sbf_msg_parser_t::BLOCKID1; + break; + case sbf_msg_parser_t::BLOCKID1: + sbf_msg.blockid = temp; + sbf_msg.sbf_state = sbf_msg_parser_t::BLOCKID2; + break; + case sbf_msg_parser_t::BLOCKID2: + sbf_msg.blockid += (uint16_t)(temp << 8); + sbf_msg.sbf_state = sbf_msg_parser_t::LENGTH1; + break; + case sbf_msg_parser_t::LENGTH1: + sbf_msg.length = temp; + sbf_msg.sbf_state = sbf_msg_parser_t::LENGTH2; + break; + case sbf_msg_parser_t::LENGTH2: + sbf_msg.length += (uint16_t)(temp << 8); + sbf_msg.sbf_state = sbf_msg_parser_t::DATA; + if (sbf_msg.length % 4 != 0) { + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; + Debug("bad packet length=%u\n", (unsigned)sbf_msg.length); + } + break; + case sbf_msg_parser_t::DATA: + if (sbf_msg.read >= sizeof(sbf_msg.data)) { + Debug("parse overflow length=%u\n", (unsigned)sbf_msg.read); + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; + break; + } + sbf_msg.data.bytes[sbf_msg.read] = temp; + sbf_msg.read++; + if (sbf_msg.read >= (sbf_msg.length - 8)) { + uint16_t crc = crc16_ccitt((uint8_t*)&sbf_msg.blockid, 2, 0); + crc = crc16_ccitt((uint8_t*)&sbf_msg.length, 2, crc); + crc = crc16_ccitt((uint8_t*)&sbf_msg.data, sbf_msg.length - 8, crc); + + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; + + if (sbf_msg.crc == crc) { + return process_message(); + } else { + Debug("crc fail\n"); + crc_error_counter++; + } + } + break; + } + + return false; +} + +void +AP_GPS_SBF::log_ExtEventPVTGeodetic(const msg4007 &temp) +{ + if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { + return; + } + + uint64_t now = hal.scheduler->micros64(); + + struct log_GPS_SBF_EVENT header = { + LOG_PACKET_HEADER_INIT(LOG_GPS_SBF_EVENT_MSG), + time_us:now, + TOW:temp.TOW, + WNc:temp.WNc, + Mode:temp.Mode, + Error:temp.Error, + Latitude:ToDeg(temp.Latitude), + Longitude:ToDeg(temp.Longitude), + Height:temp.Height, + Undulation:temp.Undulation, + Vn:temp.Vn, + Ve:temp.Ve, + Vu:temp.Vu, + COG:temp.COG + }; + + gps._DataFlash->WriteBlock(&header, sizeof(header)); +} + +bool +AP_GPS_SBF::process_message(void) +{ + uint16_t blockid = (sbf_msg.blockid & 4095u); + + Debug("BlockID %d", blockid); + + // ExtEventPVTGeodetic + if (blockid == 4038) { + log_ExtEventPVTGeodetic(sbf_msg.data.msg4007u); + } + // PVTGeodetic + if (blockid == 4007) { + const msg4007 &temp = sbf_msg.data.msg4007u; + + // Update time state + if (temp.WNc != 65535) { + state.time_week = temp.WNc; + state.time_week_ms = (uint32_t)(temp.TOW); + } + + state.last_gps_time_ms = hal.scheduler->millis(); + + state.hdop = last_hdop; + + // Update velocity state (dont use −2·10^10) + if (temp.Vn > -200000) { + state.velocity.x = (float)(temp.Vn); + state.velocity.y = (float)(temp.Ve); + state.velocity.z = (float)(-temp.Vu); + + state.have_vertical_velocity = true; + + float ground_vector_sq = state.velocity[0] * state.velocity[0] + state.velocity[1] * state.velocity[1]; + state.ground_speed = (float)safe_sqrt(ground_vector_sq); + + state.ground_course_cd = (int32_t)(100 * ToDeg(atan2f(state.velocity[1], state.velocity[0]))); + state.ground_course_cd = wrap_360_cd(state.ground_course_cd); + } + + // Update position state (dont use −2·10^10) + if (temp.Latitude > -200000) { + state.location.lat = (int32_t)(temp.Latitude * 57.295779513 * 1e7); + state.location.lng = (int32_t)(temp.Longitude * 57.295779513 * 1e7); + state.location.alt = (int32_t)((float)temp.Height * 1e2f); + } + + if (temp.NrSV != 255) { + state.num_sats = temp.NrSV; + } + + Debug("temp.Mode=0x%02x\n", (unsigned)temp.Mode); + switch (temp.Mode & 15) { + case 0: // no pvt + state.status = AP_GPS::NO_FIX; + break; + case 1: // standalone + state.status = AP_GPS::GPS_OK_FIX_3D; + break; + case 2: // dgps + state.status = AP_GPS::GPS_OK_FIX_3D_DGPS; + break; + case 3: // fixed location + state.status = AP_GPS::GPS_OK_FIX_3D; + break; + case 4: // rtk fixed + state.status = AP_GPS::GPS_OK_FIX_3D_RTK; + break; + case 5: // rtk float + state.status = AP_GPS::GPS_OK_FIX_3D_DGPS; + break; + case 6: // sbas + state.status = AP_GPS::GPS_OK_FIX_3D; + break; + case 7: // moving rtk fixed + state.status = AP_GPS::GPS_OK_FIX_3D_RTK; + break; + case 8: // moving rtk float + state.status = AP_GPS::GPS_OK_FIX_3D_DGPS; + break; + } + + if ((temp.Mode & 64) > 0) // gps is in base mode + state.status = AP_GPS::NO_FIX; + if ((temp.Mode & 128) > 0) // gps only has 2d fix + state.status = AP_GPS::GPS_OK_FIX_2D; + + return true; + } + // DOP + if (blockid == 4001) { + const msg4001 &temp = sbf_msg.data.msg4001u; + + last_hdop = temp.HDOP; + + state.hdop = last_hdop; + } + + return false; +} + +void +AP_GPS_SBF::inject_data(uint8_t *data, uint8_t len) +{ + + if (port->txspace() > len) { + last_injected_data_ms = hal.scheduler->millis(); + port->write(data, len); + } else { + Debug("SBF: Not enough TXSPACE"); + } +} + + +#endif // GPS_RTK_AVAILABLE diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h new file mode 100644 index 0000000000..499bdaa5b6 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -0,0 +1,133 @@ +// -*- 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 . + */ + +// +// Septentrio GPS driver for ArduPilot. +// Code by Michael Oborne +// + +#ifndef __AP_GPS_SBF_H__ +#define __AP_GPS_SBF_H__ + +#include "AP_GPS.h" + +#define SBF_SETUP_MSG "\nsso, Stream1, COM1, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n" + +class AP_GPS_SBF : public AP_GPS_Backend +{ +public: + AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); + + 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); + +private: + + bool parse(uint8_t temp); + bool process_message(); + + static const uint8_t SBF_PREAMBLE1 = '$'; + static const uint8_t SBF_PREAMBLE2 = '@'; + + uint8_t _init_blob_index = 0; + uint32_t _init_blob_time = 0; + const char* _initialisation_blob[4] = { + "sso, Stream1, COM1, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n", + "srd, Moderate, UAV\n", + "sem, PVT, 5\n", + "spm, Rover, StandAlone+DGPS+RTK\n"}; + + uint32_t last_hdop = 999; + uint32_t crc_error_counter = 0; + uint32_t last_injected_data_ms = 0; + + struct PACKED msg4007 + { + uint32_t TOW; + uint16_t WNc; + uint8_t Mode; + uint8_t Error; + double Latitude; + double Longitude; + double Height; + float Undulation; + float Vn; + float Ve; + float Vu; + float COG; + double RxClkBias; + float RxClkDrift; + uint8_t TimeSystem; + uint8_t Datum; + uint8_t NrSV; + uint8_t WACorrInfo; + uint16_t ReferenceID; + uint16_t MeanCorrAge; + uint32_t SignalInfo; + uint8_t AlertFlag; + }; + + struct PACKED msg4001 + { + uint32_t TOW; + uint16_t WNc; + uint8_t NrSV; + uint8_t Reserved; + uint16_t PDOP; + uint16_t TDOP; + uint16_t HDOP; + uint16_t VDOP; + float HPL; + float VPL; + }; + + union PACKED msgbuffer { + msg4007 msg4007u; + msg4001 msg4001u; + uint8_t bytes[]; + }; + + struct sbf_msg_parser_t + { + enum + { + PREAMBLE1 = 0, + PREAMBLE2, + CRC1, + CRC2, + BLOCKID1, + BLOCKID2, + LENGTH1, + LENGTH2, + DATA + } sbf_state; + uint16_t preamble; + uint16_t crc; + uint16_t blockid; + uint16_t length; + msgbuffer data; + uint16_t read; + } sbf_msg; + + void log_ExtEventPVTGeodetic(const msg4007 &temp); +}; + +#endif // __AP_GPS_SBF_H__ + diff --git a/libraries/AP_GPS/GPS_detect_state.h b/libraries/AP_GPS/GPS_detect_state.h index 5f0214c4ed..4678fea52c 100644 --- a/libraries/AP_GPS/GPS_detect_state.h +++ b/libraries/AP_GPS/GPS_detect_state.h @@ -68,3 +68,4 @@ struct SBP_detect_state { uint16_t crc_so_far; uint16_t crc; }; +