unicore: parse AGRICA message, request heading (PX4#140)

* unicore: parse AGRICA message, request heading

This add support to parse (but not use) the Unicore AGRICA message. This
allows to determine whether we are talking to a UM982 and hence request
the heading (UNIHEADINGA) message that we actually require.

Signed-off-by: Julian Oes <[email protected]>

gps-parser-test.cpp

@@ -112,6 +112,30 @@ void test_uniheadinga_twice()
 	assert(num_parsed == 2);
 }
 
+void test_agrica()
+{
+	const char str[] =
+		"#AGRICA,68,GPS,FINE,2063,454587000,0,0,18,38;GNSS,236,19,7,26,6,16,9,4,4,12,10,"
+		"9,306.7191,10724.0176,-"
+		"16.4796,0.0089,0.0070,0.0181,67.9651,29.3584,0.0000,0.003,0.003,0.001,-"
+		"0.002,0.021,0.039,0.025,40.07896719907,116.23652055432,67.3108,-"
+		"2160482.7849,4383625.2350,4084735.7632,0.0140,0.0125,0.0296,0.0107,0.0198,0.012"
+		"8,40.07627310896,116.11079363322,65.3740,0.00000000000,0.00000000000,0.0000,4"
+		"54587000,38.000,16.723207,-9.406086,0.000000,0.000000,8,0,0,0*e9402e02";
+
+	UnicoreParser unicore_parser;
+
+	for (unsigned i = 0; i < sizeof(str); ++i) {
+		auto result = unicore_parser.parseChar(str[i]);
+
+		if (result == UnicoreParser::Result::GotAgrica) {
+			return;
+		}
+	}
+
+	assert(false);
+}
+
 void test_unicore()
 {
 	test_empty();
@@ -120,6 +144,7 @@ void test_unicore()
 	test_uniheadinga_wrong_crc();
 	test_uniheadinga();
 	test_uniheadinga_twice();
+	test_agrica();
 }
 
 int main(int, char **)

src/nmea.cpp

@@ -889,8 +889,11 @@ int GPSDriverNMEA::receive(unsigned timeout)
 					handled |= handleMessage(l);
 				}
 
-				if (_unicore_parser.parseChar(buf[i]) == UnicoreParser::Result::GotHeading) {
+				UnicoreParser::Result result = _unicore_parser.parseChar(buf[i]);
+
+				if (result == UnicoreParser::Result::GotHeading) {
 					++handled;
+					_unicore_heading_received_last = gps_absolute_time();
 
 					// Unicore seems to publish heading and standard deviation of 0
 					// to signal that it has not initialized the heading yet.
@@ -911,6 +914,17 @@ int GPSDriverNMEA::receive(unsigned timeout)
 						   (double)_unicore_parser.heading().heading_deg,
 						   (double)_unicore_parser.heading().heading_stddev_deg,
 						   (double)_unicore_parser.heading().baseline_m);
+
+				} else if (result == UnicoreParser::Result::GotAgrica) {
+					++handled;
+
+					// We don't use anything of that message at this point, however, this
+					// allows to determine whether we are talking to a UM982 and hence
+					// request the heading (UNIHEADINGA) message that we actually require.
+
+					if (gps_absolute_time() - _unicore_heading_received_last > 1000000) {
+						request_unicore_heading_message();
+					}
 				}
 			}
 
@@ -945,6 +959,13 @@ void GPSDriverNMEA::handleHeading(float heading_deg, float heading_stddev_deg)
 	_gps_position->heading_accuracy = heading_stddev_rad;
 }
 
+void GPSDriverNMEA::request_unicore_heading_message()
+{
+	// Configure heading message on serial port at 5 Hz. Don't save it though.
+	uint8_t buf[] = "UNIHEADINGA COM1 0.2\r\n";
+	write(buf, sizeof(buf) - 1);
+}
+
 #define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA)))
 
 int GPSDriverNMEA::parseChar(uint8_t b)

src/nmea.h

@@ -71,8 +71,10 @@ class GPSDriverNMEA : public GPSHelper
 
 private:
 	void handleHeading(float heading_deg, float heading_stddev_deg);
+	void request_unicore_heading_message();
 
 	UnicoreParser _unicore_parser;
+	gps_abstime _unicore_heading_received_last;
 
 	enum class NMEADecodeState {
 		uninit,

src/unicore.cpp

@@ -87,6 +87,16 @@ UnicoreParser::Result UnicoreParser::parseChar(char c)
 					return Result::WrongStructure;
 				}
 
+			} else if (isAgrica()) {
+				if (extractAgrica()) {
+					reset();
+					return Result::GotAgrica;
+
+				} else {
+					reset();
+					return Result::WrongStructure;
+				}
+
 			} else {
 				reset();
 				return Result::UnknownSentence;
@@ -120,6 +130,13 @@ bool UnicoreParser::isHeading() const
 	return strncmp(header, _buffer, strlen(header)) == 0;
 }
 
+bool UnicoreParser::isAgrica() const
+{
+	const char header[] = "AGRICA";
+
+	return strncmp(header, _buffer, strlen(header)) == 0;
+}
+
 bool UnicoreParser::extractHeading()
 {
 	// The basline starts after ;,, and then follows the heading.
@@ -158,3 +175,8 @@ bool UnicoreParser::extractHeading()
 
 	return false;
 }
+
+bool UnicoreParser::extractAgrica()
+{
+	return true;
+}

src/unicore.h

@@ -44,6 +44,7 @@ class UnicoreParser
 		WrongCrc,
 		WrongStructure,
 		GotHeading,
+		GotAgrica,
 		UnknownSentence,
 	};
 
@@ -55,20 +56,30 @@ class UnicoreParser
 		float baseline_m;
 	};
 
+	struct Agrica {
+	};
+
 	Heading heading() const
 	{
 		return _heading;
 	}
 
+	Agrica agrica() const
+	{
+		return _agrica;
+	}
+
 
 private:
 	void reset();
 	bool crcCorrect() const;
 	bool isHeading() const;
+	bool isAgrica() const;
 	bool extractHeading();
+	bool extractAgrica();
 
-	// We have seen buffers with 154 bytes.
-	char _buffer[256];
+	// We have seen buffers with 540 bytes for AGRICA.
+	char _buffer[600];
 	unsigned _buffer_pos {0};
 	char _buffer_crc[9];
 	unsigned _buffer_crc_pos {0};
@@ -80,4 +91,5 @@ class UnicoreParser
 	} _state {State::Uninit};
 
 	Heading _heading{};
+	Agrica _agrica{};
 };