initial add of Vario sensor. 1st difference of baro_alt... need to add a

LPF in front of the difference

src/drivers/frsky_telemetry/frsky_telemetry.c

@@ -247,6 +247,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 			static hrt_abstime lastALT = 0;
 			static hrt_abstime lastSPD = 0;
 			static hrt_abstime lastFUEL = 0;
+			static hrt_abstime lastVSPD = 0;
 
 			switch (sbuf[1]) {
 
@@ -306,6 +307,17 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 					sPort_send_FUEL(uart);
 				}
 
+				break;
+
+			case SMARTPORT_POLL_6:
+
+				/* report vertical speed at 5Hz */
+				if (now - lastVSPD > 200 * 1000) {
+					lastVSPD = now;
+					/* send fuel */
+					sPort_send_VSPD(uart);
+				}
+
 				break;
 			}
 		}

src/drivers/frsky_telemetry/sPort_data.c

@@ -203,6 +203,31 @@ void sPort_send_SPD(int uart)
 	sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
 }
 
+// TODO: verify scaling
+void sPort_send_VSPD(int uart)
+{
+	static uint64_t last_baro_time = 0;
+	static float last_baro_alt = 0;
+
+	/* 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);
+
+	/* estimate vertical speed using first difference and delta t */
+	uint64_t baro_time = raw.baro_timestamp[0];
+	uint64_t dt = baro_time - last_baro_time;
+	float speed  = (raw.baro_alt_meter[0] - last_baro_alt) / (1e-6f * (float)dt);
+
+	/* save current alt and timestamp */
+	last_baro_alt = raw.baro_alt_meter[0];
+	last_baro_time = baro_time;
+
+	/* send data for VARIO vertical speed: int16 cm/sec */
+	int32_t ispeed = (int)(100 * speed);
+	sPort_send_data(uart, SMARTPORT_ID_VARIO, ispeed);
+}
+
 // verified scaling
 void sPort_send_FUEL(int uart)
 {

src/drivers/frsky_telemetry/sPort_data.h

@@ -50,6 +50,7 @@
 #define SMARTPORT_POLL_3    0x95
 #define SMARTPORT_POLL_4    0x16
 #define SMARTPORT_POLL_5    0xB7
+#define SMARTPORT_POLL_6    0x00
 
 /* FrSky SmartPort sensor IDs */
 #define SMARTPORT_ID_RSSI          0xf101
@@ -82,6 +83,7 @@ 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);
 void sPort_send_FUEL(int uart);
 
 #endif /* _SPORT_TELEMETRY_H */