Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: improved unicore setup

Browse Source 
log version information and improve auto-bauding
This commit is contained in:
Andrew Tridgell 2022-12-12 17:06:32 +11:00
parent 3c1a54c3df
commit d7a3c643a6
3 changed files with 101 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_GPS/AP_GPS.cpp
View File

@ -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
;

83
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -30,6 +30,8 @@
#include <AP_Common/AP_Common.h>
#include <AP_Common/NMEA.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <ctype.h>
#include <stdint.h>
@ -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();
}
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

21
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -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