From 08d4e1c0622feed50df32f78b012e5153844db71 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Sat, 13 Jun 2020 13:23:44 -0700 Subject: [PATCH] AP_GPS: Support configuring the SBF com port --- libraries/AP_GPS/AP_GPS.cpp | 20 +++++++++++++ libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_SBF.cpp | 52 ++++++++++++++++++++++++++++----- libraries/AP_GPS/AP_GPS_SBF.h | 11 ++++--- 4 files changed, 73 insertions(+), 11 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index e596f9d50f..8ed92ca967 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -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 }; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 2ba4125ae7..1368510e73 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -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; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 782a79e60f..6eb4990d4d 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -23,6 +23,7 @@ #include "AP_GPS.h" #include "AP_GPS_SBF.h" #include +#include 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); diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 1eb306faeb..50f935e2ad 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -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",