apply 1st order lowpass filter to baro alt

src/drivers/frsky_telemetry/frsky_telemetry.c

@@ -57,6 +57,7 @@
 #include <systemlib/systemlib.h>
 #include <termios.h>
 #include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
 
 #include "sPort_data.h"
 #include "frsky_data.h"
@@ -213,6 +214,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 
 		warnx("sending FrSky SmartPort telemetry");
 
+		int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+
 		/* send S.port telemetry */
 		while (!thread_should_exit) {
 
@@ -239,6 +242,21 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 
 			newBytes = read(uart, &sbuf[1], 1);
 
+			/* get a local copy of the current sensor values
+			 * in order to apply a lowpass filter to baro pressure.
+			 */
+			static float last_baro_alt = 0;
+			struct sensor_combined_s raw;
+			memset(&raw, 0, sizeof(raw));
+			orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+
+			static float filtered_alt = NAN;
+			if (isnan(filtered_alt)) {
+				filtered_alt = raw.baro_alt_meter[0];
+			} else {
+				filtered_alt = .05f * raw.baro_alt_meter[0] + (1.0f -.05f) * filtered_alt;
+			}
+
 			// allow a minimum of 500usec before reply
 			usleep(500);
 
@@ -311,11 +329,17 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 
 			case SMARTPORT_POLL_6:
 
-				/* report vertical speed at 5Hz */
-				if (now - lastVSPD > 200 * 1000) {
+				/* report vertical speed at 10Hz */
+				if (now - lastVSPD > 100 * 1000) {
+					/* estimate vertical speed using first difference and delta t */
+					uint64_t dt = now - lastVSPD;
+					float speed  = (filtered_alt - last_baro_alt) / (1e-6f * (float)dt);
+
+					/* save current alt and timestamp */
+					last_baro_alt = filtered_alt;
 					lastVSPD = now;
-					/* send fuel */
-					sPort_send_VSPD(uart);
+
+					sPort_send_VSPD(uart, speed);
 				}
 
 				break;

src/drivers/frsky_telemetry/sPort_data.c

@@ -204,25 +204,8 @@ void sPort_send_SPD(int uart)
 }
 
 // TODO: verify scaling
-void sPort_send_VSPD(int uart)
+void sPort_send_VSPD(int uart, float speed)
 {
-	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);

src/drivers/frsky_telemetry/sPort_data.h

@@ -83,7 +83,8 @@ 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_VSPD(int uart, float speed);
+
 void sPort_send_FUEL(int uart);
 
 #endif /* _SPORT_TELEMETRY_H */