add sPort GPS telemetry

src/drivers/frsky_telemetry/frsky_telemetry.c

@@ -268,6 +268,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 			static hrt_abstime lastSPD = 0;
 			static hrt_abstime lastFUEL = 0;
 			static hrt_abstime lastVSPD = 0;
+			static hrt_abstime lastGPS = 0;
 
 			switch (sbuf[1]) {
 
@@ -344,6 +345,44 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 					sPort_send_VSPD(uart, speed);
 				}
 
+				break;
+
+			case SMARTPORT_POLL_7:
+
+				/* report GPS data elements at 2*5Hz */
+				if (now - lastGPS > 100 * 1000) {
+					static int elementCount = 0;
+
+					switch (elementCount) {
+
+					case 0:
+						sPort_send_GPS_LON(uart);
+						elementCount++;
+						break;
+
+					case 1:
+						sPort_send_GPS_LAT(uart);
+						elementCount++;
+						break;
+
+					case 2:
+						sPort_send_GPS_COG(uart);
+						elementCount++;
+						break;
+
+					case 3:
+						sPort_send_GPS_ALT(uart);
+						elementCount++;
+						break;
+
+					case 4:
+						sPort_send_GPS_SPD(uart);
+						elementCount = 0;
+						break;
+					}
+
+				}
+
 				break;
 			}
 		}

src/drivers/frsky_telemetry/sPort_data.c

@@ -58,31 +58,26 @@
 
 #define frac(f) (f - (int)f)
 
-static int battery_sub = -1;
+//static int battery_sub = -1;
 static int sensor_sub = -1;
 static int global_position_sub = -1;
 static int vehicle_status_sub = -1;
 
+static struct vehicle_status_s vehicle_status;
+static struct sensor_combined_s sensor_data;
+static struct vehicle_global_position_s global_pos;
+
 /**
  * Initializes the uORB subscriptions.
  */
 void sPort_init()
 {
-	battery_sub = orb_subscribe(ORB_ID(battery_status));
+//	battery_sub = orb_subscribe(ORB_ID(battery_status));
 	global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 }
 
-/**
- * Sends a 0x10 start byte.
- */
-//static void sPort_send_start(int uart)
-//{
-//	static const uint8_t c = 0x10;
-//	write(uart, &c, 1);
-//}
-
 static void update_crc(uint16_t *crc, unsigned char b)
 {
 	*crc += b;
@@ -152,7 +147,6 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
 void sPort_send_BATV(int uart)
 {
 	/* get a local copy of the vehicle status data */
-	struct vehicle_status_s vehicle_status;
 	memset(&vehicle_status, 0, sizeof(vehicle_status));
 	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
 
@@ -165,7 +159,6 @@ void sPort_send_BATV(int uart)
 void sPort_send_CUR(int uart)
 {
 	/* get a local copy of the vehicle status data */
-	struct vehicle_status_s vehicle_status;
 	memset(&vehicle_status, 0, sizeof(vehicle_status));
 	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
 
@@ -180,12 +173,11 @@ void sPort_send_CUR(int uart)
 void sPort_send_ALT(int uart)
 {
 	/* get a local copy of the current sensor values */
-	struct sensor_combined_s raw;
-	memset(&raw, 0, sizeof(raw));
-	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+	memset(&sensor_data, 0, sizeof(sensor_data));
+	orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor_data);
 
 	/* send data */
-	uint32_t alt = (int)(100 * raw.baro_alt_meter[0]);
+	uint32_t alt = (int)(100 * sensor_data.baro_alt_meter[0]);
 	sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
 }
 
@@ -193,11 +185,10 @@ void sPort_send_ALT(int uart)
 void sPort_send_SPD(int uart)
 {
 	/* get a local copy of the global position data */
-	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
 	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
 
-	/* send data for A2 */
+	/* send data */
 	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);
@@ -215,7 +206,6 @@ void sPort_send_VSPD(int uart, float speed)
 void sPort_send_FUEL(int uart)
 {
 	/* get a local copy of the vehicle status data */
-	struct vehicle_status_s vehicle_status;
 	memset(&vehicle_status, 0, sizeof(vehicle_status));
 	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
 
@@ -223,3 +213,67 @@ void sPort_send_FUEL(int uart)
 	uint32_t fuel = (int)(100 * vehicle_status.battery_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 */
+	memset(&global_pos, 0, sizeof(global_pos));
+	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);
+	iLon |= (1 << 31);
+	if (global_pos.lon < 0) { iLon |= (1 << 30); }
+	sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon);
+}
+
+void sPort_send_GPS_LAT(int uart)
+{
+	/* send latitude */
+	/* get a local copy of the global position data */
+	memset(&global_pos, 0, sizeof(global_pos));
+	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
+
+	/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */
+	uint32_t iLat = 6E5 * fabs(global_pos.lat);
+	if (global_pos.lat < 0) { iLat |= (1 << 30); }
+	sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat);
+}
+
+void sPort_send_GPS_ALT(int uart)
+{
+	/* send altitude */
+	/* get a local copy of the global position data */
+	memset(&global_pos, 0, sizeof(global_pos));
+	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
+
+	/* convert to 100 * m/sec */
+	uint32_t iAlt = 100 * global_pos.alt;
+	sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt);
+}
+
+void sPort_send_GPS_COG(int uart)
+{
+	/* send course */
+	/* get a local copy of the global position data */
+	memset(&global_pos, 0, sizeof(global_pos));
+	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 */
+	uint32_t iYaw = 100 * global_pos.yaw;
+	sPort_send_data(uart, SMARTPORT_ID_GPS_CRS, iYaw);
+}
+
+void sPort_send_GPS_SPD(int uart)
+{
+	/* get a local copy of the global position data */
+	memset(&global_pos, 0, sizeof(global_pos));
+	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
+
+	/* send 100 * knots */
+	float speed  = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
+	uint32_t ispeed = (int)(1944 * speed);
+	sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
+}

src/drivers/frsky_telemetry/sPort_data.h

@@ -51,6 +51,7 @@
 #define SMARTPORT_POLL_4    0x16
 #define SMARTPORT_POLL_5    0xB7
 #define SMARTPORT_POLL_6    0x00
+#define SMARTPORT_POLL_7    0x83
 
 /* FrSky SmartPort sensor IDs */
 #define SMARTPORT_ID_RSSI          0xf101
@@ -84,7 +85,11 @@ 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_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);
 
 #endif /* _SPORT_TELEMETRY_H */