diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index d8650c382e..8b806451eb 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -45,8 +45,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -285,3 +287,61 @@ void frsky_send_frame3(int uart) frsky_send_startstop(uart); } + +/* parse 11 byte frames */ +bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v) +{ + bool data_ready = false; + static int dcount = 0; + static uint8_t type = 0; + static uint8_t data[11]; + static enum { + HEADER = 0, + TYPE, + DATA, + TRAILER + } state = HEADER; + + for (int i=0; iad1 = data[0]; + v->ad2 = data[1]; + v->linkq = data[2]; + } + } + break; + } + } + return data_ready; +} + diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h index a7d9eee533..4ece886055 100644 --- a/src/drivers/frsky_telemetry/frsky_data.h +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -42,10 +42,19 @@ #ifndef _FRSKY_DATA_H #define _FRSKY_DATA_H +#include + // Public functions void frsky_init(void); void frsky_send_frame1(int uart); void frsky_send_frame2(int uart); void frsky_send_frame3(int uart); +struct adc_linkquality { + uint8_t ad1; + uint8_t ad2; + uint8_t linkq; +}; +bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v); + #endif /* _FRSKY_TELEMETRY_H */ diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 91dc9011ef..385b57e4ec 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -332,23 +332,35 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) frsky_init(); warnx("sending FrSky D type telemetry"); + struct adc_linkquality host_frame; + uint8_t dbuf[45]; /* send D8 mode telemetry */ while (!thread_should_exit) { - /* Sleep 200 ms */ - usleep(200000); + /* Sleep 100 ms */ + usleep(100000); + + /* parse incoming data */ + int nbytes = read(uart, &dbuf[0], sizeof(dbuf)); + bool new_input = frsky_parse_host(&dbuf[0], nbytes, &host_frame); + if (new_input) { +// warnx("host frame: ad1:%u, ad2: %u, rssi: %u", +// host_frame.ad1, host_frame.ad2, host_frame.linkq); + } /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ - frsky_send_frame1(uart); + if (iteration % 2 == 0) { + frsky_send_frame1(uart); + } /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ - if (iteration % 5 == 0) { + if (iteration % 10 == 0) { frsky_send_frame2(uart); } /* Send frame 3 (every 5000ms): date, time */ - if (iteration % 25 == 0) { + if (iteration % 50 == 0) { frsky_send_frame3(uart); iteration = 0; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index dc4a0cfd56..02d8eb7d53 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -713,9 +713,9 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count, _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; _rc_in.rc_ppm_frame_length = 0; - if (rssi == -1) { - _rc_in.rssi = - (!frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); + /* don't touch rssi if no value was provided */ + if (rssi >= 0) { + _rc_in.rssi = rssi; } _rc_in.rc_failsafe = failsafe;