From ad9dfe2d5108a8545cfdcb0fca6e97efbabebc95 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 22 Nov 2022 16:12:36 +1100 Subject: [PATCH] AP_GPS: added support for Unicore NMEA GPS this includes dual-antenna moving baseline support, 3D velocity and accuracies --- libraries/AP_GPS/AP_GPS.cpp | 13 +- libraries/AP_GPS/AP_GPS.h | 2 + libraries/AP_GPS/AP_GPS_NMEA.cpp | 245 ++++++++++++++++++++++++++++--- libraries/AP_GPS/AP_GPS_NMEA.h | 50 +++++-- libraries/AP_GPS/GPS_Backend.cpp | 2 + 5 files changed, 268 insertions(+), 44 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index ea41e018aa..f130b0d989 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -91,7 +91,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _TYPE // @DisplayName: 1st GPS type // @Description: GPS type of 1st GPS - // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover + // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA // @RebootRequired: True // @User: Advanced AP_GROUPINFO("_TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), @@ -100,7 +100,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _TYPE2 // @DisplayName: 2nd GPS type // @Description: GPS type of 2nd GPS - // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover + // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA // @RebootRequired: True // @User: Advanced AP_GROUPINFO("_TYPE2", 1, AP_GPS, _type[1], 0), @@ -583,13 +583,6 @@ void AP_GPS::send_blob_start(uint8_t instance) } #endif -#if AP_GPS_NMEA_ENABLED - if (_type[instance] == GPS_TYPE_HEMI) { - send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); - return; - } -#endif // AP_GPS_NMEA_ENABLED - // the following devices don't have init blobs: const char *blob = nullptr; uint32_t blob_size = 0; @@ -827,6 +820,8 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) #if AP_GPS_NMEA_ENABLED if ((_type[instance] == GPS_TYPE_NMEA || _type[instance] == GPS_TYPE_HEMI || + _type[instance] == GPS_TYPE_UNICORE_NMEA || + _type[instance] == GPS_TYPE_UNICORE_MOVINGBASE_NMEA || _type[instance] == GPS_TYPE_ALLYSTAR) && AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { return new AP_GPS_NMEA(*this, state[instance], _port[instance]); diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 6a05464ca5..0519fb9b20 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -128,6 +128,8 @@ public: GPS_TYPE_EXTERNAL_AHRS = 21, GPS_TYPE_UAVCAN_RTK_BASE = 22, GPS_TYPE_UAVCAN_RTK_ROVER = 23, + GPS_TYPE_UNICORE_NMEA = 24, + GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25, #if HAL_SIM_GPS_ENABLED GPS_TYPE_SITL = 100, #endif diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index de4dd36b0c..80c1559850 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -41,6 +41,11 @@ #if AP_GPS_NMEA_ENABLED extern const AP_HAL::HAL& hal; +#ifndef AP_GPS_NMEA_CONFIG_PERIOD_MS +// how often we send board specific config commands +#define AP_GPS_NMEA_CONFIG_PERIOD_MS 15000U +#endif + // Convenience macros ////////////////////////////////////////////////////////// // #define DIGIT_TO_VAL(_x) (_x - '0') @@ -51,6 +56,10 @@ 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--) { char c = port->read(); @@ -64,22 +73,33 @@ bool AP_GPS_NMEA::read(void) return parsed; } +/* + decode one character, return true if we have successfully completed a sentence, false otherwise + */ bool AP_GPS_NMEA::_decode(char c) { - bool valid_sentence = false; - _sentence_length++; switch (c) { + case ';': + // header separator for unicore + if (_sentence_type != _GPS_SENTENCE_AGRICA) { + return false; + } + FALLTHROUGH; case ',': // term terminators _parity ^= c; + if (_is_unicore) { + _crc32 = crc_crc32(_crc32, (const uint8_t *)&c, 1); + } FALLTHROUGH; case '\r': case '\n': - case '*': + case '*': { if (_sentence_done) { return false; } + bool valid_sentence = false; if (_term_offset < sizeof(_term)) { _term[_term_offset] = 0; valid_sentence = _term_complete(); @@ -88,25 +108,33 @@ bool AP_GPS_NMEA::_decode(char c) _term_offset = 0; _is_checksum_term = c == '*'; return valid_sentence; + } case '$': // sentence begin + case '#': // unicore message begin + _is_unicore = (c == '#'); _term_number = _term_offset = 0; _parity = 0; + _crc32 = 0; _sentence_type = _GPS_SENTENCE_OTHER; _is_checksum_term = false; _gps_data_good = false; _sentence_length = 1; _sentence_done = false; - return valid_sentence; + return false; } // ordinary characters if (_term_offset < sizeof(_term) - 1) _term[_term_offset++] = c; - if (!_is_checksum_term) + if (!_is_checksum_term) { _parity ^= c; + if (_is_unicore) { + _crc32 = crc_crc32(_crc32, (const uint8_t *)&c, 1); + } + } - return valid_sentence; + return false; } int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p) @@ -235,13 +263,17 @@ bool AP_GPS_NMEA::_have_new_message() _last_KSXT_pos_ms = 0; } - // special case for fixing low output rate of ALLYSTAR GPS modules - const int32_t dt_ms = now - _last_fix_ms; - if (labs(dt_ms - gps._rate_ms[state.instance]) > 50 && - get_type() == AP_GPS::GPS_TYPE_ALLYSTAR) { - nmea_printf(port, "$PHD,06,42,UUUUTTTT,BB,0,%u,55,0,%u,0,0,0", - unsigned(1000U/gps._rate_ms[state.instance]), - unsigned(gps._rate_ms[state.instance])); + if (now - _last_AGRICA_ms > 500) { + if (_last_AGRICA_ms != 0) { + // we have lost AGRICA + state.have_gps_yaw = false; + state.have_vertical_velocity = false; + state.have_speed_accuracy = false; + state.have_horizontal_accuracy = false; + state.have_vertical_accuracy = false; + state.have_undulation = false; + _last_AGRICA_ms = 0; + } } _last_fix_ms = now; @@ -258,13 +290,9 @@ bool AP_GPS_NMEA::_term_complete() // handle the last term in a message if (_is_checksum_term) { _sentence_done = true; - uint8_t nibble_high = 0; - uint8_t nibble_low = 0; - if (!hex_to_uint8(_term[0], nibble_high) || !hex_to_uint8(_term[1], nibble_low)) { - return false; - } - const uint8_t checksum = (nibble_high << 4u) | nibble_low; - if (checksum == _parity) { + const uint32_t crc = strtoul(_term, nullptr, 16); + const bool crc_ok = _is_unicore? (_crc32 == crc) : (_parity == crc); + if (crc_ok) { if (_gps_data_good) { uint32_t now = AP_HAL::millis(); switch (_sentence_type) { @@ -272,7 +300,7 @@ bool AP_GPS_NMEA::_term_complete() _last_RMC_ms = now; //time = _new_time; //date = _new_date; - if (_last_KSXT_pos_ms == 0) { + if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) { state.location.lat = _new_latitude; state.location.lng = _new_longitude; } @@ -291,7 +319,7 @@ bool AP_GPS_NMEA::_term_complete() break; case _GPS_SENTENCE_GGA: _last_GGA_ms = now; - if (_last_KSXT_pos_ms == 0) { + if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) { state.location.alt = _new_altitude; state.location.lat = _new_latitude; state.location.lng = _new_longitude; @@ -340,6 +368,10 @@ bool AP_GPS_NMEA::_term_complete() break; case _GPS_SENTENCE_HDT: case _GPS_SENTENCE_THS: + if (_last_AGRICA_ms != 0 || _expect_agrica) { + // use AGRICA + break; + } _last_yaw_ms = now; state.gps_yaw = wrap_360(_new_gps_yaw*0.01f); state.have_gps_yaw = true; @@ -351,6 +383,10 @@ bool AP_GPS_NMEA::_term_complete() state.gps_yaw_configured = true; break; case _GPS_SENTENCE_PHD: + if (_last_AGRICA_ms != 0) { + // prefer AGRICA + break; + } if (_phd.msg_id == 12) { state.velocity.x = _phd.fields[0] * 0.01; state.velocity.y = _phd.fields[1] * 0.01; @@ -372,6 +408,10 @@ bool AP_GPS_NMEA::_term_complete() } break; case _GPS_SENTENCE_KSXT: + if (_last_AGRICA_ms != 0 || _expect_agrica) { + // prefer AGRICA + break; + } state.location.lat = _ksxt.fields[2]*1.0e7; state.location.lng = _ksxt.fields[1]*1.0e7; state.location.alt = _ksxt.fields[3]*1.0e2; @@ -398,6 +438,37 @@ bool AP_GPS_NMEA::_term_complete() state.gps_yaw_configured = true; } break; + case _GPS_SENTENCE_AGRICA: { + auto &ag = _agrica; + _last_AGRICA_ms = now; + state.location.lat = ag.lat*1.0e7; + state.location.lng = ag.lng*1.0e7; + state.location.alt = ag.alt*1.0e2; + state.undulation = -ag.undulation; + state.velocity = ag.vel_NED; + state.speed_accuracy = ag.vel_stddev.length(); + state.horizontal_accuracy = ag.pos_stddev.xy().length(); + state.vertical_accuracy = ag.pos_stddev.z; + state.have_vertical_velocity = true; + state.have_speed_accuracy = true; + state.have_horizontal_accuracy = true; + state.have_vertical_accuracy = true; + state.have_undulation = true; + + if (ag.heading_status == 4) { + Location slave( ag.slave_lat*1.0e7, ag.slave_lng*1.0e7, ag.slave_alt*1.0e2, Location::AltFrame::ABSOLUTE ); + const float dist = state.location.get_distance_NED(slave).length(); + const float bearing = degrees(state.location.get_bearing(slave)); + const float alt_diff = (state.location.alt - slave.alt)*0.01; + calculate_moving_base_yaw(bearing, dist, alt_diff); + state.gps_yaw_configured = true; + } else { + state.have_gps_yaw = false; + } + + check_new_itow(ag.itow, _sentence_length); + break; + } } } else { switch (_sentence_type) { @@ -433,6 +504,11 @@ bool AP_GPS_NMEA::_term_complete() _gps_data_good = true; return false; } + if (strcmp(_term, "AGRICA") == 0) { + _sentence_type = _GPS_SENTENCE_AGRICA; + _gps_data_good = true; + return false; + } /* The first two letters of the NMEA term are the talker ID. The most common is 'GP' but there are a bunch of others @@ -557,15 +633,83 @@ bool AP_GPS_NMEA::_term_complete() case _GPS_SENTENCE_PHD + 6 ... _GPS_SENTENCE_PHD + 11: // PHD message, fields _phd.fields[_term_number-6] = atol(_term); break; - case _GPS_SENTENCE_KSXT + 1 ... _GPS_SENTENCE_KSXT + 22: // PHD message, fields + case _GPS_SENTENCE_KSXT + 1 ... _GPS_SENTENCE_KSXT + 22: // KSXT message, fields _ksxt.fields[_term_number-1] = atof(_term); break; + case _GPS_SENTENCE_AGRICA + 1 ... _GPS_SENTENCE_AGRICA + 65: // AGRICA message + parse_agrica_field(_term_number, _term); + break; } } return false; } +/* + parse an AGRICA message term + + Example: + #AGRICA,82,GPS,FINE,2237,176366400,0,0,18,15;GNSS,232,22,11,22,0,59,8,1,5,8,12,0,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,296.4656,-26.5685,0.0000,0.005,0.000,0.000,-0.005,0.044,0.032,0.038,-35.33142715815,149.13181842030,609.1494,-4471799.0368,2672944.7758,-3668288.9857,1.3923,1.5128,3.2272,2.3026,2.1633,2.1586,0.00000000000,0.00000000000,0.0000,0.00000000000,0.00000000000,0.0000,176366400,0.000,66.175285,18.972784,0.000000,0.000000,5,0,0,0*9f704dad + */ +void AP_GPS_NMEA::parse_agrica_field(uint16_t term_number, const char *term) +{ + auto &ag = _agrica; + // subtract 8 to align term numbers with reference manual + const uint8_t hdr_align = 8; + if (term_number < hdr_align) { + // discard header; + return; + } + term_number -= hdr_align; + // useful for debugging + //::printf("AGRICA[%u]=%s\n", unsigned(term_number), term); + switch (term_number) { + case 10: + ag.rtk_status = atol(term); + break; + case 11: + ag.heading_status = atol(term); + break; + case 25 ... 26: + ag.vel_NED[term_number-25] = atof(term); + break; + case 27: + // AGRIC gives velocity up + ag.vel_NED.z = -atof(term); + break; + case 28 ... 30: + ag.vel_stddev[term_number-28] = atof(term); + break; + case 31: + ag.lat = atof(term); + break; + case 32: + ag.lng = atof(term); + break; + case 33: + ag.alt = atof(term); + break; + case 46: + ag.slave_lat = atof(term); + break; + case 47: + ag.slave_lng = atof(term); + break; + case 48: + ag.slave_alt = atof(term); + break; + case 49: + ag.itow = atol(term); + break; + case 37 ... 39: + ag.pos_stddev[term_number-37] = atof(term); + break; + case 52: + ag.undulation = atof(term); + break; + } +} + /* detect a NMEA GPS. Adds one byte, and returns true if the stream matches a NMEA string @@ -604,4 +748,57 @@ AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data) } return false; } -#endif + +/* + send type specific config strings + */ +void AP_GPS_NMEA::send_config(void) +{ + uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_config_ms < AP_GPS_NMEA_CONFIG_PERIOD_MS) { + return; + } + last_config_ms = now_ms; + const uint16_t rate_ms = gps._rate_ms[state.instance]; + const float rate_s = rate_ms * 0.001; + const uint8_t rate_hz = 1000U / rate_ms; + + switch (get_type()) { + case AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA: + port->printf("\r\nmode movingbase\r\n" \ + "CONFIG HEADING FIXLENGTH\r\n" \ + "CONFIG UNDULATION AUTO\r\n" \ + "GPGGAH %.3f\r\n", // needed to get slave antenna position in AGRICA + rate_s); + state.gps_yaw_configured = true; + FALLTHROUGH; + + case AP_GPS::GPS_TYPE_UNICORE_NMEA: { + port->printf("\r\nAGRICA %.3f\r\n", rate_s); + _expect_agrica = true; + break; + } + + case AP_GPS::GPS_TYPE_HEMI: { + port->printf( + "$JATT,NMEAHE,0\r\n" /* Prefix of GP on the HDT message */ \ + "$JASC,GPGGA,%u\r\n" /* GGA at 5Hz */ \ + "$JASC,GPRMC,%u\r\n" /* RMC at 5Hz */ \ + "$JASC,GPVTG,%u\r\n" /* VTG at 5Hz */ \ + "$JASC,GPHDT,%u\r\n" /* HDT at 5Hz */ \ + "$JMODE,SBASR,YES\r\n" /* Enable SBAS */, + rate_hz, rate_hz, rate_hz, rate_hz); + break; + } + + case AP_GPS::GPS_TYPE_ALLYSTAR: + nmea_printf(port, "$PHD,06,42,UUUUTTTT,BB,0,%u,55,0,%u,0,0,0", + unsigned(rate_hz), unsigned(rate_ms)); + break; + + default: + break; + } +} + +#endif // AP_GPS_NMEA_ENABLED diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 70b1d5f172..a223bb367e 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -68,7 +68,7 @@ public: private: /// Coding for the GPS sentences that the parser handles - enum _sentence_types : uint8_t { //there are some more than 10 fields in some sentences , thus we have to increase these value. + enum _sentence_types : uint16_t { //there are some more than 10 fields in some sentences , thus we have to increase these value. _GPS_SENTENCE_RMC = 32, _GPS_SENTENCE_GGA = 64, _GPS_SENTENCE_VTG = 96, @@ -76,6 +76,7 @@ private: _GPS_SENTENCE_PHD = 138, // extension for AllyStar GPS modules _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_OTHER = 0 }; @@ -118,11 +119,19 @@ private: /// return true if we have a new set of NMEA messages bool _have_new_message(void); + /* + parse an AGRICA field + */ + void parse_agrica_field(uint16_t term_number, const char *term); + + 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 - uint8_t _sentence_type; ///< the sentence type currently being processed - uint8_t _term_number; ///< term index 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 uint8_t _term_offset; ///< character offset with the term being received uint16_t _sentence_length; bool _gps_data_good; ///< set when the sentence indicates data is good @@ -151,6 +160,7 @@ private: uint32_t _last_vaccuracy_ms; uint32_t _last_3D_velocity_ms; uint32_t _last_KSXT_pos_ms; + uint32_t _last_AGRICA_ms; uint32_t _last_fix_ms; /// @name Init strings @@ -191,13 +201,31 @@ private: double fields[21]; } _ksxt; + /* + unicore AGRICA message parsing + */ + struct { + uint32_t start_byte; + uint8_t rtk_status; + uint8_t heading_status; + Vector3f vel_NED; + Vector3f vel_stddev; + double lat, lng; + float alt; + uint32_t itow; + float undulation; + double slave_lat, slave_lng; + float slave_alt; + Vector3f pos_stddev; + } _agrica; + bool _expect_agrica; + + // last time we sent type specific config strings + uint32_t last_config_ms; + + // send type specific config strings + void send_config(void); }; -#define AP_GPS_NMEA_HEMISPHERE_INIT_STRING \ - "$JATT,NMEAHE,0\r\n" /* Prefix of GP on the HDT message */ \ - "$JASC,GPGGA,5\r\n" /* GGA at 5Hz */ \ - "$JASC,GPRMC,5\r\n" /* RMC at 5Hz */ \ - "$JASC,GPVTG,5\r\n" /* VTG at 5Hz */ \ - "$JASC,GPHDT,5\r\n" /* HDT at 5Hz */ \ - "$JMODE,SBASR,YES\r\n" /* Enable SBAS */ -#endif +#endif // AP_GPS_NMEA_ENABLED + diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 2e1761034b..7fdcb28914 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -399,6 +399,8 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, if (fabsf(alt_error) > permitted_error_length_pct * min_dist) { // the vertical component is out of range, reject it + Debug("bad alt_err %.1f > %.1f\n", + alt_error, permitted_error_length_pct * min_dist); goto bad_yaw; } }