diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp index a19b51d8ec..8f15407435 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp @@ -42,6 +42,12 @@ extern const AP_HAL::HAL& hal; #define FPORT_TYPE_CONTROL 0 #define FPORT_TYPE_DOWNLINK 1 +#define FPORT_PRIM_NULL 0x00 +#define FPORT_PRIM_DATA 0x10 +#define FPORT_PRIM_READ 0x30 +#define FPORT_PRIM_WRITE 0x31 + +#define MAX_FPORT_CONSECUTIVE_FRAMES 2 struct PACKED FPort_Frame { uint8_t header; // 0x7E @@ -80,6 +86,18 @@ struct PACKED FPort_Frame { }; }; +struct { + bool available = false; + uint32_t data; + uint16_t appid; + uint8_t frame; +} telem_data; + +// receiver sends 0x10 when ready to receive telemetry frames (R-XSR) +bool rx_driven_frame_rate = false; +// if the receiver is not controlling frame rate apply a constraint on consecutive frames +uint8_t consecutive_telemetry_frame_count; + static_assert(sizeof(FPort_Frame) == FPORT_CONTROL_FRAME_SIZE, "FPort_Frame incorrect size"); // constructor @@ -130,10 +148,30 @@ void AP_RCProtocol_FPort::decode_control(const FPort_Frame &frame) void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame) { #if !APM_BUILD_TYPE(APM_BUILD_iofirmware) - if (frame.downlink.prim != 0x10 && frame.downlink.prim != 0x00) { - // only respond to frame types 0x00 or 0x10 - return; + switch (frame.downlink.prim) { + case FPORT_PRIM_DATA: + // we've seen at least one 0x10 frame + rx_driven_frame_rate = true; + break; + case FPORT_PRIM_NULL: + if (rx_driven_frame_rate) { + return; + } + // with 0x00 and no rx control we have a constraint + // on max consecutive frames + if (consecutive_telemetry_frame_count >= MAX_FPORT_CONSECUTIVE_FRAMES) { + consecutive_telemetry_frame_count = 0; + return; + } else { + consecutive_telemetry_frame_count++; + } + break; + case FPORT_PRIM_READ: + case FPORT_PRIM_WRITE: + // do not respond to 0x30 and 0x31 + return; } + /* if we are getting FPORT over a UART then we can ask the FrSky telem library for some passthrough data to send back, enabling @@ -146,6 +184,21 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame) return; } + /* + get SPort data from FRSky_Telem or send a null frame. + We save the data to a variable so in case we're late we'll + send it in the next call, this prevents corruption of + status text messages + */ + if (!telem_data.available) { + if (!AP_Frsky_Telem::get_telem_data(telem_data.frame, telem_data.appid, telem_data.data)) { + // nothing to send, send a null frame + telem_data.frame = 0x00; + telem_data.appid = 0x00; + telem_data.data = 0x00; + } + telem_data.available = true; + } /* check that we haven't been too slow in responding to the new UART data. If we respond too late then we will corrupt the next @@ -158,54 +211,45 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame) // we've been too slow in responding return; } + uint8_t buf[10]; - /* - get the SPort data from FRSky_Telem and send it as an uplink - packet - */ - uint8_t frametype; - uint16_t appid; - uint32_t data; - if (AP_Frsky_Telem::get_telem_data(frametype, appid, data)) { - uint8_t buf[10]; - - buf[0] = 0x08; - buf[1] = 0x81; - buf[2] = frametype; - buf[3] = appid & 0xFF; - buf[4] = appid >> 8; - memcpy(&buf[5], &data, 4); - - uint16_t sum = 0; - for (uint8_t i=0; i> 8; - sum &= 0xFF; - } - sum = 0xff - ((sum & 0xff) + (sum >> 8)); - buf[9] = sum; + buf[0] = 0x08; + buf[1] = 0x81; + buf[2] = telem_data.frame; + buf[3] = telem_data.appid & 0xFF; + buf[4] = telem_data.appid >> 8; + memcpy(&buf[5], &telem_data.data, 4); - // perform byte stuffing per FPort spec - uint8_t len = 0; - uint8_t buf2[sizeof(buf)*2+1]; + uint16_t sum = 0; + for (uint8_t i=0; i> 8; + sum &= 0xFF; + } + sum = 0xff - ((sum & 0xff) + (sum >> 8)); + buf[9] = (uint8_t)sum; - if (rc().fport_pad()) { - // this padding helps on some uarts that have hw pullups - buf2[len++] = 0xff; - } + // perform byte stuffing per FPort spec + uint8_t len = 0; + uint8_t buf2[sizeof(buf)*2+1]; - for (uint8_t i=0; iwrite(buf2, len); + for (uint8_t i=0; iwrite(buf2, len); + // get fresh telem_data in the next call + telem_data.available = false; #endif } @@ -306,6 +350,8 @@ bool AP_RCProtocol_FPort::check_checksum(void) uint16_t sum = 0; for (uint8_t i=0; i> 8; + sum &= 0xFF; } sum = (sum & 0xff) + (sum >> 8); return sum == 0xff;