Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: rework SPort-to-MAVlite conversion

Browse Source 
Breaks the single object into three distinct parts objects, and hidesas
much information from the other parts as possible.
This commit is contained in:
Peter Barker 2020-09-24 15:33:06 +10:00 committed by Andrew Tridgell
parent 18330d16de
commit b4e12da2c8
11 changed files with 487 additions and 419 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

323
libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp
View File

@ -1,323 +0,0 @@
#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)));
}

90
libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.h
View File

@ -1,74 +1,32 @@
#pragma once
#include <AP_HAL/utility/RingBuffer.h>
/*
#include "AP_Frsky_SPort.h"
Wire Protocol:
Several SPort packets make up a MAVlite message.
A maximum of six relevant data bytes are present in an SPort packet.
Each SPort packet starts with a sequence number, starting with zero.
If the sequence number is zero then the parser is reset.
The first sport packet contains len at offset 1. It is the
*payload* length - does not include checksum, for example. msgid is
at offset 2, then payload bytes.
Subsequent SPort packets contain a sequence number at offset zero,
followed by more payload bytes.
When sufficient payload bytes have been received (based on "len"), a
single checksum byte arrives. Sometimes this checksum byte goes
into an SPort packet all on its own, sharing space only with the
sequence number.
*/
#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);
};

98
libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.cpp Normal file
View File

@ -0,0 +1,98 @@
#include "AP_Frsky_MAVlite_MAVliteToSPort.h"
#include "AP_Frsky_MAVlite.h"
#include "AP_Frsky_SPort.h"
void AP_Frsky_MAVlite_MAVliteToSPort::reset()
{
checksum = 0;
packet_offs = 2; // someone else sets frame and sensorid
state = State::WANT_LEN;
next_seq = 0;
payload_count = 0;
}
void AP_Frsky_MAVlite_MAVliteToSPort::update_checksum(const uint8_t c)
{
checksum += c; //0-1FF
checksum += checksum >> 8;
checksum &= 0xFF;
}
/*
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_MAVliteToSPort::process(ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue, const AP_Frsky_MAVlite_Message &msg)
{
// let's check if there's enough room to send it
if (queue.space() < MAVLITE_MSG_SPORT_PACKETS_COUNT(msg.len)) {
return false;
}
reset();
process_byte(msg.len, queue);
process_byte(msg.msgid, queue);
for (uint8_t i=0; i<msg.len; i++) {
process_byte(msg.payload[i], queue);
}
// byte isn't important in this call; checksum is used in
// process_byte in case we need to start a new packet just to hold
// it.
process_byte(0, queue);
return true;
}
void AP_Frsky_MAVlite_MAVliteToSPort::process_byte(const uint8_t b, ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue)
{
if (packet_offs == 2) {
// start of a packet (since we skip setting sensorid and
// frame). Emit a sequence number.
packet.raw[packet_offs++] = next_seq;
update_checksum(next_seq);
next_seq += 1;
}
switch (state) {
case State::WANT_LEN:
packet.raw[packet_offs++] = b;
update_checksum(b);
payload_len = b;
state = State::WANT_MSGID;
break;
case State::WANT_MSGID:
packet.raw[packet_offs++] = b;
update_checksum(b);
if (b == 0) {
state = State::WANT_CHECKSUM;
} else {
state = State::WANT_PAYLOAD;
}
break;
case State::WANT_PAYLOAD:
packet.raw[packet_offs++] = b;
update_checksum(b);
payload_count++;
if (payload_count >= payload_len) {
state = State::WANT_CHECKSUM;
}
break;
case State::WANT_CHECKSUM:
packet.raw[packet_offs++] = checksum;
queue.push(packet);
state = State::DONE;
break;
case State::DONE:
return;
}
if (packet_offs >= ARRAY_SIZE(packet.raw)) {
// it's cooked
queue.push(packet);
packet_offs = 2; // skip setting sensorid and frame
}
}

52
libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.h Normal file
View File

@ -0,0 +1,52 @@
#pragma once
#include "AP_Frsky_MAVlite_Message.h"
#include "AP_Frsky_SPort.h"
#include <AP_HAL/utility/RingBuffer.h>
#include <stdint.h>
/*
* An instance of this class encodes a MAVlite message into several
* SPort packets, and pushes them onto the supplied queue.
*
* process() will return false if there is insufficient room in the
* queue for all SPort packets.
*
* See AP_Frsky_MAVlite.h for a description of the encoding of a
* MAVlite message in SPort packets.
*/
class AP_Frsky_MAVlite_MAVliteToSPort {
public:
// insert sport packets calculated from mavlite msg into queue
bool process(ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue,
const AP_Frsky_MAVlite_Message &msg) WARN_IF_UNUSED;
private:
enum class State : uint8_t {
WANT_LEN,
WANT_MSGID,
WANT_PAYLOAD,
WANT_CHECKSUM,
DONE,
};
State state = State::WANT_LEN;
void reset();
void process_byte(uint8_t byte, ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue);
AP_Frsky_SPort::sport_packet_t packet {};
uint8_t packet_offs = 0;
uint8_t next_seq = 0;
uint8_t payload_count = 0;
uint8_t payload_len;
int16_t checksum; // sent at end of packet
void update_checksum(const uint8_t c);
};

55
libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.cpp Normal file
View File

@ -0,0 +1,55 @@
#include "AP_Frsky_MAVlite_Message.h"
#include <AP_Math/AP_Math.h>
bool AP_Frsky_MAVlite_Message::get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const
{
if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
return false;
}
memcpy(bytes, &payload[offset], count);
return true;
}
bool AP_Frsky_MAVlite_Message::set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count)
{
if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
return false;
}
memcpy(&payload[offset], bytes, count);
len += count;
return true;
}
bool AP_Frsky_MAVlite_Message::get_string(char* value, const uint8_t offset) const
{
if (get_bytes((uint8_t*)value, offset, MIN((uint8_t)16, len - offset))) {
value[MIN((uint8_t)16, len - offset)] = 0x00; // terminator
return true;
}
return false;
}
bool AP_Frsky_MAVlite_Message::set_string(const char* value, const uint8_t offset)
{
return set_bytes((uint8_t*)value, offset, MIN((uint8_t)16, strlen(value)));
}
uint8_t AP_Frsky_MAVlite_Message::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_Message::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;
}

46
libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.h Normal file
View File

@ -0,0 +1,46 @@
#pragma once
#include "AP_Frsky_MAVlite.h"
#include <AP_Common/AP_Common.h>
#include <stdint.h>
class AP_Frsky_MAVlite_Message {
public:
// helpers
bool get_float(float &value, const uint8_t offset) const WARN_IF_UNUSED {
return get_bytes((uint8_t*)&value, offset, 4);
}
bool set_float(const float value, const uint8_t offset) WARN_IF_UNUSED {
return set_bytes((uint8_t*)&value, offset, 4);
}
bool get_string(char* value, const uint8_t offset) const WARN_IF_UNUSED;
bool set_string(const char* value, const uint8_t offset) WARN_IF_UNUSED;
bool get_uint16(uint16_t &value, const uint8_t offset) const WARN_IF_UNUSED {
return get_bytes((uint8_t*)&value, offset, 2);
}
bool set_uint16(const uint16_t value, const uint8_t offset) WARN_IF_UNUSED {
return set_bytes((uint8_t*)&value, offset, 2);
}
bool get_uint8(uint8_t &value, const uint8_t offset) const WARN_IF_UNUSED {
return get_bytes((uint8_t*)&value, offset, 1);;
}
bool set_uint8(const uint8_t value, const uint8_t offset) WARN_IF_UNUSED {
return set_bytes((uint8_t*)&value, offset, 1);
}
uint8_t msgid = 0; // ID of message in payload
uint8_t len = 0; // Length of payload
uint8_t payload[MAVLITE_MAX_PAYLOAD_LEN];
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:
bool get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const WARN_IF_UNUSED;
bool set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count) WARN_IF_UNUSED;
};

104
libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.cpp Normal file
View File

@ -0,0 +1,104 @@
#include "AP_Frsky_MAVlite_SPortToMAVlite.h"
#include "AP_Frsky_MAVlite.h"
void AP_Frsky_MAVlite_SPortToMAVlite::reset(void)
{
checksum = 0;
expected_seq = 0;
payload_next_byte = 0;
parse_state = State::WANT_LEN;
}
void AP_Frsky_MAVlite_SPortToMAVlite::update_checksum(const uint8_t c)
{
checksum += c; //0-1FF
checksum += checksum >> 8;
checksum &= 0xFF;
}
/*
Parses sport packets and if successfull fills the rxmsg mavlite struct
*/
bool AP_Frsky_MAVlite_SPortToMAVlite::process(AP_Frsky_MAVlite_Message &rxmsg, const AP_Frsky_SPort::sport_packet_t &packet)
{
// the two skipped bytes in packet.raw here are sensor and frame.
// appid and data are used to transport the mavlite message.
// deal with packet sequence number:
const uint8_t received_seq = (packet.raw[2] & 0x3F);
// if the first byte of any sport passthrough packet is zero then we reset:
if (received_seq == 0) {
reset();
}
if (received_seq != expected_seq) {
parse_state = State::ERROR;
return false;
}
update_checksum(received_seq);
expected_seq = received_seq + 1;
// deal with the remainder (post-sequence) of the packet:
for (uint8_t i=3; i<ARRAY_SIZE(packet.raw); i++) {
parse(packet.raw[i]);
}
if (parse_state == State::MESSAGE_RECEIVED) {
rxmsg = _rxmsg;
return true;
}
return false;
}
void AP_Frsky_MAVlite_SPortToMAVlite::parse(uint8_t byte)
{
switch (parse_state) {
case State::IDLE:
// it is an error to receive anything but offset==0 byte=0xx0
// in this state
parse_state = State::ERROR;
return;
case State::ERROR:
// waiting for offset==0 && byte==0x00 to bump us into WANT_LEN
return;
case State::WANT_LEN:
_rxmsg.len = byte;
update_checksum(byte);
parse_state = State::WANT_MSGID;
return;
case State::WANT_MSGID:
_rxmsg.msgid = byte;
update_checksum(byte);
if (_rxmsg.len == 0) {
parse_state = State::WANT_CHECKSUM;
} else {
parse_state = State::WANT_PAYLOAD;
}
return;
case State::WANT_PAYLOAD:
// add byte to payload
_rxmsg.payload[payload_next_byte++] = byte;
update_checksum(byte);
if (payload_next_byte >= _rxmsg.len) {
parse_state = State::WANT_CHECKSUM;
}
return;
case State::WANT_CHECKSUM:
if (checksum != byte) {
parse_state = State::ERROR;
return;
}
parse_state = State::MESSAGE_RECEIVED;
return;
case State::MESSAGE_RECEIVED:
return;
}
}

48
libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.h Normal file
View File

@ -0,0 +1,48 @@
#pragma once
#include "AP_Frsky_MAVlite_Message.h"
#include "AP_Frsky_SPort.h"
#include <stdint.h>
/*
* An instance of this class decodes a stream of SPort packets into a
* MAVlite message (see AP_Frsky_MAVlite_Message.h). It is expected
* that the same rxmsg is passed into process() multiple times, each
* time with a new sport packet. If a packet is successfully decodes
* then process() will return true and rxmsg can be used as a MAVlite
* message.
*
* See AP_Frsky_MAVlite.h for a description of the encoding of a
* MAVlite message in SPort packets.
*/
class AP_Frsky_MAVlite_SPortToMAVlite {
public:
bool process(AP_Frsky_MAVlite_Message &rxmsg,
const AP_Frsky_SPort::sport_packet_t &packet) WARN_IF_UNUSED;
private:
void reset();
uint8_t expected_seq;
uint8_t payload_next_byte;
enum class State : uint8_t {
IDLE=0,
ERROR,
WANT_LEN,
WANT_MSGID,
WANT_PAYLOAD,
WANT_CHECKSUM,
MESSAGE_RECEIVED,
};
State parse_state = State::IDLE;
AP_Frsky_MAVlite_Message _rxmsg;
void parse(const uint8_t byte);
int16_t checksum; // sent at end of packet
void update_checksum(const uint8_t c);
};

2
libraries/AP_Frsky_Telem/AP_Frsky_SPort.h
View File

@ -33,6 +33,8 @@ protected:
uint32_t calc_gps_latlng(bool &send_latitude);
static uint8_t calc_sensor_id(const uint8_t physical_id);
private:
struct {

73
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
View File

@ -674,9 +674,9 @@ 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;
AP_Frsky_MAVlite_Message rxmsg;
if (_mavlite_handler.parse(rxmsg, packet)) {
if (sport_to_mavlite.process(rxmsg, packet)) {
mavlite_process_message(rxmsg);
break; // process only 1 mavlite message each call
}
@ -711,19 +711,26 @@ void AP_Frsky_SPort_Passthrough::process_tx_queue()
* 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)
void AP_Frsky_SPort_Passthrough::mavlite_handle_command_long(const AP_Frsky_MAVlite_Message &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
if (!rxmsg.get_uint16(mav_command_long.command, 0)) {
return;
}
if (!rxmsg.get_uint8(cmd_options, 2)) {
return;
}
uint8_t param_count = AP_Frsky_MAVlite_Message::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
// base offset is 3, relative offset is 4*cmd_idx
if (!rxmsg.get_float(params[cmd_idx], 3+(4*cmd_idx))) {
return;
}
}
mav_command_long.param1 = params[0];
@ -796,10 +803,14 @@ void AP_Frsky_SPort_Passthrough::mavlite_handle_command_long(const AP_Frsky_MAVl
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;
AP_Frsky_MAVlite_Message 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);
if (!txmsg.set_uint16(cmdid, 0)) {
return;
}
if (!txmsg.set_uint8((uint8_t)mav_result, 2)) {
return;
}
mavlite_send_message(txmsg);
}
@ -844,20 +855,26 @@ MAV_RESULT AP_Frsky_SPort_Passthrough::mavlite_handle_command_do_fence_enable(ui
* 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)
void AP_Frsky_SPort_Passthrough::mavlite_handle_param_request_read(const AP_Frsky_MAVlite_Message &rxmsg)
{
float param_value;
char param_name[AP_MAX_NAME_SIZE+1];
AP_Frsky_MAVlite::mavlite_msg_get_string(param_name, rxmsg, 0);
if (!rxmsg.get_string(param_name, 0)) {
return;
}
// 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;
AP_Frsky_MAVlite_Message 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);
if (!txmsg.set_float(param_value, 0)) {
return;
}
if (!txmsg.set_string(param_name, 4)) {
return;
}
mavlite_send_message(txmsg);
}
@ -865,13 +882,17 @@ void AP_Frsky_SPort_Passthrough::mavlite_handle_param_request_read(const AP_Frsk
* 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)
void AP_Frsky_SPort_Passthrough::mavlite_handle_param_set(const AP_Frsky_MAVlite_Message &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);
if (!rxmsg.get_float(param_value, 0)) {
return;
}
if (!rxmsg.get_string(param_name, 4)) {
return;
}
// find existing param so we can get the old value
enum ap_var_type var_type;
// set parameter
@ -891,10 +912,14 @@ void AP_Frsky_SPort_Passthrough::mavlite_handle_param_set(const AP_Frsky_MAVlite
gcs().send_text(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name);
return;
}
AP_Frsky_MAVlite::mavlite_message_t txmsg;
AP_Frsky_MAVlite_Message 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);
if (!txmsg.set_float(param_value, 0)) {
return;
}
if (!txmsg.set_string(param_name, 4)) {
return;
}
mavlite_send_message(txmsg);
}
@ -945,7 +970,7 @@ MAV_RESULT AP_Frsky_SPort_Passthrough::mavlite_handle_command_preflight_reboot(v
* 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)
void AP_Frsky_SPort_Passthrough::mavlite_process_message(const AP_Frsky_MAVlite_Message &rxmsg)
{
switch (rxmsg.msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
@ -965,9 +990,9 @@ void AP_Frsky_SPort_Passthrough::mavlite_process_message(const AP_Frsky_MAVlite:
* 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)
bool AP_Frsky_SPort_Passthrough::mavlite_send_message(AP_Frsky_MAVlite_Message &txmsg)
{
return _mavlite_handler.encode(_SPort_bidir.tx_packet_queue, txmsg);
return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg);
}

15
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h
View File

@ -9,6 +9,8 @@
#include "AP_Frsky_Telem.h"
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
#include "AP_Frsky_MAVlite_SPortToMAVlite.h"
#include "AP_Frsky_MAVlite_MAVliteToSPort.h"
#define FRSKY_WFQ_TIME_SLOT_MAX 12U
#define SPORT_TX_PACKET_DUPLICATES 1 // number of duplicates packets we send (fport only)
#else
@ -116,7 +118,8 @@ private:
} _SPort_bidir;
AP_Frsky_SPortParser _sport_handler;
AP_Frsky_MAVlite _mavlite_handler;
AP_Frsky_MAVlite_SPortToMAVlite sport_to_mavlite;
AP_Frsky_MAVlite_MAVliteToSPort mavlite_to_sport;
void set_sensor_id(AP_Int8 idx, uint8_t &sensor);
// tx/rx sport packet processing
@ -125,13 +128,13 @@ private:
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);
bool mavlite_send_message(AP_Frsky_MAVlite_Message &txmsg);
void mavlite_process_message(const AP_Frsky_MAVlite_Message &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_handle_param_request_read(const AP_Frsky_MAVlite_Message &rxmsg);
void mavlite_handle_param_set(const AP_Frsky_MAVlite_Message &rxmsg);
void mavlite_handle_command_long(const AP_Frsky_MAVlite_Message &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);