Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Support configuring the SBF com port

This commit is contained in:
Michael du Breuil 2020-06-13 13:23:44 -07:00 committed by Peter Barker
parent 3c6d1a359a
commit 08d4e1c062
4 changed files with 73 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_GPS/AP_GPS.cpp
View File

@ -299,6 +299,26 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
AP_GROUPINFO("DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
#endif
// @Param: COM_PORT
// @DisplayName: GPS physical COM port
// @Description: The physical COM port on the connected device, currently only applies to SBF GPS
// @Range: 0 10
// @Increment: 1
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("COM_PORT", 23, AP_GPS, _com_port[0], 1),
#if GPS_MAX_RECEIVERS > 1
// @Param: COM_PORT2
// @DisplayName: GPS physical COM port
// @Description: The physical COM port on the connected device, currently only applies to SBF GPS
// @Range: 0 10
// @Increment: 1
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("COM_PORT2", 24, AP_GPS, _com_port[1], 1),
#endif
AP_GROUPEND
};

1
libraries/AP_GPS/AP_GPS.h
View File

@ -500,6 +500,7 @@ protected:
AP_Int8 _auto_config;
AP_Vector3f _antenna_offset[GPS_MAX_RECEIVERS];
AP_Int16 _delay_ms[GPS_MAX_RECEIVERS];
AP_Int8 _com_port[GPS_MAX_RECEIVERS];
AP_Int8 _blend_mask;
AP_Float _blend_tc;
AP_Int16 _driver_options;

52
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -23,6 +23,7 @@
#include "AP_GPS.h"
#include "AP_GPS_SBF.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
extern const AP_HAL::HAL& hal;
@ -40,6 +41,10 @@ do { \
# define Debug(fmt, args ...)
#endif
#ifndef GPS_SBF_STREAM_NUMBER
#define GPS_SBF_STREAM_NUMBER 1
#endif
#define SBF_EXCESS_COMMAND_BYTES 5 // 2 start bytes + validity byte + space byte + endline byte
#define RX_ERROR_MASK (CONGESTION | \
@ -59,6 +64,10 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
_config_last_ack_time = AP_HAL::millis();
}
AP_GPS_SBF::~AP_GPS_SBF (void) {
free(_initial_sso);
}
// Process all bytes available from the stream
//
bool
@ -74,8 +83,6 @@ AP_GPS_SBF::read(void)
if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
uint32_t now = AP_HAL::millis();
const char *init_str = _initialisation_blob[_init_blob_index];
if (now > _init_blob_time) {
if (now > _config_last_ack_time + 2500) {
// try to enable input on the GPS port if we have not made progress on configuring it
@ -83,8 +90,25 @@ AP_GPS_SBF::read(void)
port->write((const uint8_t*)_port_enable, strlen(_port_enable));
_config_last_ack_time = now;
} else {
Debug("SBF sending init string: %s", init_str);
port->write((const uint8_t*)init_str, strlen(init_str));
char *init_str = nullptr;
if (!_validated_initial_sso) {
if (_initial_sso == nullptr) {
if (asprintf(&_initial_sso, "sso, Stream%d, COM%d%s",
(int)GPS_SBF_STREAM_NUMBER,
(int)gps._com_port[state.instance],
_sso_normal) == -1) {
_initial_sso = nullptr;
}
}
init_str = _initial_sso;
} else {
init_str = (char *)_initialisation_blob[_init_blob_index];
}
if (init_str != nullptr) {
Debug("SBF sending init string: %s", init_str);
port->write((const uint8_t*)init_str, strlen(init_str));
}
}
_init_blob_time = now + 1000;
}
@ -220,11 +244,25 @@ AP_GPS_SBF::parse(uint8_t temp)
if (sbf_msg.data.bytes[0] == ':') {
// valid command, determine if it was the one we were trying
// to send in the configuration sequence
if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
if (!strncmp(_initialisation_blob[_init_blob_index], (char *)(sbf_msg.data.bytes + 2),
const char * reference_blob = nullptr;
if (!_validated_initial_sso) {
reference_blob = _initial_sso;
} else {
if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
reference_blob = _initialisation_blob[_init_blob_index];
}
}
if (reference_blob != nullptr) {
if (!strncmp(reference_blob, (char *)(sbf_msg.data.bytes + 2),
sbf_msg.read - SBF_EXCESS_COMMAND_BYTES)) {
Debug("SBF Ack Command: %s\n", sbf_msg.data.bytes);
_init_blob_index++;
if (!_validated_initial_sso) {
free(_initial_sso);
_initial_sso = nullptr;
_validated_initial_sso = true;
} else {
_init_blob_index++;
}
_config_last_ack_time = AP_HAL::millis();
} else {
Debug("SBF Ack command (unexpected): %s\n", sbf_msg.data.bytes);

11
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -30,6 +30,7 @@ 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_SBF();
AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }
@ -60,10 +61,12 @@ private:
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[5] = {
"sso, Stream1, COM1, PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic, msec100\n",
bool _validated_initial_sso;
uint8_t _init_blob_index;
uint32_t _init_blob_time;
char *_initial_sso;
const char* _sso_normal = ", PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic, msec100\n";
const char* _initialisation_blob[4] = {
"srd, Moderate, UAV\n",
"sem, PVT, 5\n",
"spm, Rover, all\n",