mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-27 02:58:31 -04:00
7d2b8b6bde
Unsure what the underlying problem is, but the length of the first string in the initilisation_blob increasing resulted in a race condition, waiting longer before retrying the message resolves it, but we still need to identify the underlying problem. This patch just results in the GPS working with current configurations. Tested against AsteRx-M firmware 3.6.3
316 lines
10 KiB
C++
316 lines
10 KiB
C++
/*
|
||
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>
|
||
#include <GCS_MAVLink/GCS.h>
|
||
|
||
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;
|
||
|
||
port->write((const uint8_t*)_initialisation_blob[0], strlen(_initialisation_blob[0]));
|
||
}
|
||
|
||
// Process all bytes available from the stream
|
||
//
|
||
bool
|
||
AP_GPS_SBF::read(void)
|
||
{
|
||
uint32_t now = AP_HAL::millis();
|
||
|
||
if (_init_blob_index < (sizeof(_initialisation_blob) / sizeof(_initialisation_blob[0]))) {
|
||
const char *init_str = _initialisation_blob[_init_blob_index];
|
||
if (validcommand) {
|
||
_init_blob_index++;
|
||
validcommand = false;
|
||
_init_blob_time = 0;
|
||
}
|
||
|
||
if (now > _init_blob_time) {
|
||
port->write((const uint8_t*)init_str, strlen(init_str));
|
||
// if this is too low a race condition on start occurs and the GPS isn't detected
|
||
_init_blob_time = now + 2000;
|
||
}
|
||
}
|
||
|
||
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 if (temp == 'R') {
|
||
validcommand = true;
|
||
sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
|
||
}
|
||
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 == nullptr || !gps._DataFlash->logging_started()) {
|
||
return;
|
||
}
|
||
|
||
uint64_t now = AP_HAL::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
|
||
else 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 = AP_HAL::millis();
|
||
|
||
state.hdop = last_hdop;
|
||
|
||
// Update velocity state (don't 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 = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
|
||
|
||
// value is expressed as twice the rms error = int16 * 0.01/2
|
||
state.horizontal_accuracy = (float)temp.HAccuracy * 0.005f;
|
||
state.vertical_accuracy = (float)temp.VAccuracy * 0.005f;
|
||
state.have_horizontal_accuracy = true;
|
||
state.have_vertical_accuracy = true;
|
||
}
|
||
|
||
// Update position state (don't use −2·10^10)
|
||
if (temp.Latitude > -200000) {
|
||
state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * (double)1e7);
|
||
state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * (double)1e7);
|
||
state.location.alt = (int32_t)(((float)temp.Height - temp.Undulation) * 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_FIXED;
|
||
break;
|
||
case 5: // rtk float
|
||
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
|
||
break;
|
||
case 6: // sbas
|
||
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
|
||
break;
|
||
case 7: // moving rtk fixed
|
||
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
|
||
break;
|
||
case 8: // moving rtk float
|
||
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
|
||
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
|
||
else if (blockid == 4001) {
|
||
const msg4001 &temp = sbf_msg.data.msg4001u;
|
||
|
||
last_hdop = temp.HDOP;
|
||
|
||
state.hdop = last_hdop;
|
||
}
|
||
// ReceiverStatus
|
||
else if (blockid == 4014) {
|
||
const msg4014 &temp = sbf_msg.data.msg4014u;
|
||
RxState = temp.RxState;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
|
||
{
|
||
if (gps._raw_data) {
|
||
if (!(RxState & SBF_DISK_MOUNTED)){
|
||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS %d: SBF disk is not mounted", state.instance + 1);
|
||
}
|
||
else if (RxState & SBF_DISK_FULL) {
|
||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS %d: SBF disk is full", state.instance + 1);
|
||
}
|
||
else if (!(RxState & SBF_DISK_ACTIVITY)) {
|
||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS %d: SBF is not currently logging", state.instance + 1);
|
||
}
|
||
}
|
||
}
|