add parsing of host frames for D type telemetry

src/drivers/frsky_telemetry/frsky_data.c

@@ -45,8 +45,10 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
+#include <stdbool.h>
 #include <arch/math.h>
 #include <geo/geo.h>
+#include <stdbool.h>
 
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_combined.h>
@@ -285,3 +287,61 @@ void frsky_send_frame3(int uart)
 
 	frsky_send_startstop(uart);
 }
+
+/* parse 11 byte frames */
+bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v)
+{
+	bool data_ready = false;
+	static int dcount = 0;
+	static uint8_t type = 0;
+	static uint8_t data[11];
+	static enum {
+		HEADER = 0,
+		TYPE,
+		DATA,
+		TRAILER
+	} state = HEADER;
+
+	for (int i=0; i<nbytes; i++) {
+		switch (state) {
+		case HEADER:
+			if (sbuf[i] == 0x7E) {
+				state = TYPE;
+			}
+			break;
+		case TYPE:
+			if (sbuf[i] != 0x7E) {
+				state = DATA;
+				type = sbuf[i];
+				dcount = 0;
+			}
+			break;
+		case DATA:
+			/* read 8 data bytes */
+			if (dcount < 7) {
+				data[dcount++] = sbuf[i];
+			}
+			else {
+				/* received all data bytes */
+				state = TRAILER;
+			}
+			break;
+		case TRAILER:
+			state = HEADER;
+			if (sbuf[i] != 0x7E) {
+				warnx("host packet error: %x", sbuf[i]);
+			} else {
+				data_ready = true;
+				if (type == 0xFE) {
+					/* this is an adc_linkquality packet */
+					v->ad1 = data[0];
+					v->ad2 = data[1];
+					v->linkq = data[2];
+				}
+			}
+			break;
+		}
+	}
+	return data_ready;
+}
+

src/drivers/frsky_telemetry/frsky_data.h

@@ -42,10 +42,19 @@
 #ifndef _FRSKY_DATA_H
 #define _FRSKY_DATA_H
 
+#include <stdbool.h>
+
 // Public functions
 void frsky_init(void);
 void frsky_send_frame1(int uart);
 void frsky_send_frame2(int uart);
 void frsky_send_frame3(int uart);
 
+struct adc_linkquality {
+	uint8_t ad1;
+	uint8_t ad2;
+	uint8_t linkq;
+};
+bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v);
+
 #endif /* _FRSKY_TELEMETRY_H */

src/drivers/frsky_telemetry/frsky_telemetry.c

@@ -332,23 +332,35 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 		frsky_init();
 
 		warnx("sending FrSky D type telemetry");
+		struct adc_linkquality host_frame;
+		uint8_t dbuf[45];
 
 		/* send D8 mode telemetry */
 		while (!thread_should_exit) {
 
-			/* Sleep 200 ms */
-			usleep(200000);
+			/* Sleep 100 ms */
+			usleep(100000);
+
+			/* parse incoming data */
+			int nbytes = read(uart, &dbuf[0], sizeof(dbuf));
+			bool new_input = frsky_parse_host(&dbuf[0], nbytes, &host_frame);
+			if (new_input) {
+//				warnx("host frame: ad1:%u, ad2: %u, rssi: %u",
+//						host_frame.ad1, host_frame.ad2, host_frame.linkq);
+			}
 
 			/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
+			if (iteration % 2 == 0) {
 				frsky_send_frame1(uart);
+			}
 
 			/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
-			if (iteration % 5 == 0) {
+			if (iteration % 10 == 0) {
 				frsky_send_frame2(uart);
 			}
 
 			/* Send frame 3 (every 5000ms): date, time */
-			if (iteration % 25 == 0) {
+			if (iteration % 50 == 0) {
 				frsky_send_frame3(uart);
 
 				iteration = 0;

src/drivers/px4fmu/fmu.cpp

@@ -713,9 +713,9 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count,
 	_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
 	_rc_in.rc_ppm_frame_length = 0;
 
-	if (rssi == -1) {
-		_rc_in.rssi =
-			(!frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2);
+	/* don't touch rssi if no value was provided */
+	if (rssi >= 0) {
+		_rc_in.rssi = rssi;
 	}
 
 	_rc_in.rc_failsafe = failsafe;