diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 61808172bb..ba8ba2e387 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/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; } } diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c index 6af5ecc09e..a1d25935b8 100644 --- a/src/drivers/frsky_telemetry/sPort_data.c +++ b/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) { diff --git a/src/drivers/frsky_telemetry/sPort_data.h b/src/drivers/frsky_telemetry/sPort_data.h index 8173780b88..cc98035e42 100644 --- a/src/drivers/frsky_telemetry/sPort_data.h +++ b/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 */