From d7a3c643a6cd5de2df2e3d7c6ad67b73c4016982 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Dec 2022 17:06:32 +1100 Subject: [PATCH] AP_GPS: improved unicore setup log version information and improve auto-bauding --- libraries/AP_GPS/AP_GPS.cpp | 3 ++ libraries/AP_GPS/AP_GPS_NMEA.cpp | 85 +++++++++++++++++++++++++++++--- libraries/AP_GPS/AP_GPS_NMEA.h | 21 +++++++- 3 files changed, 101 insertions(+), 8 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 85f0cb09d6..58569c60d0 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -80,6 +80,9 @@ const char AP_GPS::_initialisation_blob[] = #endif #if AP_GPS_SIRF_ENABLED SIRF_SET_BINARY +#endif +#if AP_GPS_NMEA_UNICORE_ENABLED + NMEA_UNICORE_SETUP #endif "" // to compile we need *some_initialiser if all backends compiled out ; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index cc28e88b81..ec31bcb683 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -30,6 +30,8 @@ #include #include +#include +#include #include #include @@ -56,9 +58,7 @@ bool AP_GPS_NMEA::read(void) int16_t numc; bool parsed = false; - if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) { - send_config(); - } + send_config(); numc = port->available(); while (numc--) { @@ -83,7 +83,7 @@ bool AP_GPS_NMEA::_decode(char c) switch (c) { case ';': // header separator for unicore - if (_sentence_type != _GPS_SENTENCE_AGRICA) { + if (!_is_unicore) { return false; } FALLTHROUGH; @@ -481,6 +481,15 @@ bool AP_GPS_NMEA::_term_complete() check_new_itow(ag.itow, _sentence_length); break; } + case _GPS_SENTENCE_VERSIONA: { + _have_unicore_versiona = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, + "NMEA %s %s %s", + _versiona.type, + _versiona.version, + _versiona.build_date); + break; + } #endif // AP_GPS_NMEA_UNICORE_ENABLED } // see if we got a good message @@ -504,10 +513,14 @@ bool AP_GPS_NMEA::_term_complete() return false; } #if AP_GPS_NMEA_UNICORE_ENABLED - if (strcmp(_term, "AGRICA") == 0) { + if (strcmp(_term, "AGRICA") == 0 && _expect_agrica) { _sentence_type = _GPS_SENTENCE_AGRICA; return false; } + if (strcmp(_term, "VERSIONA") == 0) { + _sentence_type = _GPS_SENTENCE_VERSIONA; + return false; + } #endif /* The first two letters of the NMEA term are the talker @@ -627,6 +640,9 @@ bool AP_GPS_NMEA::_term_complete() case _GPS_SENTENCE_AGRICA + 1 ... _GPS_SENTENCE_AGRICA + 65: // AGRICA message parse_agrica_field(_term_number, _term); break; + case _GPS_SENTENCE_VERSIONA + 1 ... _GPS_SENTENCE_VERSIONA + 20: + parse_versiona_field(_term_number, _term); + break; #endif } } @@ -700,6 +716,25 @@ void AP_GPS_NMEA::parse_agrica_field(uint16_t term_number, const char *term) break; } } + +// parse VERSIONA fields +void AP_GPS_NMEA::parse_versiona_field(uint16_t term_number, const char *term) +{ + // printf useful for debugging + // ::printf("VERSIONA[%u]='%s'\n", term_number, term); + auto &v = _versiona; + switch (term_number) { + case 10: + strncpy(v.type, _term, sizeof(v.type)-1); + break; + case 11: + strncpy(v.version, _term, sizeof(v.version)-1); + break; + case 15: + strncpy(v.build_date, _term, sizeof(v.build_date)-1); + break; + } +} #endif // AP_GPS_NMEA_UNICORE_ENABLED /* @@ -746,6 +781,13 @@ AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data) */ void AP_GPS_NMEA::send_config(void) { + const auto type = get_type(); + _expect_agrica = (type == AP_GPS::GPS_TYPE_UNICORE_NMEA || + type == AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA); + if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) { + // not doing auto-config + return; + } uint32_t now_ms = AP_HAL::millis(); if (now_ms - last_config_ms < AP_GPS_NMEA_CONFIG_PERIOD_MS) { return; @@ -760,9 +802,10 @@ void AP_GPS_NMEA::send_config(void) switch (get_type()) { #if AP_GPS_NMEA_UNICORE_ENABLED case AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA: - port->printf("\r\nmode movingbase\r\n" \ + port->printf("\r\nMODE MOVINGBASE\r\n" \ "CONFIG HEADING FIXLENGTH\r\n" \ "CONFIG UNDULATION AUTO\r\n" \ + "CONFIG\r\n" \ "GPGGAH %.3f\r\n", // needed to get slave antenna position in AGRICA rate_s); state.gps_yaw_configured = true; @@ -773,7 +816,14 @@ void AP_GPS_NMEA::send_config(void) "GNGGA %.3f\r\n" \ "GNRMC %.3f\r\n", rate_s, rate_s, rate_s); - _expect_agrica = true; + if (!_have_unicore_versiona) { + // get version information for logging if we don't have it yet + port->printf("VERSIONA\r\n"); + if (gps._save_config) { + // save config changes for fast startup + port->printf("SAVECONFIG\r\n"); + } + } break; } #endif // AP_GPS_NMEA_UNICORE_ENABLED @@ -798,6 +848,11 @@ void AP_GPS_NMEA::send_config(void) default: break; } + +#ifdef AP_GPS_NMEA_CUSTOM_CONFIG_STRING + // allow for custom config strings, useful for peripherals + port->printf("%s\r\n", AP_GPS_NMEA_CUSTOM_CONFIG_STRING); +#endif } /* @@ -845,4 +900,20 @@ bool AP_GPS_NMEA::get_lag(float &lag_sec) const return true; } +void AP_GPS_NMEA::Write_AP_Logger_Log_Startup_messages() const +{ +#if HAL_LOGGING_ENABLED + AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages(); +#if AP_GPS_NMEA_UNICORE_ENABLED + if (_have_unicore_versiona) { + AP::logger().Write_MessageF("NMEA %u %s %s %s", + state.instance+1, + _versiona.type, + _versiona.version, + _versiona.build_date); + } +#endif +#endif +} + #endif // AP_GPS_NMEA_ENABLED diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index f7f9bfbd0d..64ca76d4a3 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -72,6 +72,8 @@ public: // get lag in seconds bool get_lag(float &lag_sec) const override; + void Write_AP_Logger_Log_Startup_messages() const override; + private: /// Coding for the GPS sentences that the parser handles enum _sentence_types : uint16_t { //there are some more than 10 fields in some sentences , thus we have to increase these value. @@ -83,6 +85,7 @@ private: _GPS_SENTENCE_THS = 160, // True heading with quality indicator, available on Trimble MB-Two _GPS_SENTENCE_KSXT = 170, // extension for Unicore, 21 fields _GPS_SENTENCE_AGRICA = 193, // extension for Unicore, 65 fields + _GPS_SENTENCE_VERSIONA = 270, // extension for Unicore, version _GPS_SENTENCE_OTHER = 0 }; @@ -130,13 +133,16 @@ private: parse an AGRICA field */ void parse_agrica_field(uint16_t term_number, const char *term); + + // parse VERSIONA field + void parse_versiona_field(uint16_t term_number, const char *term); #endif uint8_t _parity; ///< NMEA message checksum accumulator uint32_t _crc32; ///< CRC for unicore messages bool _is_checksum_term; ///< current term is the checksum - char _term[15]; ///< buffer for the current term within the current sentence + char _term[30]; ///< buffer for the current term within the current sentence uint16_t _sentence_type; ///< the sentence type currently being processed bool _is_unicore; ///< true if in a unicore '#' sentence uint16_t _term_number; ///< term index within the current sentence @@ -226,6 +232,13 @@ private: float slave_alt; Vector3f pos_stddev; } _agrica; + struct { + char type[10]; + char version[20]; + char build_date[13]; + } _versiona; + bool _have_unicore_versiona; + #endif // AP_GPS_NMEA_UNICORE_ENABLED bool _expect_agrica; @@ -236,5 +249,11 @@ private: void send_config(void); }; +#if AP_GPS_NMEA_UNICORE_ENABLED && !defined(NMEA_UNICORE_SETUP) +// we don't know what port the GPS may be using, so configure all 3. We need to get it sending +// one message to allow the NMEA detector to run +#define NMEA_UNICORE_SETUP "CONFIG COM1 230400 8 n 1\r\nCONFIG COM2 230400 8 n 1\r\nCONFIG COM3 230400 8 n 1\r\nGPGGA 0.2\r\n" +#endif + #endif // AP_GPS_NMEA_ENABLED