From 988af83fce88c7255dc667d6d44ec087585ed81f Mon Sep 17 00:00:00 2001 From: yaapu Date: Thu, 10 Sep 2020 14:55:53 +0200 Subject: [PATCH] AP_Frsky_Telem: added support for frsky sport/fport bidirectional telemetry --- libraries/AP_Frsky_Telem/AP_Frsky_Backend.h | 4 + libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp | 323 +++++++++++++ libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.h | 74 +++ .../AP_Frsky_Telem/AP_Frsky_Parameters.cpp | 49 ++ .../AP_Frsky_Telem/AP_Frsky_Parameters.h | 41 ++ libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp | 139 ++++++ libraries/AP_Frsky_Telem/AP_Frsky_SPort.h | 10 + .../AP_Frsky_Telem/AP_Frsky_SPortParser.h | 49 ++ .../AP_Frsky_SPort_Passthrough.cpp | 456 +++++++++++++++++- .../AP_Frsky_SPort_Passthrough.h | 67 ++- libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 46 +- libraries/AP_Frsky_Telem/AP_Frsky_Telem.h | 17 + 12 files changed, 1256 insertions(+), 19 deletions(-) create mode 100644 libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp create mode 100644 libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.h create mode 100644 libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp create mode 100644 libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h create mode 100644 libraries/AP_Frsky_Telem/AP_Frsky_SPortParser.h diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h index 7468c1b457..066b4a2ce9 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h @@ -26,6 +26,10 @@ public: { return false; } + virtual bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) + { + return false; + } virtual void queue_text_message(MAV_SEVERITY severity, const char *text) { } diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp new file mode 100644 index 0000000000..3072dab31e --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp @@ -0,0 +1,323 @@ +#include +#include + +#include "AP_Frsky_MAVlite.h" + +extern const AP_HAL::HAL& hal; + +void AP_Frsky_MAVlite::update_checksum(mavlite_message_t &msg, const uint8_t c) +{ + msg.checksum += c; //0-1FF + msg.checksum += msg.checksum >> 8; + msg.checksum &= 0xFF; +} + +void AP_Frsky_MAVlite::parser_reset(void) +{ + _rxmsg.checksum = 0; + _rxmsg.len = 0; + _rxmsg.msgid = 0; + + _rxstatus.current_rx_seq = 0; + _rxstatus.payload_next_byte = 0; + _rxstatus.parse_state = ParseState::IDLE; +} + +void AP_Frsky_MAVlite::encoder_reset(mavlite_message_t &txmsg) +{ + txmsg.checksum = 0; + + _txstatus.current_rx_seq = 0; + _txstatus.payload_next_byte = 0; + _txstatus.parse_state = ParseState::IDLE; +} + +/* + Parses sport packets and if successfull fills the rxmsg mavlite struct + */ +bool AP_Frsky_MAVlite::parse(mavlite_message_t &rxmsg, const AP_Frsky_SPort::sport_packet_t packet) +{ + for (uint8_t i=0; i<6; i++) { + parse(packet.raw[i+2], i); + } + if (_rxstatus.parse_state == ParseState::MESSAGE_RECEIVED) { + rxmsg = _rxmsg; + return true; + } + return false; +} + +/* + Warning: + make sure that all packets pushed by this method are sequential and not interleaved by packets inserted by another thread! + */ +bool AP_Frsky_MAVlite::encode(ObjectBuffer_TS &queue, mavlite_message_t &msg) +{ + // let's check if there's enough room to send it + if (queue.space() < MAVLITE_MSG_SPORT_PACKETS_COUNT(msg.len)) { + return false; + } + encoder_reset(msg); + // prevent looping forever + uint8_t packet_count = 0; + while (_txstatus.parse_state != ParseState::MESSAGE_RECEIVED && packet_count++ < MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN)) { + + AP_Frsky_SPort::sport_packet_t packet {0}; + + for (uint8_t i=0; i<6; i++) { + // read msg and fill packet one byte at the time, ignore sensor and frame byte + encode(packet.raw[i+2], i, msg); + } + + if (_txstatus.parse_state == ParseState::ERROR) { + break; + } + + queue.push(packet); + } + _txstatus.parse_state = ParseState::IDLE; + return true; +} + +bool AP_Frsky_MAVlite::parse(uint8_t byte, uint8_t offset) +{ + switch (_rxstatus.parse_state) { + case ParseState::IDLE: + case ParseState::ERROR: + if ( offset == 0 && byte == 0x00 ) { + parser_reset(); + _rxstatus.parse_state = ParseState::GOT_START; + } else { + _rxstatus.parse_state = ParseState::ERROR; + } + break; + case ParseState::GOT_START: + if ( offset == 0 && byte == 0x00 ) { + parser_reset(); + _rxstatus.parse_state = ParseState::GOT_START; + } else { + _rxmsg.len = byte; + _rxstatus.parse_state = ParseState::GOT_LEN; + update_checksum(_rxmsg, byte); + } + break; + case ParseState::GOT_LEN: + if ( offset == 0 && byte == 0x00 ) { + parser_reset(); + _rxstatus.parse_state = ParseState::GOT_START; + } else { + _rxmsg.msgid = byte; + _rxstatus.parse_state = ParseState::GOT_MSGID; + update_checksum(_rxmsg, byte); + } + break; + case ParseState::GOT_MSGID: + if ( offset == 0 && byte == 0x00 ) { + parser_reset(); + _rxstatus.parse_state = ParseState::GOT_START; + } else { + _rxmsg.payload[_rxstatus.payload_next_byte++] = byte; + _rxstatus.parse_state = ParseState::GOT_PAYLOAD; + update_checksum(_rxmsg, byte); + } + break; + case ParseState::GOT_SEQ: + if ( offset == 0 && byte == 0x00 ) { + parser_reset(); + _rxstatus.parse_state = ParseState::GOT_START; + } else { + if ( _rxstatus.payload_next_byte < _rxmsg.len ) { + _rxmsg.payload[_rxstatus.payload_next_byte++] = byte; + _rxstatus.parse_state = ParseState::GOT_PAYLOAD; + update_checksum(_rxmsg, byte); + } else { + if ( _rxmsg.checksum == byte ) { + _rxstatus.parse_state = ParseState::MESSAGE_RECEIVED; + return true; + } else { + _rxstatus.parse_state = ParseState::ERROR; + } + } + } + break; + case ParseState::GOT_PAYLOAD: + if ( offset == 0) { + if ( byte == 0x00 ) { + parser_reset(); + _rxstatus.parse_state = ParseState::GOT_START; + } else { + if ((byte & 0x3F) != _rxstatus.current_rx_seq + 1) { + _rxstatus.parse_state = ParseState::ERROR; + } else { + _rxstatus.current_rx_seq = (byte & 0x3F); + _rxstatus.parse_state = ParseState::GOT_SEQ; + } + update_checksum(_rxmsg, byte); + } + } else { + if ( _rxstatus.payload_next_byte < _rxmsg.len ) { + _rxmsg.payload[_rxstatus.payload_next_byte++] = byte; + update_checksum(_rxmsg, byte); + } else { + if ( _rxmsg.checksum == byte ) { + _rxstatus.parse_state = ParseState::MESSAGE_RECEIVED; + return true; + } else { + _rxstatus.parse_state = ParseState::ERROR; + } + } + } + break; + case ParseState::MESSAGE_RECEIVED: + if ( offset == 0 && byte == 0x00 ) { + parser_reset(); + _rxstatus.parse_state = ParseState::GOT_START; + } + break; + } + return false; +} + +bool AP_Frsky_MAVlite::encode(uint8_t &byte, const uint8_t offset, mavlite_message_t &txmsg) +{ + switch (_txstatus.parse_state) { + case ParseState::IDLE: + case ParseState::ERROR: + if ( offset == 0 ) { + byte = 0x00; + encoder_reset(txmsg); + _txstatus.parse_state = ParseState::GOT_START; + } else { + _txstatus.parse_state = ParseState::ERROR; + } + break; + case ParseState::GOT_START: + byte = txmsg.len; + _txstatus.parse_state = ParseState::GOT_LEN; + update_checksum(txmsg, byte); + break; + case ParseState::GOT_LEN: + byte = txmsg.msgid; + _txstatus.parse_state = ParseState::GOT_MSGID; + update_checksum(txmsg, byte); + break; + case ParseState::GOT_MSGID: + byte = txmsg.payload[_txstatus.payload_next_byte++]; + _txstatus.parse_state = ParseState::GOT_PAYLOAD; + update_checksum(txmsg, byte); + break; + case ParseState::GOT_SEQ: + if ( _txstatus.payload_next_byte < txmsg.len ) { + byte = txmsg.payload[_txstatus.payload_next_byte++]; + _txstatus.parse_state = ParseState::GOT_PAYLOAD; + update_checksum(txmsg, byte); + } else { + byte = txmsg.checksum; + _txstatus.parse_state = ParseState::MESSAGE_RECEIVED; + return true; + } + break; + case ParseState::GOT_PAYLOAD: + if ( offset == 0) { + byte = ++_txstatus.current_rx_seq; + update_checksum(txmsg, byte); + _txstatus.parse_state = ParseState::GOT_SEQ; + } else { + if ( _txstatus.payload_next_byte < txmsg.len ) { + byte = txmsg.payload[_txstatus.payload_next_byte++]; + update_checksum(txmsg, byte); + } else { + byte = (uint8_t)txmsg.checksum; + _txstatus.parse_state = ParseState::MESSAGE_RECEIVED; + return true; + } + } + break; + case ParseState::MESSAGE_RECEIVED: + break; + } + return false; +} + +bool AP_Frsky_MAVlite::mavlite_msg_get_bytes(uint8_t *bytes, const mavlite_message_t &msg, const uint8_t offset, const uint8_t count) +{ + if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) { + return false; + } + memcpy(bytes, &msg.payload[offset], count); + return true; +} + +bool AP_Frsky_MAVlite::mavlite_msg_set_bytes(mavlite_message_t &msg, const uint8_t *bytes, const uint8_t offset, const uint8_t count) +{ + if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) { + return false; + } + memcpy(&msg.payload[offset], bytes, count); + msg.len += count; + return true; +} + + +bool AP_Frsky_MAVlite::mavlite_msg_get_float(float &value, const mavlite_message_t &msg, const uint8_t offset) +{ + return mavlite_msg_get_bytes((uint8_t*)&value, msg, offset, 4); +} + +bool AP_Frsky_MAVlite::mavlite_msg_get_uint16(uint16_t &value, const mavlite_message_t &msg, const uint8_t offset) +{ + return mavlite_msg_get_bytes((uint8_t*)&value, msg, offset, 2); +} + +bool AP_Frsky_MAVlite::mavlite_msg_get_uint8(uint8_t &value, const mavlite_message_t &msg, const uint8_t offset) +{ + return mavlite_msg_get_bytes((uint8_t*)&value, msg, offset, 1); +} + +uint8_t AP_Frsky_MAVlite::bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset) +{ + uint8_t mask = 0; + for (uint8_t i=bit_offset; i<=bit_count; i++) { + mask |= 1 << i; + } + return (value & mask) >> bit_offset; +} + +void AP_Frsky_MAVlite::bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset) +{ + uint8_t mask = 0; + for (uint8_t i=bit_offset; i<=bit_count; i++) { + mask |= 1 << i; + } + value |= (bit_value< + +#include "AP_Frsky_SPort.h" + +#include + +#define MAVLITE_MAX_PAYLOAD_LEN 31 // 7 float params + cmd_id + options +#define MAVLITE_MSG_SPORT_PACKETS_COUNT(LEN) static_cast(1 + ceilf((LEN-2)/5.0f)) // number of sport packets required to transport a message with LEN payload +#define SPORT_PACKET_QUEUE_LENGTH static_cast(30U*MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN)) + +class AP_Frsky_MAVlite +{ +public: + typedef struct { + uint8_t msgid = 0; // ID of message in payload + uint8_t len = 0; // Length of payload + uint8_t payload[MAVLITE_MAX_PAYLOAD_LEN]; + int16_t checksum = 0; // sent at end of packet + } mavlite_message_t; + + // public parsing methods + bool parse(mavlite_message_t &rxmsg, const AP_Frsky_SPort::sport_packet_t packet) ; + bool encode(ObjectBuffer_TS &queue, mavlite_message_t &msg); + + // public mavlite message helpers + static bool mavlite_msg_get_float(float &value, const mavlite_message_t &msg, const uint8_t offset); + static bool mavlite_msg_set_float(mavlite_message_t &msg, const float value, const uint8_t offset); + + static bool mavlite_msg_get_string(char* value, const mavlite_message_t &msg, const uint8_t offset); + static bool mavlite_msg_set_string(mavlite_message_t &msg, const char* value, const uint8_t offset); + + static bool mavlite_msg_get_uint16(uint16_t &value, const mavlite_message_t &msg, const uint8_t offset); + static bool mavlite_msg_set_uint16(mavlite_message_t &msg, const uint16_t value, const uint8_t offset); + + static bool mavlite_msg_get_uint8(uint8_t &value, const mavlite_message_t &msg, const uint8_t offset); + static bool mavlite_msg_set_uint8(mavlite_message_t &msg, const uint8_t value, const uint8_t offset); + + static void bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset); + static uint8_t bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset); + +private: + enum class ParseState : uint8_t { + IDLE=0, + ERROR, + GOT_START, + GOT_LEN, + GOT_SEQ, + GOT_MSGID, + GOT_PAYLOAD, + MESSAGE_RECEIVED, + }; // state machine for mavlite messages + + typedef struct { + ParseState parse_state = ParseState::IDLE; + uint8_t current_rx_seq = 0; + uint8_t payload_next_byte = 0; + } mavlite_status_t; + + mavlite_status_t _txstatus; + mavlite_status_t _rxstatus; + + mavlite_message_t _rxmsg; + + static bool mavlite_msg_get_bytes(uint8_t *bytes, const mavlite_message_t &msg, const uint8_t offset, const uint8_t count); + static bool mavlite_msg_set_bytes(mavlite_message_t &msg, const uint8_t *bytes, const uint8_t offset, const uint8_t count); + + void encoder_reset(mavlite_message_t &txmsg); + void parser_reset(); + bool parse(const uint8_t byte, const uint8_t offset); + bool encode(uint8_t &byte, uint8_t offset, mavlite_message_t &txmsg); + void update_checksum(mavlite_message_t &msg, const uint8_t c); +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp new file mode 100644 index 0000000000..dc672674c3 --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp @@ -0,0 +1,49 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#include "AP_Frsky_Parameters.h" + +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + +const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = { + // @Param: UPLINK_ID + // @DisplayName: Uplink sensor id + // @Description: Change the uplink sensor id (SPort only) + // @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26 + // @User: Advanced + AP_GROUPINFO("UPLINK_ID", 1, AP_Frsky_Parameters, _uplink_id, 13), + + // @Param: DNLINK1_ID + // @DisplayName: First downlink sensor id + // @Description: Change the first extra downlink sensor id (SPort only) + // @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26 + // @User: Advanced + AP_GROUPINFO("DNLINK1_ID", 2, AP_Frsky_Parameters, _dnlink1_id, 20), + + // @Param: DNLINK2_ID + // @DisplayName: Second downlink sensor id + // @Description: Change the second extra downlink sensor id (SPort only) + // @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26 + // @User: Advanced + AP_GROUPINFO("DNLINK2_ID", 3, AP_Frsky_Parameters, _dnlink2_id, 7), + + AP_GROUPEND +}; + +AP_Frsky_Parameters::AP_Frsky_Parameters() +{ + AP_Param::setup_object_defaults(this, var_info); +} + +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h new file mode 100644 index 0000000000..b4fd2a52a9 --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h @@ -0,0 +1,41 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#pragma once + +#include "AP_Frsky_Telem.h" + +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL +#include +#include + +class AP_Frsky_Telem; + +class AP_Frsky_Parameters +{ + friend class AP_Frsky_SPort_Passthrough; +public: + AP_Frsky_Parameters(); + + // parameters + static const struct AP_Param::GroupInfo var_info[]; + +private: + // settable parameters + AP_Int8 _uplink_id; + AP_Int8 _dnlink1_id; + AP_Int8 _dnlink2_id; +}; + +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp index 43c467c4b6..7d5c56b9e5 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp @@ -1,9 +1,14 @@ #include "AP_Frsky_SPort.h" +#include #include #include #include +#include "AP_Frsky_SPortParser.h" + +#include + /* * send telemetry data * for FrSky SPort protocol (X-receivers) @@ -204,3 +209,137 @@ void AP_Frsky_SPort::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t d #endif _port->write(buf2, len); } + +extern const AP_HAL::HAL& hal; + +bool AP_Frsky_SPortParser::should_process_packet(const uint8_t *packet, bool discard_duplicates) +{ + // check for duplicate packets + if (discard_duplicates && _parse_state.last_packet != nullptr) { + /* + Note: the polling byte packet[0] should be ignored in the comparison + because we might get the same packet with different polling bytes + We have 2 types of duplicate packets: ghost identical packets sent by the receiver + and user duplicate packets sent to compensate for bad link and frame loss, this + check should address both types. + */ + if (memcmp(&packet[1], &_parse_state.last_packet[1], SPORT_PACKET_SIZE-1) == 0) { + return false; + } + memcpy(_parse_state.last_packet, packet, SPORT_PACKET_SIZE); + } + //check CRC + int16_t crc = 0; + for (uint8_t i=1; i> 8; // 0-1FF + crc &= 0x00ff; // 0-FF + } + return (crc == 0x00ff); +} + +bool AP_Frsky_SPortParser::process_byte(AP_Frsky_SPort::sport_packet_t &sport_packet, const uint8_t data) +{ + switch (_parse_state.state) { + case ParseState::START: + if (_parse_state.rx_buffer_count < TELEMETRY_RX_BUFFER_SIZE) { + _parse_state.rx_buffer[_parse_state.rx_buffer_count++] = data; + } + _parse_state.state = ParseState::IN_FRAME; + break; + + case ParseState::IN_FRAME: + if (data == FRAME_DLE) { + _parse_state.state = ParseState::XOR; // XOR next byte + } else if (data == FRAME_HEAD) { + _parse_state.state = ParseState::IN_FRAME ; + _parse_state.rx_buffer_count = 0; + break; + } else if (_parse_state.rx_buffer_count < TELEMETRY_RX_BUFFER_SIZE) { + _parse_state.rx_buffer[_parse_state.rx_buffer_count++] = data; + } + break; + + case ParseState::XOR: + if (_parse_state.rx_buffer_count < TELEMETRY_RX_BUFFER_SIZE) { + _parse_state.rx_buffer[_parse_state.rx_buffer_count++] = data ^ STUFF_MASK; + } + _parse_state.state = ParseState::IN_FRAME; + break; + + case ParseState::IDLE: + if (data == FRAME_HEAD) { + _parse_state.rx_buffer_count = 0; + _parse_state.state = ParseState::START; + } + break; + + } // switch + + if (_parse_state.rx_buffer_count >= SPORT_PACKET_SIZE) { + _parse_state.rx_buffer_count = 0; + _parse_state.state = ParseState::IDLE; + // feed the packet only if it's not a duplicate + return get_packet(sport_packet, true); + } + return false; +} + +bool AP_Frsky_SPortParser::get_packet(AP_Frsky_SPort::sport_packet_t &sport_packet, bool discard_duplicates) +{ + if (!should_process_packet(_parse_state.rx_buffer, discard_duplicates)) { + return false; + } + + const AP_Frsky_SPort::sport_packet_t sp { + _parse_state.rx_buffer[0], + _parse_state.rx_buffer[1], + le16toh_ptr(&_parse_state.rx_buffer[2]), + le32toh_ptr(&_parse_state.rx_buffer[4]) + }; + + sport_packet = sp; + return true; +} + +/* + * Calculates the sensor id from the physical sensor index [0-27] + 0x00, // Physical ID 0 - Vario2 (altimeter high precision) + 0xA1, // Physical ID 1 - FLVSS Lipo sensor + 0x22, // Physical ID 2 - FAS-40S current sensor + 0x83, // Physical ID 3 - GPS / altimeter (normal precision) + 0xE4, // Physical ID 4 - RPM + 0x45, // Physical ID 5 - SP2UART(Host) + 0xC6, // Physical ID 6 - SPUART(Remote) + 0x67, // Physical ID 7 - Ardupilot/Betaflight EXTRA DOWNLINK + 0x48, // Physical ID 8 - + 0xE9, // Physical ID 9 - + 0x6A, // Physical ID 10 - + 0xCB, // Physical ID 11 - + 0xAC, // Physical ID 12 - + 0x0D, // Physical ID 13 - Ardupilot/Betaflight UPLINK + 0x8E, // Physical ID 14 - + 0x2F, // Physical ID 15 - + 0xD0, // Physical ID 16 - + 0x71, // Physical ID 17 - + 0xF2, // Physical ID 18 - + 0x53, // Physical ID 19 - + 0x34, // Physical ID 20 - Ardupilot/Betaflight EXTRA DOWNLINK + 0x95, // Physical ID 21 - + 0x16, // Physical ID 22 - GAS Suite + 0xB7, // Physical ID 23 - IMU ACC (x,y,z) + 0x98, // Physical ID 24 - + 0x39, // Physical ID 25 - Power Box + 0xBA // Physical ID 26 - Temp + 0x1B // Physical ID 27 - ArduPilot/Betaflight DEFAULT DOWNLINK + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +#define BIT(x, index) (((x) >> index) & 0x01) +uint8_t AP_Frsky_SPort::calc_sensor_id(const uint8_t physical_id) +{ + uint8_t result = physical_id; + result += (BIT(physical_id, 0) ^ BIT(physical_id, 1) ^ BIT(physical_id, 2)) << 5; + result += (BIT(physical_id, 2) ^ BIT(physical_id, 3) ^ BIT(physical_id, 4)) << 6; + result += (BIT(physical_id, 0) ^ BIT(physical_id, 2) ^ BIT(physical_id, 4)) << 7; + return result; +} diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h index 1d22411de6..8c8ef26270 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h @@ -11,6 +11,16 @@ public: void send() override; + typedef union { + struct PACKED { + uint8_t sensor; + uint8_t frame; + uint16_t appid; + uint32_t data; + }; + uint8_t raw[8]; + } sport_packet_t; + protected: void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data); diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPortParser.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPortParser.h new file mode 100644 index 0000000000..409e5104cc --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPortParser.h @@ -0,0 +1,49 @@ +#pragma once + +#include + +#include "AP_Frsky_SPort.h" + +#include + +// for SPort X protocol +#define FRAME_HEAD 0x7E +#define FRAME_DLE 0x7D +#define FRAME_XOR 0x20 +// for SPort D protocol +#define START_STOP_D 0x5E +#define BYTESTUFF_D 0x5D +// for SPort packet parser +#define TELEMETRY_RX_BUFFER_SIZE 19U // 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1) +#define SPORT_PACKET_SIZE 9U +#define STUFF_MASK 0x20 +#define SPORT_DATA_FRAME 0x10 +#define SPORT_UPLINK_FRAME 0x30 +#define SPORT_UPLINK_FRAME_RW 0x31 +#define SPORT_DOWNLINK_FRAME 0x32 + +class AP_Frsky_SPortParser +{ +public: + + // packet parser helpers + bool process_byte(AP_Frsky_SPort::sport_packet_t &sport_packet, const uint8_t data); + +private: + enum class ParseState : uint8_t { + IDLE, + START, + IN_FRAME, + XOR, + }; + + struct { + uint8_t rx_buffer_count; + uint8_t rx_buffer[TELEMETRY_RX_BUFFER_SIZE]; + uint8_t last_packet[SPORT_PACKET_SIZE]; + ParseState state; + } _parse_state; + + bool should_process_packet(const uint8_t *packet, bool discard_duplicates); + bool get_packet(AP_Frsky_SPort::sport_packet_t &sport_packet, bool discard_duplicates); +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index 0f127e1200..f0659e84a9 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -9,6 +9,16 @@ #include #include +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL +#include +#include +#include +#include +#include +#include +#include "AP_Frsky_MAVlite.h" +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + /* for FrSky SPort Passthrough */ @@ -99,12 +109,50 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void) set_scheduler_entry(GPS_STATUS, 700, 500); // 0x5002 GPS status set_scheduler_entry(HOME, 400, 500); // 0x5004 Home set_scheduler_entry(BATT_2, 1300, 500); // 0x5008 Battery 2 status - set_scheduler_entry(BATT_1, 1300, 500); // 0x5008 Battery 1 status + set_scheduler_entry(BATT_1, 1300, 500); // 0x5003 Battery 1 status set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + set_scheduler_entry(MAV, 35, 25); // mavlite + // initialize sport sensor IDs + set_sensor_id(_frsky_parameters->_uplink_id, _SPort_bidir.uplink_sensor_id); + set_sensor_id(_frsky_parameters->_dnlink1_id, _SPort_bidir.downlink1_sensor_id); + set_sensor_id(_frsky_parameters->_dnlink2_id, _SPort_bidir.downlink2_sensor_id); + // initialize sport + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Frsky_SPort_Passthrough::process_rx_queue, void)); +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL } +/* + dynamically change scheduler priorities based on queue sizes +*/ void AP_Frsky_SPort_Passthrough::adjust_packet_weight(bool queue_empty) { + /* + When queues are empty set a low priority (high weight), when queues + are not empty set a higher priority (low weight) based on the following + relative priority order: mavlite > status text > attitude. + */ +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + if (!_SPort_bidir.tx_packet_queue.is_empty()) { + _scheduler.packet_weight[MAV] = 30; // mavlite + if (!queue_empty) { + _scheduler.packet_weight[TEXT] = 45; // messages + _scheduler.packet_weight[ATTITUDE] = 80; // attitude + } else { + _scheduler.packet_weight[TEXT] = 5000; // messages + _scheduler.packet_weight[ATTITUDE] = 80; // attitude + } + } else { + _scheduler.packet_weight[MAV] = 5000; // mavlite + if (!queue_empty) { + _scheduler.packet_weight[TEXT] = 45; // messages + _scheduler.packet_weight[ATTITUDE] = 80; // attitude + } else { + _scheduler.packet_weight[TEXT] = 5000; // messages + _scheduler.packet_weight[ATTITUDE] = 45; // attitude + } + } +#else //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL if (!queue_empty) { _scheduler.packet_weight[TEXT] = 45; // messages _scheduler.packet_weight[ATTITUDE] = 80; // attitude @@ -112,6 +160,7 @@ void AP_Frsky_SPort_Passthrough::adjust_packet_weight(bool queue_empty) _scheduler.packet_weight[TEXT] = 5000; // messages _scheduler.packet_weight[ATTITUDE] = 45; // attitude } +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL } // WFQ scheduler @@ -122,12 +171,22 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty) case TEXT: packet_ready = !queue_empty; break; + case GPS_LAT: + case GPS_LON: + // force gps coords to use sensor 0x1B, always send when used with external data + packet_ready = _use_external_data || (_passthrough.new_byte == SENSOR_ID_27); + break; case AP_STATUS: packet_ready = gcs().vehicle_initialised(); break; case BATT_2: packet_ready = AP::battery().num_instances() > 1; break; +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + case MAV: + packet_ready = !_SPort_bidir.tx_packet_queue.is_empty(); + break; +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL default: packet_ready = true; break; @@ -157,8 +216,7 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx) send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(_passthrough.send_latitude)); // gps latitude or longitude _passthrough.gps_lng_sample = calc_gps_latlng(_passthrough.send_latitude); // force the scheduler to select GPS lon as packet that's been waiting the most - // this guarantees that gps coords are sent at max - // _scheduler.avg_polling_period*number_of_downlink_sensors time separation + // this guarantees that lat and lon are sent as consecutive packets _scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000; break; case GPS_LON: // 0x800 GPS lon @@ -185,6 +243,11 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx) case PARAM: // 0x5007 parameters send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param()); break; +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + case MAV: // mavlite + process_tx_queue(); + break; +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL } } @@ -205,11 +268,17 @@ void AP_Frsky_SPort_Passthrough::send(void) for (uint16_t i = 0; i < numc; i++) { prev_byte = _passthrough.new_byte; _passthrough.new_byte = _port->read(); - } - if (prev_byte == FRAME_HEAD) { - if (_passthrough.new_byte == SENSOR_ID_27) { // byte 0x7E is the header of each poll request - run_wfq_scheduler(); +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + AP_Frsky_SPort::sport_packet_t sp; + + if (_sport_handler.process_byte(sp, _passthrough.new_byte)) { + queue_rx_packet(sp); } +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + } + // check if we should respond to this polling byte + if (prev_byte == FRAME_HEAD && is_passthrough_byte(_passthrough.new_byte)) { + run_wfq_scheduler(); } } @@ -358,6 +427,20 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_batt(uint8_t instance) return batt; } +/* + * true if we need to respond to the last polling byte + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +bool AP_Frsky_SPort_Passthrough::is_passthrough_byte(const uint8_t byte) +{ +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + if( byte == _SPort_bidir.downlink1_sensor_id || byte == _SPort_bidir.downlink2_sensor_id ) { + return true; + } +#endif + return byte == SENSOR_ID_27; +} + /* * prepare various autopilot status data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) @@ -465,6 +548,27 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void) return attiandrng; } +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL +/* + allow external transports (e.g. FPort), to supply telemetry data + */ +bool AP_Frsky_SPort_Passthrough::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) +{ + // queue only Uplink packets + if (frame == SPORT_UPLINK_FRAME || frame == SPORT_UPLINK_FRAME_RW) { + const AP_Frsky_SPort::sport_packet_t sp { + 0x00, // this is ignored by process_sport_rx_queue() so no need for a real sensor ID + frame, + appid, + data + }; + + _SPort_bidir.rx_packet_queue.push_force(sp); + return true; + } + return false; +} + /* fetch Sport data for an external transport, such as FPort */ @@ -545,3 +649,341 @@ uint16_t AP_Frsky_SPort_Passthrough::prep_number(int32_t number, uint8_t digits, } return res; } + + + + +/* + * Queue uplink packets in the sport rx queue + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +void AP_Frsky_SPort_Passthrough::queue_rx_packet(const AP_Frsky_SPort::sport_packet_t packet) +{ + // queue only Uplink packets + if (packet.sensor == _SPort_bidir.uplink_sensor_id && packet.frame == SPORT_UPLINK_FRAME) { + _SPort_bidir.rx_packet_queue.push_force(packet); + } +} + +/* + * Extract up to 1 mavlite message from the sport rx packet queue + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +void AP_Frsky_SPort_Passthrough::process_rx_queue() +{ + AP_Frsky_SPort::sport_packet_t packet; + uint8_t loop_count = 0; // prevent looping forever + while (_SPort_bidir.rx_packet_queue.pop(packet) && loop_count++ < MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN)) { + AP_Frsky_MAVlite::mavlite_message_t rxmsg; + + if (_mavlite_handler.parse(rxmsg, packet)) { + mavlite_process_message(rxmsg); + break; // process only 1 mavlite message each call + } + } +} + +/* + * Process the sport tx queue + * pop and send 1 sport packet + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +void AP_Frsky_SPort_Passthrough::process_tx_queue() +{ + AP_Frsky_SPort::sport_packet_t packet; + + if (!_SPort_bidir.tx_packet_queue.peek(packet)) { + return; + } + + // when using fport repeat each packet to account for + // fport packet loss (around 15%) + if (!_use_external_data || _SPort_bidir.tx_packet_duplicates++ == SPORT_TX_PACKET_DUPLICATES) { + _SPort_bidir.tx_packet_queue.pop(); + _SPort_bidir.tx_packet_duplicates = 0; + } + + send_sport_frame(SPORT_DOWNLINK_FRAME, packet.appid, packet.data); +} + + +/* + * Handle the COMMAND_LONG mavlite message + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +void AP_Frsky_SPort_Passthrough::mavlite_handle_command_long(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg) +{ + mavlink_command_long_t mav_command_long {}; + + uint8_t cmd_options; + float params[7] {}; + + AP_Frsky_MAVlite::mavlite_msg_get_uint16(mav_command_long.command, rxmsg, 0); + AP_Frsky_MAVlite::mavlite_msg_get_uint8(cmd_options, rxmsg, 2); + uint8_t param_count = AP_Frsky_MAVlite::bit8_unpack(cmd_options, 3, 0); // first 3 bits + + for (uint8_t cmd_idx=0; cmd_idxset_mode(mav_command_long.param1, ModeReason::FRSKY_COMMAND)) { + mav_result = MAV_RESULT_ACCEPTED; + } + break; + //case MAV_CMD_DO_SET_HOME: + case MAV_CMD_DO_FENCE_ENABLE: + mav_result = mavlite_handle_command_do_fence_enable((uint16_t)mav_command_long.param1); + break; + case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_equal(mav_command_long.param1,1.0f) && is_zero(mav_command_long.param2)) { + mav_result = mavlite_handle_command_preflight_reboot(); + } + break; + //case MAV_CMD_DO_START_MAG_CAL: + //case MAV_CMD_DO_ACCEPT_MAG_CAL: + //case MAV_CMD_DO_CANCEL_MAG_CAL: + //case MAV_CMD_START_RX_PAIR: + //case MAV_CMD_DO_DIGICAM_CONFIGURE: + //case MAV_CMD_DO_DIGICAM_CONTROL: + //case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + //case MAV_CMD_DO_GRIPPER: + //case MAV_CMD_DO_MOUNT_CONFIGURE: + //case MAV_CMD_DO_MOUNT_CONTROL: + //case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: + //case MAV_CMD_DO_SET_ROI_SYSID: + //case MAV_CMD_DO_SET_ROI_LOCATION: + //case MAV_CMD_DO_SET_ROI: + case MAV_CMD_PREFLIGHT_CALIBRATION: + if (is_equal(mav_command_long.param1,0.0f) && is_equal(mav_command_long.param2,0.0f) && is_equal(mav_command_long.param3,1.0f)) { + mav_result = mavlite_handle_command_preflight_calibration_baro(); + } + break; + //case MAV_CMD_BATTERY_RESET: + //case MAV_CMD_PREFLIGHT_UAVCAN: + //case MAV_CMD_FLASH_BOOTLOADER: + //case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: + //case MAV_CMD_GET_HOME_POSITION: + //case MAV_CMD_PREFLIGHT_STORAGE: + //case MAV_CMD_SET_MESSAGE_INTERVAL: + //case MAV_CMD_GET_MESSAGE_INTERVAL: + //case MAV_CMD_REQUEST_MESSAGE: + //case MAV_CMD_DO_SET_SERVO: + //case MAV_CMD_DO_REPEAT_SERVO: + //case MAV_CMD_DO_SET_RELAY: + //case MAV_CMD_DO_REPEAT_RELAY: + //case MAV_CMD_DO_FLIGHTTERMINATION: + //case MAV_CMD_COMPONENT_ARM_DISARM: + //case MAV_CMD_FIXED_MAG_CAL_YAW: + default: + mav_result = MAV_RESULT_UNSUPPORTED; + break; + } + mavlite_send_command_ack(mav_result, mav_command_long.command); +} + +void AP_Frsky_SPort_Passthrough::mavlite_send_command_ack(const MAV_RESULT mav_result, const uint16_t cmdid) +{ + AP_Frsky_MAVlite::mavlite_message_t txmsg; + txmsg.msgid = MAVLINK_MSG_ID_COMMAND_ACK; + AP_Frsky_MAVlite::mavlite_msg_set_uint16(txmsg, cmdid, 0); + AP_Frsky_MAVlite::mavlite_msg_set_uint8(txmsg, (uint8_t)mav_result, 2); + mavlite_send_message(txmsg); +} + +MAV_RESULT AP_Frsky_SPort_Passthrough::mavlite_handle_command_preflight_calibration_baro() +{ + if (hal.util->get_soft_armed()) { + return MAV_RESULT_DENIED; + } + // fast barometer calibration + gcs().send_text(MAV_SEVERITY_INFO, "Updating barometer calibration"); + AP::baro().update_calibration(); + gcs().send_text(MAV_SEVERITY_INFO, "Barometer calibration complete"); + + AP_Airspeed *airspeed = AP_Airspeed::get_singleton(); + if (airspeed != nullptr) { + airspeed->calibrate(false); + } + + return MAV_RESULT_ACCEPTED; +} + +MAV_RESULT AP_Frsky_SPort_Passthrough::mavlite_handle_command_do_fence_enable(uint16_t param1) +{ + AC_Fence *fence = AP::fence(); + if (fence == nullptr) { + return MAV_RESULT_UNSUPPORTED; + } + + switch (param1) { + case 0: + fence->enable(false); + return MAV_RESULT_ACCEPTED; + case 1: + fence->enable(true); + return MAV_RESULT_ACCEPTED; + default: + return MAV_RESULT_FAILED; + } +} + +/* + * Handle the PARAM_REQUEST_READ mavlite message + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +void AP_Frsky_SPort_Passthrough::mavlite_handle_param_request_read(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg) +{ + float param_value; + char param_name[AP_MAX_NAME_SIZE+1]; + AP_Frsky_MAVlite::mavlite_msg_get_string(param_name, rxmsg, 0); + // find existing param + if (!AP_Param::get(param_name,param_value)) { + gcs().send_text(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name); + return; + } + AP_Frsky_MAVlite::mavlite_message_t txmsg; + txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE; + AP_Frsky_MAVlite::mavlite_msg_set_float(txmsg, param_value, 0); + AP_Frsky_MAVlite::mavlite_msg_set_string(txmsg, param_name, 4); + mavlite_send_message(txmsg); +} + +/* + * Handle the PARAM_SET mavlite message + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +void AP_Frsky_SPort_Passthrough::mavlite_handle_param_set(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg) +{ + float param_value; + char param_name[AP_MAX_NAME_SIZE+1]; + // populate packet with mavlite payload + AP_Frsky_MAVlite::mavlite_msg_get_float(param_value, rxmsg, 0); + AP_Frsky_MAVlite::mavlite_msg_get_string(param_name, rxmsg, 4); + // find existing param so we can get the old value + enum ap_var_type var_type; + // set parameter + AP_Param *vp; + uint16_t parameter_flags = 0; + vp = AP_Param::find(param_name, &var_type, ¶meter_flags); + if (vp == nullptr || isnan(param_value) || isinf(param_value)) { + return; + } + if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Param write denied (%s)", param_name); + } else if (!AP_Param::set_and_save(param_name, param_value)) { + gcs().send_text(MAV_SEVERITY_WARNING, "Param write failed (%s)", param_name); + } + // let's read back the last value, either the readonly one or the updated one + if (!AP_Param::get(param_name,param_value)) { + gcs().send_text(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name); + return; + } + AP_Frsky_MAVlite::mavlite_message_t txmsg; + txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE; + AP_Frsky_MAVlite::mavlite_msg_set_float(txmsg, param_value, 0); + AP_Frsky_MAVlite::mavlite_msg_set_string(txmsg, param_name, 4); + mavlite_send_message(txmsg); +} + +/* + Handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command + for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + Optionally disable PX4IO overrides. This is done for quadplanes to + prevent the mixer running while rebooting which can start the VTOL + motors. That can be dangerous when a preflight reboot is done with + the pilot close to the aircraft and can also damage the aircraft + */ +MAV_RESULT AP_Frsky_SPort_Passthrough::mavlite_handle_command_preflight_reboot(void) +{ + if (hal.util->get_soft_armed()) { + // refuse reboot when armed + return MAV_RESULT_FAILED; + } +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) + SRV_Channels::zero_rc_outputs(); +#endif + // send ack before we reboot + mavlite_send_command_ack(MAV_RESULT_ACCEPTED, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN); + // Notify might want to blink some LEDs: + AP_Notify *notify = AP_Notify::get_singleton(); + if (notify) { + AP_Notify::flags.firmware_update = 1; + notify->update(); + } + // force safety on + hal.rcout->force_safety_on(); + + // flush pending parameter writes + AP_Param::flush(); + + // do not process incoming mavlink messages while we delay: + hal.scheduler->register_delay_callback(nullptr, 5); + + // delay to give the ACK a chance to get out, the LEDs to flash, + // the IO board safety to be forced on, the parameters to flush, ... + hal.scheduler->delay(1000); + + hal.scheduler->reboot(false); + + return MAV_RESULT_FAILED; +} + +/* + * Process an incoming mavlite message + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +void AP_Frsky_SPort_Passthrough::mavlite_process_message(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg) +{ + switch (rxmsg.msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: + mavlite_handle_param_request_read(rxmsg); + break; + case MAVLINK_MSG_ID_PARAM_SET: + mavlite_handle_param_set(rxmsg); + break; + case MAVLINK_MSG_ID_COMMAND_LONG: + mavlite_handle_command_long(rxmsg); + break; + } +} + +/* + * Send a mavlite message + * Message is chunked in sport packets pushed in the tx queue + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +bool AP_Frsky_SPort_Passthrough::mavlite_send_message(AP_Frsky_MAVlite::mavlite_message_t &txmsg) +{ + return _mavlite_handler.encode(_SPort_bidir.tx_packet_queue, txmsg); +} + + +/* + * Utility method to apply constraints in changing sensor id values + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +void AP_Frsky_SPort_Passthrough::set_sensor_id(AP_Int8 param_idx, uint8_t &sensor) +{ + int8_t idx = param_idx.get(); + + if (idx == -1) { + // disable this sensor + sensor = 0xFF; + return; + } + sensor = calc_sensor_id(idx); +} +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index 595c0c4712..4d2181a713 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -3,17 +3,27 @@ #include "AP_Frsky_SPort.h" #include -// for fair scheduler -#define TIME_SLOT_MAX 11 +#include "AP_Frsky_SPortParser.h" +#include "AP_Frsky_MAVlite.h" + +#include "AP_Frsky_Telem.h" + +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL +#define FRSKY_WFQ_TIME_SLOT_MAX 12U +#define SPORT_TX_PACKET_DUPLICATES 1 // number of duplicates packets we send (fport only) +#else +#define FRSKY_WFQ_TIME_SLOT_MAX 11U +#endif class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry { public: - AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data) : + AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data, AP_Frsky_Parameters *&frsky_parameters) : AP_Frsky_SPort(port), - AP_RCTelemetry(TIME_SLOT_MAX), - _use_external_data(use_external_data) + AP_RCTelemetry(FRSKY_WFQ_TIME_SLOT_MAX), + _use_external_data(use_external_data), + _frsky_parameters(frsky_parameters) { } bool init() override; @@ -29,19 +39,21 @@ public: bool get_next_msg_chunk(void) override; bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) override; + bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) override; void queue_text_message(MAV_SEVERITY severity, const char *text) override { AP_RCTelemetry::queue_message(severity, text); } - protected: void send() override; private: + AP_Frsky_Parameters *&_frsky_parameters; + enum PassthroughParam : uint8_t { FRAME_TYPE = 1, BATT_FS_VOLTAGE = 2, @@ -61,7 +73,10 @@ private: HOME = 7, // 0x5004 Home BATT_2 = 8, // 0x5008 Battery 2 status BATT_1 = 9, // 0x5008 Battery 1 status - PARAM = 10 // 0x5007 parameters + PARAM = 10, // 0x5007 parameters +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + MAV = 11, // mavlite +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL }; // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format @@ -88,8 +103,46 @@ private: uint8_t char_index; // index of which character to get in the message } _msg_chunk; + +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + // bidirectional sport telemetry + struct { + uint8_t uplink_sensor_id = 0x0D; + uint8_t downlink1_sensor_id = 0x34; + uint8_t downlink2_sensor_id = 0x67; + uint8_t tx_packet_duplicates; + ObjectBuffer_TS rx_packet_queue{SPORT_PACKET_QUEUE_LENGTH}; + ObjectBuffer_TS tx_packet_queue{SPORT_PACKET_QUEUE_LENGTH}; + } _SPort_bidir; + + AP_Frsky_SPortParser _sport_handler; + AP_Frsky_MAVlite _mavlite_handler; + + void set_sensor_id(AP_Int8 idx, uint8_t &sensor); + // tx/rx sport packet processing + void queue_rx_packet(const AP_Frsky_SPort::sport_packet_t sp); + void process_rx_queue(void); + void process_tx_queue(void); + + // mavlite messages tx/rx methods + bool mavlite_send_message(AP_Frsky_MAVlite::mavlite_message_t &txmsg); + void mavlite_process_message(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg); + + // gcs mavlite methods + void mavlite_handle_param_request_read(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg); + void mavlite_handle_param_set(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg); + void mavlite_handle_command_long(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg); + void mavlite_send_command_ack(const MAV_RESULT mav_result, const uint16_t cmdid); + MAV_RESULT mavlite_handle_command_preflight_calibration_baro(); + MAV_RESULT mavlite_handle_command_do_fence_enable(uint16_t param1); + MAV_RESULT mavlite_handle_command_preflight_reboot(void); +#endif + void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data); + // true if we need to respond to the last polling byte + bool is_passthrough_byte(const uint8_t byte); + uint8_t _paramID; uint32_t calc_gps_status(void); diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index 77267276ee..b840eef67b 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -23,9 +23,12 @@ */ #include "AP_Frsky_Telem.h" +#include "AP_Frsky_Parameters.h" #include +#include + #include "AP_Frsky_D.h" #include "AP_Frsky_SPort.h" #include "AP_Frsky_SPort_Passthrough.h" @@ -37,8 +40,12 @@ AP_Frsky_Telem *AP_Frsky_Telem::singleton; AP_Frsky_Telem::AP_Frsky_Telem() { singleton = this; +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + _frsky_parameters = &AP::vehicle()->frsky_parameters; +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL } + AP_Frsky_Telem::~AP_Frsky_Telem(void) { singleton = nullptr; @@ -58,7 +65,7 @@ bool AP_Frsky_Telem::init(bool use_external_data) } else if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) { _backend = new AP_Frsky_SPort(port); } else if (use_external_data || (port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) { - _backend = new AP_Frsky_SPort_Passthrough(port, use_external_data); + _backend = new AP_Frsky_SPort_Passthrough(port, use_external_data, _frsky_parameters); } if (_backend == nullptr) { @@ -82,6 +89,14 @@ bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t & return _backend->get_telem_data(frame, appid, data); } +bool AP_Frsky_Telem::_set_telem_data(uint8_t frame, uint16_t appid, uint32_t data) +{ + if (_backend == nullptr) { + return false; + } + return _backend->set_telem_data(frame, appid, data); +} + /* fetch Sport data for an external transport, such as FPort */ @@ -96,14 +111,35 @@ bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &d singleton->init(true); } } - if (!singleton) { + if (singleton == nullptr) { return false; } return singleton->_get_telem_data(frame, appid, data); } -namespace AP { - AP_Frsky_Telem *frsky_telem() { - return AP_Frsky_Telem::get_singleton(); +/* + allow external transports (e.g. FPort), to supply telemetry data + */ +bool AP_Frsky_Telem::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) +{ + if (!singleton && !hal.util->get_soft_armed()) { + // if telem data is requested when we are disarmed and don't + // yet have a AP_Frsky_Telem object then try to allocate one + new AP_Frsky_Telem(); + if (singleton) { + singleton->init(true); + } + } + if (singleton == nullptr) { + return false; } + return singleton->_set_telem_data(frame, appid, data); +} + +namespace AP +{ +AP_Frsky_Telem *frsky_telem() +{ + return AP_Frsky_Telem::get_singleton(); +} }; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 33f5f48710..3d580b67c5 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -14,9 +14,16 @@ */ #pragma once +#ifndef HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL +#define HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL 1 +#endif + #include "AP_Frsky_Backend.h" +class AP_Frsky_Parameters; + class AP_Frsky_Telem { + public: AP_Frsky_Telem(); @@ -35,6 +42,10 @@ public: // get next telemetry data for external consumers of SPort data static bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data); +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + // set telemetry data from external producer of SPort data + static bool set_telem_data(const uint8_t frame,const uint16_t appid, const uint32_t data); +#endif void queue_message(MAV_SEVERITY severity, const char *text) { if (_backend == nullptr) { @@ -47,8 +58,14 @@ private: AP_Frsky_Backend *_backend; +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + AP_Frsky_Parameters* _frsky_parameters; + // get next telemetry data for external consumers of SPort data (internal function) bool _get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data); + // set telemetry data from external producer of SPort data (internal function) + bool _set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data); +#endif static AP_Frsky_Telem *singleton;