mirror of https://github.com/ArduPilot/ardupilot
AP_Frsky_Telem: added support for frsky sport/fport bidirectional telemetry
This commit is contained in:
yaapu
Andrew Tridgell
parent
a4297bcdd7
commit
988af83fce
|
|
@ -26,6 +26,10 @@ public:
|
||||||
{
|
{
|
||||||
return false;
|
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) { }
|
virtual void queue_text_message(MAV_SEVERITY severity, const char *text) { }
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -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)));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -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);
|
||||||
|
};
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -1,9 +1,14 @@
|
||||||
#include "AP_Frsky_SPort.h"
|
#include "AP_Frsky_SPort.h"
|
||||||
|
|
||||||
|
#include <AP_HAL/utility/sparse-endian.h>
|
||||||
#include <AP_BattMonitor/AP_BattMonitor.h>
|
#include <AP_BattMonitor/AP_BattMonitor.h>
|
||||||
#include <AP_GPS/AP_GPS.h>
|
#include <AP_GPS/AP_GPS.h>
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
|
||||||
|
#include "AP_Frsky_SPortParser.h"
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* send telemetry data
|
* send telemetry data
|
||||||
* for FrSky SPort protocol (X-receivers)
|
* 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
|
#endif
|
||||||
_port->write(buf2, len);
|
_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;
|
||||||
|
}
|
||||||
|
|
|
||||||
|
|
@ -11,6 +11,16 @@ public:
|
||||||
|
|
||||||
void send() override;
|
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:
|
protected:
|
||||||
|
|
||||||
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
|
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
|
||||||
|
|
|
||||||
|
|
@ -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);
|
||||||
|
};
|
||||||
|
|
@ -9,6 +9,16 @@
|
||||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||||
#include <GCS_MAVLink/GCS.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
|
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(GPS_STATUS, 700, 500); // 0x5002 GPS status
|
||||||
set_scheduler_entry(HOME, 400, 500); // 0x5004 Home
|
set_scheduler_entry(HOME, 400, 500); // 0x5004 Home
|
||||||
set_scheduler_entry(BATT_2, 1300, 500); // 0x5008 Battery 2 status
|
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
|
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)
|
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) {
|
if (!queue_empty) {
|
||||||
_scheduler.packet_weight[TEXT] = 45; // messages
|
_scheduler.packet_weight[TEXT] = 45; // messages
|
||||||
_scheduler.packet_weight[ATTITUDE] = 80; // attitude
|
_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[TEXT] = 5000; // messages
|
||||||
_scheduler.packet_weight[ATTITUDE] = 45; // attitude
|
_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
|
// WFQ scheduler
|
||||||
|
|
@ -122,12 +171,22 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
|
||||||
case TEXT:
|
case TEXT:
|
||||||
packet_ready = !queue_empty;
|
packet_ready = !queue_empty;
|
||||||
break;
|
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:
|
case AP_STATUS:
|
||||||
packet_ready = gcs().vehicle_initialised();
|
packet_ready = gcs().vehicle_initialised();
|
||||||
break;
|
break;
|
||||||
case BATT_2:
|
case BATT_2:
|
||||||
packet_ready = AP::battery().num_instances() > 1;
|
packet_ready = AP::battery().num_instances() > 1;
|
||||||
break;
|
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:
|
default:
|
||||||
packet_ready = true;
|
packet_ready = true;
|
||||||
break;
|
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
|
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);
|
_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
|
// force the scheduler to select GPS lon as packet that's been waiting the most
|
||||||
// this guarantees that gps coords are sent at max
|
// this guarantees that lat and lon are sent as consecutive packets
|
||||||
// _scheduler.avg_polling_period*number_of_downlink_sensors time separation
|
|
||||||
_scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000;
|
_scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000;
|
||||||
break;
|
break;
|
||||||
case GPS_LON: // 0x800 GPS lon
|
case GPS_LON: // 0x800 GPS lon
|
||||||
|
|
@ -185,6 +243,11 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
|
||||||
case PARAM: // 0x5007 parameters
|
case PARAM: // 0x5007 parameters
|
||||||
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
|
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
|
||||||
break;
|
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++) {
|
for (uint16_t i = 0; i < numc; i++) {
|
||||||
prev_byte = _passthrough.new_byte;
|
prev_byte = _passthrough.new_byte;
|
||||||
_passthrough.new_byte = _port->read();
|
_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) {
|
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
if (_passthrough.new_byte == SENSOR_ID_27) { // byte 0x7E is the header of each poll request
|
}
|
||||||
|
// check if we should respond to this polling byte
|
||||||
|
if (prev_byte == FRAME_HEAD && is_passthrough_byte(_passthrough.new_byte)) {
|
||||||
run_wfq_scheduler();
|
run_wfq_scheduler();
|
||||||
}
|
}
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
@ -358,6 +427,20 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_batt(uint8_t instance)
|
||||||
return batt;
|
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
|
* prepare various autopilot status data
|
||||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||||
|
|
@ -465,6 +548,27 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void)
|
||||||
return attiandrng;
|
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
|
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;
|
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, ¶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
|
||||||
|
|
|
||||||
|
|
@ -3,17 +3,27 @@
|
||||||
#include "AP_Frsky_SPort.h"
|
#include "AP_Frsky_SPort.h"
|
||||||
#include <AP_RCTelemetry/AP_RCTelemetry.h>
|
#include <AP_RCTelemetry/AP_RCTelemetry.h>
|
||||||
|
|
||||||
// for fair scheduler
|
#include "AP_Frsky_SPortParser.h"
|
||||||
#define TIME_SLOT_MAX 11
|
#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
|
class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry
|
||||||
{
|
{
|
||||||
public:
|
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_Frsky_SPort(port),
|
||||||
AP_RCTelemetry(TIME_SLOT_MAX),
|
AP_RCTelemetry(FRSKY_WFQ_TIME_SLOT_MAX),
|
||||||
_use_external_data(use_external_data)
|
_use_external_data(use_external_data),
|
||||||
|
_frsky_parameters(frsky_parameters)
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
bool init() override;
|
bool init() override;
|
||||||
|
|
@ -29,19 +39,21 @@ public:
|
||||||
bool get_next_msg_chunk(void) override;
|
bool get_next_msg_chunk(void) override;
|
||||||
|
|
||||||
bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) 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
|
void queue_text_message(MAV_SEVERITY severity, const char *text) override
|
||||||
{
|
{
|
||||||
AP_RCTelemetry::queue_message(severity, text);
|
AP_RCTelemetry::queue_message(severity, text);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
void send() override;
|
void send() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
AP_Frsky_Parameters *&_frsky_parameters;
|
||||||
|
|
||||||
enum PassthroughParam : uint8_t {
|
enum PassthroughParam : uint8_t {
|
||||||
FRAME_TYPE = 1,
|
FRAME_TYPE = 1,
|
||||||
BATT_FS_VOLTAGE = 2,
|
BATT_FS_VOLTAGE = 2,
|
||||||
|
|
@ -61,7 +73,10 @@ private:
|
||||||
HOME = 7, // 0x5004 Home
|
HOME = 7, // 0x5004 Home
|
||||||
BATT_2 = 8, // 0x5008 Battery 2 status
|
BATT_2 = 8, // 0x5008 Battery 2 status
|
||||||
BATT_1 = 9, // 0x5008 Battery 1 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
|
// 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
|
uint8_t char_index; // index of which character to get in the message
|
||||||
} _msg_chunk;
|
} _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);
|
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;
|
uint8_t _paramID;
|
||||||
|
|
||||||
uint32_t calc_gps_status(void);
|
uint32_t calc_gps_status(void);
|
||||||
|
|
|
||||||
|
|
@ -23,9 +23,12 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "AP_Frsky_Telem.h"
|
#include "AP_Frsky_Telem.h"
|
||||||
|
#include "AP_Frsky_Parameters.h"
|
||||||
|
|
||||||
#include <AP_SerialManager/AP_SerialManager.h>
|
#include <AP_SerialManager/AP_SerialManager.h>
|
||||||
|
|
||||||
|
#include <AP_Vehicle/AP_Vehicle.h>
|
||||||
|
|
||||||
#include "AP_Frsky_D.h"
|
#include "AP_Frsky_D.h"
|
||||||
#include "AP_Frsky_SPort.h"
|
#include "AP_Frsky_SPort.h"
|
||||||
#include "AP_Frsky_SPort_Passthrough.h"
|
#include "AP_Frsky_SPort_Passthrough.h"
|
||||||
|
|
@ -37,8 +40,12 @@ AP_Frsky_Telem *AP_Frsky_Telem::singleton;
|
||||||
AP_Frsky_Telem::AP_Frsky_Telem()
|
AP_Frsky_Telem::AP_Frsky_Telem()
|
||||||
{
|
{
|
||||||
singleton = this;
|
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)
|
AP_Frsky_Telem::~AP_Frsky_Telem(void)
|
||||||
{
|
{
|
||||||
singleton = nullptr;
|
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))) {
|
} else if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) {
|
||||||
_backend = new AP_Frsky_SPort(port);
|
_backend = new AP_Frsky_SPort(port);
|
||||||
} else if (use_external_data || (port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) {
|
} 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) {
|
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);
|
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
|
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);
|
singleton->init(true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (!singleton) {
|
if (singleton == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return singleton->_get_telem_data(frame, appid, data);
|
return singleton->_get_telem_data(frame, appid, data);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace AP {
|
/*
|
||||||
AP_Frsky_Telem *frsky_telem() {
|
allow external transports (e.g. FPort), to supply telemetry data
|
||||||
return AP_Frsky_Telem::get_singleton();
|
*/
|
||||||
|
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();
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
|
||||||
|
|
@ -14,9 +14,16 @@
|
||||||
*/
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#ifndef HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
|
#define HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL 1
|
||||||
|
#endif
|
||||||
|
|
||||||
#include "AP_Frsky_Backend.h"
|
#include "AP_Frsky_Backend.h"
|
||||||
|
|
||||||
|
class AP_Frsky_Parameters;
|
||||||
|
|
||||||
class AP_Frsky_Telem {
|
class AP_Frsky_Telem {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
AP_Frsky_Telem();
|
AP_Frsky_Telem();
|
||||||
|
|
||||||
|
|
@ -35,6 +42,10 @@ public:
|
||||||
|
|
||||||
// get next telemetry data for external consumers of SPort data
|
// get next telemetry data for external consumers of SPort data
|
||||||
static bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &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) {
|
void queue_message(MAV_SEVERITY severity, const char *text) {
|
||||||
if (_backend == nullptr) {
|
if (_backend == nullptr) {
|
||||||
|
|
@ -47,8 +58,14 @@ private:
|
||||||
|
|
||||||
AP_Frsky_Backend *_backend;
|
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)
|
// 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);
|
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;
|
static AP_Frsky_Telem *singleton;
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue