mirror of https://github.com/ArduPilot/ardupilot
AP_GPS_SBF: add support for Septentrio gps
This commit is contained in:
Michael Oborne
Andrew Tridgell
parent
52577e4105
commit
cce46cf0c5
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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__
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
//
|
||||
// Septentrio GPS driver for ArduPilot.
|
||||
// Code by Michael Oborne
|
||||
//
|
||||
|
||||
#include "AP_GPS.h"
|
||||
#include "AP_GPS_SBF.h"
|
||||
#include <DataFlash/DataFlash.h>
|
||||
|
||||
#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
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
//
|
||||
// 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__
|
||||
|
||||
|
|
@ -68,3 +68,4 @@ struct SBP_detect_state {
|
|||
uint16_t crc_so_far;
|
||||
uint16_t crc;
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue