Small improvements and corrections

Added more SPort GPS Sensors

Fix code style

Adding DIY IDs

Moved navstate and gpsfix away from Temp1 and Temp2 to their own IDs

More consistent names

Fix codestyle

Rebase work off master frsky_telemetry driver

Fixed code-style error

forgot something

src/drivers/frsky_telemetry/CMakeLists.txt

@@ -43,4 +43,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/frsky_telemetry/frsky_data.c

@@ -53,6 +53,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -93,10 +94,12 @@
 
 static struct battery_status_s *battery_status;
 static struct vehicle_global_position_s *global_pos;
+static struct vehicle_status_s *vehicle_status;
 static struct sensor_combined_s *sensor_combined;
 
 static int battery_status_sub = -1;
-static int global_position_sub = -1;
+static int vehicle_global_position_sub = -1;
+static int vehicle_status_sub = -1;
 static int sensor_sub = -1;
 
 /**
@@ -107,13 +110,15 @@ bool frsky_init()
 	battery_status = malloc(sizeof(struct battery_status_s));
 	global_pos = malloc(sizeof(struct vehicle_global_position_s));
 	sensor_combined = malloc(sizeof(struct sensor_combined_s));
+	vehicle_status = malloc(sizeof(struct vehicle_status_s));
 
-	if (battery_status == NULL || global_pos == NULL || sensor_combined == NULL) {
+	if (battery_status == NULL || global_pos == NULL || sensor_combined == NULL || vehicle_status) {
 		return false;
 	}
 
 	battery_status_sub = orb_subscribe(ORB_ID(battery_status));
-	global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	return true;
 }
@@ -123,6 +128,7 @@ void frsky_deinit()
 	free(battery_status);
 	free(global_pos);
 	free(sensor_combined);
+	free(vehicle_status);
 }
 
 /**
@@ -175,7 +181,6 @@ static void frsky_send_data(int uart, uint8_t id, int16_t data)
 void frsky_update_topics()
 {
 	bool updated;
-
 	/* get a local copy of the current sensor values */
 	orb_check(sensor_sub, &updated);
 
@@ -191,10 +196,17 @@ void frsky_update_topics()
 	}
 
 	/* get a local copy of the global position data */
-	orb_check(global_position_sub, &updated);
+	orb_check(vehicle_global_position_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, global_pos);
+	}
+
+	/* get a local copy of the vehicle status data */
+	orb_check(vehicle_status_sub, &updated);
 
 	if (updated) {
-		orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
+		orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status);
 	}
 }
 
@@ -374,4 +386,3 @@ bool frsky_parse_host(uint8_t *sbuf, int nbytes, struct adc_linkquality *v)
 
 	return data_ready;
 }
-

src/drivers/frsky_telemetry/frsky_telemetry.c

@@ -283,6 +283,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 			// allow a minimum of 500usec before reply
 			usleep(500);
 
+			sPort_update_topics();
+
 			static hrt_abstime lastBATV = 0;
 			static hrt_abstime lastCUR = 0;
 			static hrt_abstime lastALT = 0;
@@ -290,6 +292,9 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 			static hrt_abstime lastFUEL = 0;
 			static hrt_abstime lastVSPD = 0;
 			static hrt_abstime lastGPS = 0;
+			static hrt_abstime lastNAV_STATE = 0;
+			static hrt_abstime lastGPS_FIX = 0;
+
 
 			switch (sbuf[1]) {
 
@@ -370,7 +375,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 
 			case SMARTPORT_POLL_7:
 
-				/* report GPS data elements at 2*5Hz */
+				/* report GPS data elements at 5*5Hz */
 				if (now - lastGPS > 100 * 1000) {
 					static int elementCount = 0;
 
@@ -387,7 +392,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 						break;
 
 					case 2:
-						sPort_send_GPS_COG(uart);
+						sPort_send_GPS_CRS(uart);
 						elementCount++;
 						break;
 
@@ -398,12 +403,33 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 
 					case 4:
 						sPort_send_GPS_SPD(uart);
+						elementCount++;
+						break;
+
+					case 5:
+						sPort_send_GPS_TIME(uart);
 						elementCount = 0;
 						break;
 					}
 
 				}
 
+			case SMARTPORT_POLL_8:
+
+				/* report nav_state as DIY_NAVSTATE at 1Hz */
+				if (now - lastNAV_STATE > 1000 * 1000) {
+					lastNAV_STATE = now;
+					/* send T1 */
+					sPort_send_NAV_STATE(uart);
+				}
+
+				/* report satcount and fix as DIY_GPSFIX at 1Hz */
+				else if (now - lastGPS_FIX > 1000 * 1000) {
+					lastGPS_FIX = now;
+					/* send T2 */
+					sPort_send_GPS_FIX(uart);
+				}
+
 				break;
 			}
 		}

src/drivers/frsky_telemetry/sPort_data.c

@@ -49,9 +49,11 @@
 #include <arch/math.h>
 #include <geo/geo.h>
 
+#include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_gps_position.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -60,36 +62,93 @@
 static int sensor_sub = -1;
 static int global_position_sub = -1;
 static int battery_status_sub = -1;
-static struct sensor_combined_s *sensor_data;
+static int vehicle_status_sub = -1;
+static int gps_position_sub = -1;
+
+static struct sensor_combined_s *sensor_combined;
 static struct vehicle_global_position_s *global_pos;
 static struct battery_status_s *battery_status;
+static struct vehicle_status_s *vehicle_status;
+static struct vehicle_gps_position_s *gps_position;
+
 
 /**
  * Initializes the uORB subscriptions.
  */
 bool sPort_init()
 {
-	sensor_data = malloc(sizeof(struct sensor_combined_s));
+
+	sensor_combined = malloc(sizeof(struct sensor_combined_s));
 	global_pos = malloc(sizeof(struct vehicle_global_position_s));
 	battery_status = malloc(sizeof(struct battery_status_s));
+	vehicle_status = malloc(sizeof(struct vehicle_status_s));
+	gps_position = malloc(sizeof(struct vehicle_gps_position_s));
+
 
-	if (sensor_data == NULL || global_pos == NULL || battery_status == NULL) {
+	if (sensor_combined == NULL || global_pos == NULL || battery_status == NULL || vehicle_status == NULL
+	    || gps_position == NULL) {
 		return false;
 	}
 
+
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	battery_status_sub = orb_subscribe(ORB_ID(battery_status));
+	vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+
 	return true;
 }
 
 void sPort_deinit()
 {
-	free(sensor_data);
+	free(sensor_combined);
 	free(global_pos);
 	free(battery_status);
+	free(vehicle_status);
+	free(gps_position);
+}
+
+void sPort_update_topics()
+{
+	bool updated;
+	/* get a local copy of the current sensor values */
+	orb_check(sensor_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_combined);
+	}
+
+	/* get a local copy of the battery data */
+	orb_check(battery_status_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
+	}
+
+	/* get a local copy of the global position data */
+	orb_check(global_position_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
+	}
+
+	/* get a local copy of the vehicle status data */
+	orb_check(vehicle_status_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status);
+	}
+
+	/* get a local copy of the gps position data */
+	orb_check(gps_position_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_gps_position), gps_position_sub, gps_position);
+	}
 }
 
+
 static void update_crc(uint16_t *crc, unsigned char b)
 {
 	*crc += b;
@@ -158,24 +217,16 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
 // scaling correct with OpenTX 2.1.7
 void sPort_send_BATV(int uart)
 {
-	/* get a local copy of the battery data */
-	orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
-
 	/* send battery voltage as VFAS */
 	uint32_t voltage = (int)(100 * battery_status->voltage_v);
-
 	sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage);
 }
 
 // verified scaling
 void sPort_send_CUR(int uart)
 {
-	/* get a local copy of the battery data */
-	orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
-
 	/* send data */
 	uint32_t current = (int)(10 * battery_status->current_a);
-
 	sPort_send_data(uart, SMARTPORT_ID_CURR, current);
 }
 
@@ -184,18 +235,15 @@ void sPort_send_CUR(int uart)
 // the difference (altitude - field)
 void sPort_send_ALT(int uart)
 {
-	/* get a local copy of the current sensor values */
-	orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_data);
-
 	/* send data */
-	uint32_t alt = (int)(100 * sensor_data->baro_alt_meter[0]);
+	uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter[0]);
 	sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
 }
 
 // verified scaling for "calculated" option
 void sPort_send_SPD(int uart)
 {
-	/* send data */
+	/* send data for A2 */
 	float speed  = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e);
 	uint32_t ispeed = (int)(10 * speed);
 	sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
@@ -212,27 +260,20 @@ void sPort_send_VSPD(int uart, float speed)
 // verified scaling
 void sPort_send_FUEL(int uart)
 {
-	/* get a local copy of the battery data */
-	orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
-
 	/* send data */
 	uint32_t fuel = (int)(100 * battery_status->remaining);
-
 	sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel);
 }
 
 void sPort_send_GPS_LON(int uart)
 {
 	/* send longitude */
-	/* get a local copy of the global position data */
-	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
-
 	/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
 	/* precision is approximately 0.1m */
-	uint32_t iLon = 6E5 * fabs(global_pos->lon);
+	uint32_t iLon = 6E5 * fabs(gps_position->lon);
 	iLon |= (1 << 31);
 
-	if (global_pos->lon < 0) { iLon |= (1 << 30); }
+	if (gps_position->lon < 0) { iLon |= (1 << 30); }
 
 	sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon);
 }
@@ -240,11 +281,10 @@ void sPort_send_GPS_LON(int uart)
 void sPort_send_GPS_LAT(int uart)
 {
 	/* send latitude */
-
 	/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */
-	uint32_t iLat = 6E5 * fabs(global_pos->lat);
+	uint32_t iLat = 6E5 * fabs(gps_position->lat);
 
-	if (global_pos->lat < 0) { iLat |= (1 << 30); }
+	if (gps_position->lat < 0) { iLat |= (1 << 30); }
 
 	sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat);
 }
@@ -252,13 +292,12 @@ void sPort_send_GPS_LAT(int uart)
 void sPort_send_GPS_ALT(int uart)
 {
 	/* send altitude */
-
 	/* convert to 100 * m/sec */
-	uint32_t iAlt = 100 * global_pos->alt;
+	uint32_t iAlt = 100 * gps_position->alt;
 	sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt);
 }
 
-void sPort_send_GPS_COG(int uart)
+void sPort_send_GPS_CRS(int uart)
 {
 	/* send course */
 
@@ -267,6 +306,11 @@ void sPort_send_GPS_COG(int uart)
 	sPort_send_data(uart, SMARTPORT_ID_GPS_CRS, iYaw);
 }
 
+void sPort_send_GPS_TIME(int uart)
+{
+	sPort_send_data(uart, SMARTPORT_ID_GPS_TIME, gps_position->time_utc_usec);
+}
+
 void sPort_send_GPS_SPD(int uart)
 {
 	/* send 100 * knots */
@@ -274,3 +318,25 @@ void sPort_send_GPS_SPD(int uart)
 	uint32_t ispeed = (int)(1944 * speed);
 	sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
 }
+
+/*
+ * Sends nav_state + 128
+ */
+void sPort_send_NAV_STATE(int uart)
+{
+	uint32_t navstate = (int)(128 + vehicle_status->nav_state);
+
+	/* send data */
+	sPort_send_data(uart, SMARTPORT_ID_DIY_NAVSTATE, navstate);
+}
+
+// verified scaling
+// sends number of sats and type of gps fix
+void sPort_send_GPS_FIX(int uart)
+{
+	/* send data */
+	uint32_t satcount = (int)(gps_position->satellites_used);
+	uint32_t fixtype = (int)(gps_position->fix_type);
+	uint32_t t2 = satcount * 10 + fixtype;
+	sPort_send_data(uart, SMARTPORT_ID_DIY_GPSFIX, t2);
+}

src/drivers/frsky_telemetry/sPort_data.h

@@ -53,45 +53,55 @@
 #define SMARTPORT_POLL_5    0xB7
 #define SMARTPORT_POLL_6    0x00
 #define SMARTPORT_POLL_7    0x83
+#define SMARTPORT_POLL_8    0xBA
 
-/* FrSky SmartPort sensor IDs */
+/* FrSky SmartPort sensor IDs. See more here: https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h#L109 */
 #define SMARTPORT_ID_RSSI          0xf101
 #define SMARTPORT_ID_RXA1          0xf102	// supplied by RX
 #define SMARTPORT_ID_RXA2          0xf103	// supplied by RX
 #define SMARTPORT_ID_BATV          0xf104
-#define SMARTPORT_ID_SWR           0xf105
+#define SMARTPORT_ID_SWR           0xf105   // Standing Wave Ratio
 #define SMARTPORT_ID_T1            0x0400
 #define SMARTPORT_ID_T2            0x0410
 #define SMARTPORT_ID_RPM           0x0500
 #define SMARTPORT_ID_FUEL          0x0600
 #define SMARTPORT_ID_ALT           0x0100
-#define SMARTPORT_ID_VARIO         0x0110
+#define SMARTPORT_ID_VARIO         0x0110   //VSPEED
 #define SMARTPORT_ID_ACCX          0x0700
 #define SMARTPORT_ID_ACCY          0x0710
 #define SMARTPORT_ID_ACCZ          0x0720
 #define SMARTPORT_ID_CURR          0x0200
-#define SMARTPORT_ID_VFAS          0x0210
+#define SMARTPORT_ID_VFAS          0x0210  //Volt per Cell
 #define SMARTPORT_ID_CELLS         0x0300
 #define SMARTPORT_ID_GPS_LON_LAT   0x0800
 #define SMARTPORT_ID_GPS_ALT       0x0820
 #define SMARTPORT_ID_GPS_SPD       0x0830
 #define SMARTPORT_ID_GPS_CRS       0x0840
 #define SMARTPORT_ID_GPS_TIME      0x0850
+#define SMARTPORT_ID_DIY_FIRST     0x5000
+#define SMARTPORT_ID_DIY_LAST      0x50ff  //We have 256 possible ID's for custom values :)
+#define SMARTPORT_ID_DIY_NAVSTATE  0x5000
+#define SMARTPORT_ID_DIY_GPSFIX    0x5001
 
 // Public functions
 bool sPort_init(void);
 void sPort_deinit(void);
+void sPort_update_topics(void);
 void sPort_send_data(int uart, uint16_t id, uint32_t data);
 void sPort_send_BATV(int uart);
 void sPort_send_CUR(int uart);
 void sPort_send_ALT(int uart);
 void sPort_send_SPD(int uart);
 void sPort_send_VSPD(int uart, float speed);
+void sPort_send_FUEL(int uart);
 void sPort_send_GPS_LON(int uart);
 void sPort_send_GPS_LAT(int uart);
 void sPort_send_GPS_ALT(int uart);
-void sPort_send_GPS_COG(int uart);
 void sPort_send_GPS_SPD(int uart);
-void sPort_send_FUEL(int uart);
+void sPort_send_GPS_CRS(int uart);
+void sPort_send_GPS_TIME(int uart);
+
+void sPort_send_NAV_STATE(int uart);
+void sPort_send_GPS_FIX(int uart);
 
 #endif /* _SPORT_TELEMETRY_H */