diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index c42ea90ffc..c150b85bd8 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -27,6 +27,7 @@ class AP_RCProtocol { public: AP_RCProtocol() {} ~AP_RCProtocol(); + friend class AP_RCProtocol_Backend; enum rcprotocol_t { PPM = 0, diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 684325f64d..41154bdf14 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -55,6 +55,17 @@ public: int16_t get_RSSI(void) const { return rssi; } + + // get UART for RCIN, if available. This will return false if we + // aren't getting the active RC input protocol via the uart + AP_HAL::UARTDriver *get_UART(void) const { + return frontend._detected_with_bytes?frontend.added.uart:nullptr; + } + + // return true if we have a uart available for protocol handling. + bool have_UART(void) const { + return frontend.added.uart != nullptr; + } protected: void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe, int16_t rssi=-1); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp index 73f3962f96..1623c95e04 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp @@ -12,11 +12,20 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . */ +/* + FRSky FPort implementation, with thanks to BetaFlight for + specification and code reference + */ #include "AP_RCProtocol_FPort.h" #include +#include + +extern const AP_HAL::HAL& hal; #define FRAME_HEAD 0x7E +#define FRAME_DLE 0x7D +#define FRAME_XOR 0x20 #define FRAME_LEN_CONTROL 0x19 #define FRAME_LEN_DOWNLINK 0x08 #define MIN_FRAME_SIZE 12 @@ -108,10 +117,86 @@ void AP_RCProtocol_FPort::decode_control(const FPort_Frame &frame) add_input(MAX_CHANNELS, values, failsafe, frame.control.rssi); } -// decode a full FPort downlink frame +/* + decode a full FPort downlink frame +*/ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame) { - // not implemented yet +#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; + } + /* + if we are getting FPORT over a UART then we can ask the FrSky + telem library for some passthrough data to send back, enabling + telemetry on the receiver via the same uart pin as we use for + incoming RC frames + */ + + AP_HAL::UARTDriver *uart = get_UART(); + if (!uart) { + return; + } + + AP_Frsky_Telem *frsky = AP::frsky_telem(); + if (!frsky) { + return; + } + + /* + 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 + incoming control frame + */ + uint64_t tend = uart->receive_time_constraint_us(1); + uint64_t now = AP_HAL::micros64(); + uint64_t tdelay = now - tend; + if (tdelay > 2500) { + // we've been too slow in responding + return; + } + + /* + get the SPort data from FRSky_Telem and send it as an uplink + packet + */ + uint8_t frametype; + uint16_t appid; + uint32_t data; + if (frsky->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)); + buf[9] = sum; + + // perform byte stuffing per FPort spec + uint8_t len = 0; + uint8_t buf2[sizeof(buf)*2]; + for (uint8_t i=0; iwrite(buf2, len); + } +#endif } /* @@ -119,6 +204,11 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame) */ void AP_RCProtocol_FPort::process_pulse(uint32_t width_s0, uint32_t width_s1) { + if (have_UART()) { + // if we can use a UART we would much prefer to, as it allows + // us to send SPORT data out + return; + } uint32_t w0 = width_s0; uint32_t w1 = width_s1; if (inverted) { @@ -143,24 +233,19 @@ void AP_RCProtocol_FPort::_process_byte(uint32_t timestamp_us, uint8_t b) // if we have a frame gap then this must be the start of a new // frame byte_input.ofs = 0; - byte_input.got_7D = false; + byte_input.got_DLE = false; } if (b != FRAME_HEAD && byte_input.ofs == 0) { // definately not FPort, missing header byte return; } - if (byte_input.ofs == 0 && !have_frame_gap) { - // must have a frame gap before the start of a new FPort frame - return; - } // handle byte-stuffing decode - if (byte_input.got_7D) { - b ^= 0x20; - byte_input.got_7D = false; - } - if (b == 0x7D) { - byte_input.got_7D = true; + if (byte_input.got_DLE) { + b ^= FRAME_XOR; + byte_input.got_DLE = false; + } else if (b == FRAME_DLE) { + byte_input.got_DLE = true; return; } @@ -200,7 +285,7 @@ void AP_RCProtocol_FPort::_process_byte(uint32_t timestamp_us, uint8_t b) reset: byte_input.ofs = 0; - byte_input.got_7D = false; + byte_input.got_DLE = false; } // check checksum byte diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.h b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.h index 8c4e04ea88..352ee4568e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.h @@ -43,7 +43,7 @@ private: uint8_t buf[FPORT_CONTROL_FRAME_SIZE]; uint8_t ofs; uint32_t last_byte_us; - bool got_7D; + bool got_DLE; } byte_input; const bool inverted;