AP_Frsky_Telem: added support for frsky sport/fport bidirectional telemetry

This commit is contained in:
yaapu 2020-09-10 14:55:53 +02:00 committed by Andrew Tridgell
parent a4297bcdd7
commit 988af83fce
12 changed files with 1256 additions and 19 deletions

View File

@ -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) { }

View File

@ -0,0 +1,323 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#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<AP_Frsky_SPort::sport_packet_t> &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<<bit_offset) & mask;
}
bool AP_Frsky_MAVlite::mavlite_msg_get_string(char* value, const mavlite_message_t &msg, const uint8_t offset)
{
if (mavlite_msg_get_bytes((uint8_t*)value, msg, offset, MIN((uint8_t)16, msg.len - offset))) {
value[MIN((uint8_t)16, msg.len - offset)] = 0x00; // terminator
return true;
}
return false;
}
bool AP_Frsky_MAVlite::mavlite_msg_set_float(mavlite_message_t &msg, const float value, const uint8_t offset)
{
return mavlite_msg_set_bytes(msg, (uint8_t*)&value, offset, 4);
}
bool AP_Frsky_MAVlite::mavlite_msg_set_uint16(mavlite_message_t &msg, const uint16_t value, const uint8_t offset)
{
return mavlite_msg_set_bytes(msg, (uint8_t*)&value, offset, 2);
}
bool AP_Frsky_MAVlite::mavlite_msg_set_uint8(mavlite_message_t &msg, const uint8_t value, const uint8_t offset)
{
return mavlite_msg_set_bytes(msg, (uint8_t*)&value, offset, 1);
}
bool AP_Frsky_MAVlite::mavlite_msg_set_string(mavlite_message_t &msg, const char* value, const uint8_t offset)
{
return mavlite_msg_set_bytes(msg, (uint8_t*)value, offset, MIN((uint8_t)16, strlen(value)));
}

View File

@ -0,0 +1,74 @@
#pragma once
#include <AP_HAL/utility/RingBuffer.h>
#include "AP_Frsky_SPort.h"
#include <stdint.h>
#define MAVLITE_MAX_PAYLOAD_LEN 31 // 7 float params + cmd_id + options
#define MAVLITE_MSG_SPORT_PACKETS_COUNT(LEN) static_cast<uint8_t>(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<uint8_t>(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<AP_Frsky_SPort::sport_packet_t> &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);
};

View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "AP_Frsky_Telem.h"
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
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

View File

@ -1,9 +1,14 @@
#include "AP_Frsky_SPort.h"
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_GPS/AP_GPS.h>
#include <GCS_MAVLink/GCS.h>
#include "AP_Frsky_SPortParser.h"
#include <string.h>
/*
* 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<SPORT_PACKET_SIZE; ++i) {
crc += _parse_state.rx_buffer[i]; // 0-1FE
crc += crc >> 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;
}

View File

@ -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);

View File

@ -0,0 +1,49 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_Frsky_SPort.h"
#include <stdint.h>
// 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);
};

View File

@ -9,6 +9,16 @@
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <GCS_MAVLink/GCS.h>
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
#include <AC_Fence/AC_Fence.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Param/AP_Param.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <SRV_Channel/SRV_Channel.h>
#include <stdio.h>
#include "AP_Frsky_MAVlite.h"
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
/*
for FrSky SPort Passthrough
*/
@ -99,12 +109,41 @@ 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
@ -112,6 +151,16 @@ void AP_Frsky_SPort_Passthrough::adjust_packet_weight(bool queue_empty)
_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
} else {
_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,12 +268,18 @@ 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 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);
}
if (prev_byte == FRAME_HEAD) {
if (_passthrough.new_byte == SENSOR_ID_27) { // byte 0x7E is the header of each poll request
#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_idx<param_count; cmd_idx++) {
AP_Frsky_MAVlite::mavlite_msg_get_float(params[cmd_idx], rxmsg, 3+(4*cmd_idx)); // base offset is 3, relative offset is 4*cmd_idx
}
mav_command_long.param1 = params[0];
mav_command_long.param2 = params[1];
mav_command_long.param3 = params[2];
mav_command_long.param4 = params[3];
mav_command_long.param5 = params[4];
mav_command_long.param6 = params[5];
mav_command_long.param7 = params[6];
MAV_RESULT mav_result = MAV_RESULT_FAILED;
// filter allowed commands
switch (mav_command_long.command) {
//case MAV_CMD_ACCELCAL_VEHICLE_POS:
case MAV_CMD_DO_SET_MODE:
if (AP::vehicle()->set_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, &parameter_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

View File

@ -3,17 +3,27 @@
#include "AP_Frsky_SPort.h"
#include <AP_RCTelemetry/AP_RCTelemetry.h>
// 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<AP_Frsky_SPort::sport_packet_t> rx_packet_queue{SPORT_PACKET_QUEUE_LENGTH};
ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> 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);

View File

@ -23,9 +23,12 @@
*/
#include "AP_Frsky_Telem.h"
#include "AP_Frsky_Parameters.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Vehicle/AP_Vehicle.h>
#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();
}
};

View File

@ -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;