diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index ba8ba2e387..c6ea2303e1 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -57,6 +57,7 @@ #include #include #include +#include #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; diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c index a1d25935b8..b079c2a00d 100644 --- a/src/drivers/frsky_telemetry/sPort_data.c +++ b/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); diff --git a/src/drivers/frsky_telemetry/sPort_data.h b/src/drivers/frsky_telemetry/sPort_data.h index cc98035e42..f3e49e8c7d 100644 --- a/src/drivers/frsky_telemetry/sPort_data.h +++ b/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 */