mirror of https://github.com/ArduPilot/ardupilot
AP_RCProtocol: IRC Ghost protocol
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
0467ccc1fd
commit
19c6b0b8ae
|
|
@ -33,6 +33,7 @@
|
||||||
#include "AP_RCProtocol_FPort.h"
|
#include "AP_RCProtocol_FPort.h"
|
||||||
#include "AP_RCProtocol_FPort2.h"
|
#include "AP_RCProtocol_FPort2.h"
|
||||||
#include "AP_RCProtocol_DroneCAN.h"
|
#include "AP_RCProtocol_DroneCAN.h"
|
||||||
|
#include "AP_RCProtocol_GHST.h"
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <RC_Channel/RC_Channel.h>
|
#include <RC_Channel/RC_Channel.h>
|
||||||
|
|
||||||
|
|
@ -82,6 +83,9 @@ void AP_RCProtocol::init()
|
||||||
#if AP_RCPROTOCOL_DRONECAN_ENABLED
|
#if AP_RCPROTOCOL_DRONECAN_ENABLED
|
||||||
backend[AP_RCProtocol::DRONECAN] = new AP_RCProtocol_DroneCAN(*this);
|
backend[AP_RCProtocol::DRONECAN] = new AP_RCProtocol_DroneCAN(*this);
|
||||||
#endif
|
#endif
|
||||||
|
#if AP_RCPROTOCOL_GHST_ENABLED
|
||||||
|
backend[AP_RCProtocol::GHST] = new AP_RCProtocol_GHST(*this);
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
AP_RCProtocol::~AP_RCProtocol()
|
AP_RCProtocol::~AP_RCProtocol()
|
||||||
|
|
@ -303,6 +307,10 @@ static const AP_RCProtocol::SerialConfig serial_configs[] {
|
||||||
// CRSFv3 can negotiate higher rates which are sticky on soft reboot
|
// CRSFv3 can negotiate higher rates which are sticky on soft reboot
|
||||||
{ 2000000, 0, 1, false },
|
{ 2000000, 0, 1, false },
|
||||||
#endif
|
#endif
|
||||||
|
#if AP_RCPROTOCOL_GHST_ENABLED
|
||||||
|
// Ghost:
|
||||||
|
{ 420000, 0, 1, false },
|
||||||
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config");
|
static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config");
|
||||||
|
|
@ -504,6 +512,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
|
||||||
#if AP_RCPROTOCOL_DRONECAN_ENABLED
|
#if AP_RCPROTOCOL_DRONECAN_ENABLED
|
||||||
case DRONECAN:
|
case DRONECAN:
|
||||||
return "DroneCAN";
|
return "DroneCAN";
|
||||||
|
#endif
|
||||||
|
#if AP_RCPROTOCOL_GHST_ENABLED
|
||||||
|
case GHST:
|
||||||
|
return "GHST";
|
||||||
#endif
|
#endif
|
||||||
case NONE:
|
case NONE:
|
||||||
break;
|
break;
|
||||||
|
|
|
||||||
|
|
@ -69,6 +69,9 @@ public:
|
||||||
#endif
|
#endif
|
||||||
#if AP_RCPROTOCOL_DRONECAN_ENABLED
|
#if AP_RCPROTOCOL_DRONECAN_ENABLED
|
||||||
DRONECAN = 13,
|
DRONECAN = 13,
|
||||||
|
#endif
|
||||||
|
#if AP_RCPROTOCOL_GHST_ENABLED
|
||||||
|
GHST = 14,
|
||||||
#endif
|
#endif
|
||||||
NONE //last enum always is None
|
NONE //last enum always is None
|
||||||
};
|
};
|
||||||
|
|
@ -129,6 +132,9 @@ public:
|
||||||
#endif
|
#endif
|
||||||
#if AP_RCPROTOCOL_CRSF_ENABLED
|
#if AP_RCPROTOCOL_CRSF_ENABLED
|
||||||
case CRSF:
|
case CRSF:
|
||||||
|
#endif
|
||||||
|
#if AP_RCPROTOCOL_GHST_ENABLED
|
||||||
|
case GHST:
|
||||||
#endif
|
#endif
|
||||||
return true;
|
return true;
|
||||||
#if AP_RCPROTOCOL_IBUS_ENABLED
|
#if AP_RCPROTOCOL_IBUS_ENABLED
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,434 @@
|
||||||
|
/*
|
||||||
|
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/>.
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
GHST protocol decoder based on betaflight implementation
|
||||||
|
Code by Andy Piper
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_GHST_ENABLED
|
||||||
|
|
||||||
|
#include "AP_RCProtocol.h"
|
||||||
|
#include "AP_RCProtocol_GHST.h"
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include <AP_Math/crc.h>
|
||||||
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||||
|
#include <AP_RCTelemetry/AP_GHST_Telem.h>
|
||||||
|
#include <AP_SerialManager/AP_SerialManager.h>
|
||||||
|
|
||||||
|
/*
|
||||||
|
* GHST protocol
|
||||||
|
*
|
||||||
|
* GHST protocol uses a single wire half duplex uart connection.
|
||||||
|
*
|
||||||
|
* 420000 baud
|
||||||
|
* not inverted
|
||||||
|
* 8 Bit
|
||||||
|
* 1 Stop bit
|
||||||
|
* Big endian
|
||||||
|
* Max frame size is 14 bytes
|
||||||
|
*
|
||||||
|
* Every frame has the structure:
|
||||||
|
* <Device address><Frame length><Type><Payload><CRC>
|
||||||
|
*
|
||||||
|
* Device address: (uint8_t)
|
||||||
|
* Frame length: length in bytes including Type (uint8_t)
|
||||||
|
* Type: (uint8_t)
|
||||||
|
* CRC: (uint8_t)
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
//#define GHST_DEBUG
|
||||||
|
//#define GHST_DEBUG_CHARS
|
||||||
|
#ifdef GHST_DEBUG
|
||||||
|
# define debug(fmt, args...) hal.console->printf("GHST: " fmt "\n", ##args)
|
||||||
|
static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0)
|
||||||
|
{
|
||||||
|
switch(byte) {
|
||||||
|
case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_5TO8:
|
||||||
|
case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_5TO8:
|
||||||
|
return "RC5_8";
|
||||||
|
case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_9TO12:
|
||||||
|
case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_9TO12:
|
||||||
|
return "RC9_12";
|
||||||
|
case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_13TO16:
|
||||||
|
case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_13TO16:
|
||||||
|
return "RC13_16";
|
||||||
|
case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_RSSI:
|
||||||
|
case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_12_RSSI:
|
||||||
|
return "RSSI";
|
||||||
|
case AP_RCProtocol_GHST::GHST_UL_RC_VTX_CTRL:
|
||||||
|
return "VTX_CTRL";
|
||||||
|
case AP_RCProtocol_GHST::GHST_UL_VTX_SETUP:
|
||||||
|
return "VTX_SETUP";
|
||||||
|
}
|
||||||
|
return "UNKNOWN";
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
# define debug(fmt, args...) do {} while(0)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define GHST_MAX_FRAME_TIME_US 500U // 14 bytes @ 420k = ~450us
|
||||||
|
#define GHST_FRAME_TIMEOUT_US 10000U // 10ms to account for scheduling delays
|
||||||
|
#define GHST_INTER_FRAME_TIME_US 2000U // At fastest, frames are sent by the transmitter every 2 ms, 500 Hz
|
||||||
|
#define GHST_HEADER_TYPE_LEN (GHST_HEADER_LEN + 1) // header length including type
|
||||||
|
|
||||||
|
const uint16_t AP_RCProtocol_GHST::RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES] = {
|
||||||
|
55, 160, 250, 19, 250, 500, 150, 250,
|
||||||
|
};
|
||||||
|
|
||||||
|
AP_RCProtocol_GHST* AP_RCProtocol_GHST::_singleton;
|
||||||
|
|
||||||
|
AP_RCProtocol_GHST::AP_RCProtocol_GHST(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend)
|
||||||
|
{
|
||||||
|
#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
|
||||||
|
if (_singleton != nullptr) {
|
||||||
|
AP_HAL::panic("Duplicate GHST handler");
|
||||||
|
}
|
||||||
|
|
||||||
|
_singleton = this;
|
||||||
|
#else
|
||||||
|
if (_singleton == nullptr) {
|
||||||
|
_singleton = this;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
AP_RCProtocol_GHST::~AP_RCProtocol_GHST() {
|
||||||
|
_singleton = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get the protocol string
|
||||||
|
const char* AP_RCProtocol_GHST::get_protocol_string() const {
|
||||||
|
return "GHST";
|
||||||
|
}
|
||||||
|
|
||||||
|
// return the link rate as defined by the LinkStatistics
|
||||||
|
uint16_t AP_RCProtocol_GHST::get_link_rate() const {
|
||||||
|
return RF_MODE_RATES[_link_status.rf_mode - GHST_RF_MODE_NORMAL];
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_RCProtocol_GHST::_process_byte(uint32_t timestamp_us, uint8_t byte)
|
||||||
|
{
|
||||||
|
//debug("process_byte(0x%x)", byte);
|
||||||
|
// we took too long decoding, start again - the RX will only send complete frames so this is unlikely to fail,
|
||||||
|
// however thread scheduling can introduce longer delays even when the data has been received
|
||||||
|
if (_frame_ofs > 0 && (timestamp_us - _start_frame_time_us) > GHST_FRAME_TIMEOUT_US) {
|
||||||
|
_frame_ofs = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// overflow check
|
||||||
|
if (_frame_ofs >= GHST_FRAMELEN_MAX) {
|
||||||
|
_frame_ofs = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// start of a new frame
|
||||||
|
if (_frame_ofs == 0) {
|
||||||
|
_start_frame_time_us = timestamp_us;
|
||||||
|
}
|
||||||
|
|
||||||
|
add_to_buffer(_frame_ofs++, byte);
|
||||||
|
|
||||||
|
// need a header to get the length
|
||||||
|
if (_frame_ofs < GHST_HEADER_TYPE_LEN) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// parse the length
|
||||||
|
if (_frame_ofs == GHST_HEADER_TYPE_LEN) {
|
||||||
|
_frame_crc = crc8_dvb_s2(0, _frame.type);
|
||||||
|
// check for garbage frame
|
||||||
|
if (_frame.length > GHST_FRAME_PAYLOAD_MAX) {
|
||||||
|
_frame_ofs = 0;
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// update crc
|
||||||
|
if (_frame_ofs < _frame.length + GHST_HEADER_LEN) {
|
||||||
|
_frame_crc = crc8_dvb_s2(_frame_crc, byte);
|
||||||
|
}
|
||||||
|
|
||||||
|
// overflow check
|
||||||
|
if (_frame_ofs > _frame.length + GHST_HEADER_LEN) {
|
||||||
|
_frame_ofs = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// decode whatever we got and expect
|
||||||
|
if (_frame_ofs == _frame.length + GHST_HEADER_LEN) {
|
||||||
|
log_data(AP_RCProtocol::GHST, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - GHST_HEADER_LEN);
|
||||||
|
|
||||||
|
// we consumed the partial frame, reset
|
||||||
|
_frame_ofs = 0;
|
||||||
|
|
||||||
|
// bad CRC (payload start is +1 from frame start, so need to subtract that from frame length to get index)
|
||||||
|
if (_frame_crc != _frame.payload[_frame.length - 2]) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
_last_frame_time_us = _last_rx_frame_time_us = timestamp_us;
|
||||||
|
// decode here
|
||||||
|
if (decode_ghost_packet()) {
|
||||||
|
_last_tx_frame_time_us = timestamp_us; // we have received a frame from the transmitter
|
||||||
|
add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_RCProtocol_GHST::update(void)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
// write out a frame of any type
|
||||||
|
bool AP_RCProtocol_GHST::write_frame(Frame* frame)
|
||||||
|
{
|
||||||
|
AP_HAL::UARTDriver *uart = get_current_UART();
|
||||||
|
|
||||||
|
if (!uart) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// check that we haven't been too slow in responding to the new UART data. If we respond too late then we will
|
||||||
|
// corrupt the next incoming control frame. incoming packets at max 126bits @500Hz @420k baud gives total budget of 2ms
|
||||||
|
// per packet of which we need 300us to receive a packet. outgoing packets are 126bits which require 300us to send
|
||||||
|
// leaving at most 1.4ms of delay that can be tolerated
|
||||||
|
uint64_t tend = uart->receive_time_constraint_us(1);
|
||||||
|
uint64_t now = AP_HAL::micros64();
|
||||||
|
uint64_t tdelay = now - tend;
|
||||||
|
if (tdelay > 1000) {
|
||||||
|
// we've been too slow in responding
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// calculate crc
|
||||||
|
uint8_t crc = crc8_dvb_s2(0, frame->type);
|
||||||
|
for (uint8_t i = 0; i < frame->length - 2; i++) {
|
||||||
|
crc = crc8_dvb_s2(crc, frame->payload[i]);
|
||||||
|
}
|
||||||
|
frame->payload[frame->length - 2] = crc;
|
||||||
|
|
||||||
|
uart->write((uint8_t*)frame, frame->length + 2);
|
||||||
|
uart->flush();
|
||||||
|
|
||||||
|
#ifdef GHST_DEBUG
|
||||||
|
hal.console->printf("GHST: writing %s:", get_frame_type(frame->type, frame->payload[0]));
|
||||||
|
for (uint8_t i = 0; i < frame->length + 2; i++) {
|
||||||
|
uint8_t val = ((uint8_t*)frame)[i];
|
||||||
|
#ifdef GHST_DEBUG_CHARS
|
||||||
|
if (val >= 32 && val <= 126) {
|
||||||
|
hal.console->printf(" 0x%x '%c'", val, (char)val);
|
||||||
|
} else {
|
||||||
|
#endif
|
||||||
|
hal.console->printf(" 0x%x", val);
|
||||||
|
#ifdef GHST_DEBUG_CHARS
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
hal.console->printf("\n");
|
||||||
|
#endif
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_RCProtocol_GHST::decode_ghost_packet()
|
||||||
|
{
|
||||||
|
#ifdef GHST_DEBUG
|
||||||
|
hal.console->printf("GHST: received %s:", get_frame_type(_frame.type));
|
||||||
|
uint8_t* fptr = (uint8_t*)&_frame;
|
||||||
|
for (uint8_t i = 0; i < _frame.length + 2; i++) {
|
||||||
|
#ifdef GHST_DEBUG_CHARS
|
||||||
|
if (fptr[i] >= 32 && fptr[i] <= 126) {
|
||||||
|
hal.console->printf(" 0x%x '%c'", fptr[i], (char)fptr[i]);
|
||||||
|
} else {
|
||||||
|
#endif
|
||||||
|
hal.console->printf(" 0x%x", fptr[i]);
|
||||||
|
#ifdef GHST_DEBUG_CHARS
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
hal.console->printf("\n");
|
||||||
|
#endif
|
||||||
|
|
||||||
|
const RadioFrame* radio_frame = (const RadioFrame*)(&_frame.payload);
|
||||||
|
const Channels12Bit_4Chan* channels = &(radio_frame->channels);
|
||||||
|
const uint8_t* lowres_channels = radio_frame->lowres_channels;
|
||||||
|
|
||||||
|
// Scaling from Betaflight
|
||||||
|
// Scaling 12bit channels (8bit channels in brackets)
|
||||||
|
// OpenTx RC PWM (BF)
|
||||||
|
// min -1024 0( 0) 988us
|
||||||
|
// ctr 0 2048(128) 1500us
|
||||||
|
// max 1024 4096(256) 2012us
|
||||||
|
//
|
||||||
|
|
||||||
|
// Scaling legacy (nearly 10bit)
|
||||||
|
// derived from original SBus scaling, with slight correction for offset
|
||||||
|
// now symmetrical around OpenTx 0 value
|
||||||
|
// scaling is:
|
||||||
|
// OpenTx RC PWM (BF)
|
||||||
|
// min -1024 172( 22) 988us
|
||||||
|
// ctr 0 992(124) 1500us
|
||||||
|
// max 1024 1811(226) 2012us
|
||||||
|
|
||||||
|
#define CHANNEL_RESCALE(x) (((5 * x) >> 2) - 430)
|
||||||
|
#define CHANNEL_SCALE(x) (int32_t(x) / 4 + 988)
|
||||||
|
#define CHANNEL_SCALE_LEGACY(x) CHANNEL_SCALE(CHANNEL_RESCALE(x))
|
||||||
|
|
||||||
|
// legacy scaling
|
||||||
|
if (_frame.type >= GHST_UL_RC_CHANS_HS4_5TO8 && _frame.type <= GHST_UL_RC_CHANS_RSSI) {
|
||||||
|
_channels[0] = CHANNEL_SCALE_LEGACY(channels->ch0);
|
||||||
|
_channels[1] = CHANNEL_SCALE_LEGACY(channels->ch1);
|
||||||
|
_channels[2] = CHANNEL_SCALE_LEGACY(channels->ch2);
|
||||||
|
_channels[3] = CHANNEL_SCALE_LEGACY(channels->ch3);
|
||||||
|
} else {
|
||||||
|
_channels[0] = CHANNEL_SCALE(channels->ch0);
|
||||||
|
_channels[1] = CHANNEL_SCALE(channels->ch1);
|
||||||
|
_channels[2] = CHANNEL_SCALE(channels->ch2);
|
||||||
|
_channels[3] = CHANNEL_SCALE(channels->ch3);
|
||||||
|
}
|
||||||
|
|
||||||
|
#define CHANNEL_LR_RESCALE(x) (5 * x - 108)
|
||||||
|
#define CHANNEL_LR_SCALE(x) (int32_t(x) * 2 + 988)
|
||||||
|
#define CHANNEL_LR_SCALE_LEGACY(x) (CHANNEL_LR_RESCALE(x) + 988)
|
||||||
|
|
||||||
|
switch (_frame.type) {
|
||||||
|
case GHST_UL_RC_CHANS_HS4_5TO8:
|
||||||
|
case GHST_UL_RC_CHANS_HS4_9TO12:
|
||||||
|
case GHST_UL_RC_CHANS_HS4_13TO16: {
|
||||||
|
uint8_t offset = (_frame.type - GHST_UL_RC_CHANS_HS4_5TO8 + 1) * 4;
|
||||||
|
_channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[0]);
|
||||||
|
_channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[1]);
|
||||||
|
_channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[2]);
|
||||||
|
_channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[3]);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case GHST_UL_RC_CHANS_HS4_12_5TO8:
|
||||||
|
case GHST_UL_RC_CHANS_HS4_12_9TO12:
|
||||||
|
case GHST_UL_RC_CHANS_HS4_12_13TO16: {
|
||||||
|
uint8_t offset = (_frame.type - GHST_UL_RC_CHANS_HS4_12_5TO8 + 1) * 4;
|
||||||
|
_channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[0]);
|
||||||
|
_channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[1]);
|
||||||
|
_channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[2]);
|
||||||
|
_channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[3]);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case GHST_UL_RC_CHANS_RSSI:
|
||||||
|
case GHST_UL_RC_CHANS_12_RSSI:
|
||||||
|
process_link_stats_frame((uint8_t*)&_frame.payload);
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
#if AP_GHST_TELEM_ENABLED
|
||||||
|
if (AP_GHST_Telem::process_frame(FrameType(_frame.type), (uint8_t*)&_frame.payload)) {
|
||||||
|
process_telemetry();
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// send out telemetry
|
||||||
|
bool AP_RCProtocol_GHST::process_telemetry(bool check_constraint)
|
||||||
|
{
|
||||||
|
|
||||||
|
AP_HAL::UARTDriver *uart = get_current_UART();
|
||||||
|
if (!uart) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!telem_available) {
|
||||||
|
#if AP_GHST_TELEM_ENABLED && !APM_BUILD_TYPE(APM_BUILD_iofirmware)
|
||||||
|
if (AP_GHST_Telem::get_telem_data(&_telemetry_frame, is_tx_active())) {
|
||||||
|
telem_available = true;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
return false;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
if (!write_frame(&_telemetry_frame)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get fresh telem_data in the next call
|
||||||
|
telem_available = false;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// process link statistics to get RSSI
|
||||||
|
void AP_RCProtocol_GHST::process_link_stats_frame(const void* data)
|
||||||
|
{
|
||||||
|
const LinkStatisticsFrame* link = (const LinkStatisticsFrame*)data;
|
||||||
|
|
||||||
|
uint8_t rssi_dbm;
|
||||||
|
rssi_dbm = link->rssi_dbm;
|
||||||
|
_link_status.link_quality = link->link_quality;
|
||||||
|
if (_use_lq_for_rssi) {
|
||||||
|
_link_status.rssi = derive_scaled_lq_value(link->link_quality);
|
||||||
|
} else{
|
||||||
|
// AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
|
||||||
|
if (rssi_dbm < 50) {
|
||||||
|
_link_status.rssi = 255;
|
||||||
|
} else if (rssi_dbm > 120) {
|
||||||
|
_link_status.rssi = 0;
|
||||||
|
} else {
|
||||||
|
// this is an approximation recommended by Remo from TBS
|
||||||
|
_link_status.rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_link_status.rf_mode = link->protocol;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_RCProtocol_GHST::is_telemetry_supported() const
|
||||||
|
{
|
||||||
|
return _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_NORMAL
|
||||||
|
|| _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_RACE
|
||||||
|
|| _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_LR
|
||||||
|
|| _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_RACE250;
|
||||||
|
}
|
||||||
|
|
||||||
|
// process a byte provided by a uart
|
||||||
|
void AP_RCProtocol_GHST::process_byte(uint8_t byte, uint32_t baudrate)
|
||||||
|
{
|
||||||
|
// reject RC data if we have been configured for standalone mode
|
||||||
|
if (baudrate != GHST_BAUDRATE) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
_process_byte(AP_HAL::micros(), byte);
|
||||||
|
}
|
||||||
|
|
||||||
|
//returns uplink link quality on 0-255 scale
|
||||||
|
int16_t AP_RCProtocol_GHST::derive_scaled_lq_value(uint8_t uplink_lq)
|
||||||
|
{
|
||||||
|
return int16_t(roundf(constrain_float(uplink_lq*2.5f,0,255)));
|
||||||
|
}
|
||||||
|
|
||||||
|
namespace AP {
|
||||||
|
AP_RCProtocol_GHST* ghost() {
|
||||||
|
return AP_RCProtocol_GHST::get_singleton();
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_GHST_ENABLED
|
||||||
|
|
@ -0,0 +1,198 @@
|
||||||
|
/*
|
||||||
|
* This file 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 file 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_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_GHST_ENABLED
|
||||||
|
|
||||||
|
#include "AP_RCProtocol.h"
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include <RC_Channel/RC_Channel.h>
|
||||||
|
#include "SoftSerial.h"
|
||||||
|
|
||||||
|
#define GHST_MAX_CHANNELS 16U // Maximum number of channels from GHST datastream
|
||||||
|
#define GHST_FRAMELEN_MAX 14U // maximum possible framelength
|
||||||
|
#define GHST_HEADER_LEN 2U // header length
|
||||||
|
#define GHST_FRAME_PAYLOAD_MAX (GHST_FRAMELEN_MAX - GHST_HEADER_LEN) // maximum size of the frame length field in a packet
|
||||||
|
#define GHST_BAUDRATE 420000U
|
||||||
|
#define GHST_TX_TIMEOUT 500000U // the period after which the transmitter is considered disconnected (matches copters failsafe)
|
||||||
|
#define GHST_RX_TIMEOUT 150000U // the period after which the receiver is considered disconnected (>ping frequency)
|
||||||
|
|
||||||
|
class AP_RCProtocol_GHST : public AP_RCProtocol_Backend {
|
||||||
|
public:
|
||||||
|
AP_RCProtocol_GHST(AP_RCProtocol &_frontend);
|
||||||
|
virtual ~AP_RCProtocol_GHST();
|
||||||
|
void process_byte(uint8_t byte, uint32_t baudrate) override;
|
||||||
|
void update(void) override;
|
||||||
|
|
||||||
|
// is the receiver active, used to detect power loss and baudrate changes
|
||||||
|
bool is_rx_active() const override {
|
||||||
|
return AP_HAL::micros() < _last_rx_frame_time_us + GHST_RX_TIMEOUT;
|
||||||
|
}
|
||||||
|
|
||||||
|
// is the transmitter active, used to adjust telemetry data
|
||||||
|
bool is_tx_active() const {
|
||||||
|
// this is the same as the Copter failsafe timeout
|
||||||
|
return AP_HAL::micros() < _last_tx_frame_time_us + GHST_TX_TIMEOUT;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get singleton instance
|
||||||
|
static AP_RCProtocol_GHST* get_singleton() {
|
||||||
|
return _singleton;
|
||||||
|
}
|
||||||
|
|
||||||
|
enum FrameType {
|
||||||
|
GHST_UL_RC_CHANS_HS4_5TO8 = 0x10, // Control packet with 4 primary channels + CH5-8
|
||||||
|
GHST_UL_RC_CHANS_HS4_9TO12 = 0x11, // Control packet with 4 primary channels + CH9-12
|
||||||
|
GHST_UL_RC_CHANS_HS4_13TO16 = 0x12, // Control packet with 4 primary channels + CH13-16
|
||||||
|
GHST_UL_RC_CHANS_RSSI = 0x13, // Control packet with RSSI and LQ data
|
||||||
|
GHST_UL_RC_VTX_CTRL = 0x14, // Goggle/FC channel changing
|
||||||
|
// -> 0x1F reserved
|
||||||
|
GHST_UL_VTX_SETUP = 0x20, // vTx Setup w/o 4 primary channels (GECO Only)
|
||||||
|
GHST_UL_MSP_REQ = 0x21, // MSP frame, Request
|
||||||
|
GHST_UL_MSP_WRITE = 0x22, // MSP frame, Write
|
||||||
|
|
||||||
|
GHST_DL_PACK_STAT = 0x23, // Battery Status
|
||||||
|
GHST_DL_GPS_PRIMARY = 0x25, // Primary GPS Data
|
||||||
|
GHST_DL_GPS_SECONDARY = 0x26, // Secondary GPS Data
|
||||||
|
GHST_DL_MAGBARO = 0x27, // Magnetometer, Barometer (and Vario) Data
|
||||||
|
GHST_DL_MSP_RESP = 0x28, // MSP Response
|
||||||
|
|
||||||
|
GHST_UL_RC_CHANS_HS4_12_5TO8 = 0x30, // Control packet with 4 primary channels + CH5-8
|
||||||
|
GHST_UL_RC_CHANS_HS4_12_9TO12 = 0x31, // Control packet with 4 primary channels + CH9-12
|
||||||
|
GHST_UL_RC_CHANS_HS4_12_13TO16 = 0x32, // Control packet with 4 primary channels + CH13-16
|
||||||
|
GHST_UL_RC_CHANS_12_RSSI = 0x33, // Control packet with RSSI and LQ data
|
||||||
|
// 0x30 -> 0x3f - raw 12 bit packets
|
||||||
|
};
|
||||||
|
|
||||||
|
enum DeviceAddress {
|
||||||
|
GHST_ADDRESS_FLIGHT_CONTROLLER = 0x82,
|
||||||
|
GHST_ADDRESS_GOGGLES = 0x83,
|
||||||
|
GHST_ADDRESS_GHST_RECEIVER = 0x89,
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Frame {
|
||||||
|
uint8_t device_address;
|
||||||
|
uint8_t length;
|
||||||
|
uint8_t type;
|
||||||
|
uint8_t payload[GHST_FRAME_PAYLOAD_MAX - 1]; // type is already accounted for
|
||||||
|
} PACKED;
|
||||||
|
|
||||||
|
struct Channels12Bit_4Chan {
|
||||||
|
#if __BYTE_ORDER != __LITTLE_ENDIAN
|
||||||
|
#error "Only supported on little-endian architectures"
|
||||||
|
#endif
|
||||||
|
uint32_t ch0 : 12;
|
||||||
|
uint32_t ch1 : 12;
|
||||||
|
uint32_t ch2 : 12;
|
||||||
|
uint32_t ch3 : 12;
|
||||||
|
} PACKED;
|
||||||
|
|
||||||
|
struct RadioFrame {
|
||||||
|
#if __BYTE_ORDER != __LITTLE_ENDIAN
|
||||||
|
#error "Only supported on little-endian architectures"
|
||||||
|
#endif
|
||||||
|
Channels12Bit_4Chan channels; // high-res channels
|
||||||
|
uint8_t lowres_channels[4]; // low-res channels
|
||||||
|
} PACKED;
|
||||||
|
|
||||||
|
struct LinkStatisticsFrame {
|
||||||
|
#if __BYTE_ORDER != __LITTLE_ENDIAN
|
||||||
|
#error "Only supported on little-endian architectures"
|
||||||
|
#endif
|
||||||
|
Channels12Bit_4Chan channels;
|
||||||
|
uint8_t link_quality; // ( 0 - 100)
|
||||||
|
uint8_t rssi_dbm; // ( dBm * -1 )
|
||||||
|
uint8_t protocol : 5;
|
||||||
|
uint8_t telemetry : 1;
|
||||||
|
uint8_t alt_scale : 1;
|
||||||
|
uint8_t reserved : 1;
|
||||||
|
int8_t tx_power;
|
||||||
|
} PACKED;
|
||||||
|
|
||||||
|
enum RFMode {
|
||||||
|
GHST_RF_MODE_NORMAL = 5, // 55Hz
|
||||||
|
GHST_RF_MODE_RACE = 6, // 160Hz
|
||||||
|
GHST_RF_MODE_PURERACE = 7, // 250Hz
|
||||||
|
GHST_RF_MODE_LR = 8, // 19Hz
|
||||||
|
GHST_RF_MODE_RACE250 = 10, // 250Hz
|
||||||
|
GHST_RF_MODE_RACE500 = 11, // 500Hz
|
||||||
|
GHTS_RF_MODE_SOLID150 = 12, // 150Hz
|
||||||
|
GHST_RF_MODE_SOLID250 = 13, // 250Hz
|
||||||
|
RF_MODE_MAX_MODES,
|
||||||
|
RF_MODE_UNKNOWN,
|
||||||
|
};
|
||||||
|
|
||||||
|
struct LinkStatus {
|
||||||
|
int16_t rssi = -1;
|
||||||
|
int16_t link_quality = -1;
|
||||||
|
uint8_t rf_mode;
|
||||||
|
};
|
||||||
|
|
||||||
|
bool is_telemetry_supported() const;
|
||||||
|
|
||||||
|
// this will be used by AP_GHST_Telem to access link status data
|
||||||
|
// from within AP_RCProtocol_GHST thread so no need for cross-thread synch
|
||||||
|
const volatile LinkStatus& get_link_status() const {
|
||||||
|
return _link_status;
|
||||||
|
}
|
||||||
|
|
||||||
|
// return the link rate as defined by the LinkStatistics
|
||||||
|
uint16_t get_link_rate() const;
|
||||||
|
|
||||||
|
// return the protocol string
|
||||||
|
const char* get_protocol_string() const;
|
||||||
|
|
||||||
|
private:
|
||||||
|
struct Frame _frame;
|
||||||
|
struct Frame _telemetry_frame;
|
||||||
|
uint8_t _frame_ofs;
|
||||||
|
uint8_t _frame_crc;
|
||||||
|
|
||||||
|
const uint8_t MAX_CHANNELS = MIN((uint8_t)GHST_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS);
|
||||||
|
|
||||||
|
static AP_RCProtocol_GHST* _singleton;
|
||||||
|
|
||||||
|
void _process_byte(uint32_t timestamp_us, uint8_t byte);
|
||||||
|
bool decode_ghost_packet();
|
||||||
|
bool process_telemetry(bool check_constraint = true);
|
||||||
|
void process_link_stats_frame(const void* data);
|
||||||
|
bool write_frame(Frame* frame);
|
||||||
|
AP_HAL::UARTDriver* get_current_UART() { return get_available_UART(); }
|
||||||
|
|
||||||
|
uint16_t _channels[GHST_MAX_CHANNELS]; /* buffer for extracted RC channel data as pulsewidth in microseconds */
|
||||||
|
|
||||||
|
void add_to_buffer(uint8_t index, uint8_t b) { ((uint8_t*)&_frame)[index] = b; }
|
||||||
|
|
||||||
|
uint32_t _last_frame_time_us;
|
||||||
|
uint32_t _last_tx_frame_time_us;
|
||||||
|
uint32_t _last_uart_start_time_ms;
|
||||||
|
uint32_t _last_rx_frame_time_us;
|
||||||
|
uint32_t _start_frame_time_us;
|
||||||
|
bool telem_available;
|
||||||
|
bool _use_lq_for_rssi;
|
||||||
|
int16_t derive_scaled_lq_value(uint8_t uplink_lq);
|
||||||
|
|
||||||
|
volatile struct LinkStatus _link_status;
|
||||||
|
|
||||||
|
static const uint16_t RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES];
|
||||||
|
};
|
||||||
|
|
||||||
|
namespace AP {
|
||||||
|
AP_RCProtocol_GHST* ghost();
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_GHST_ENABLED
|
||||||
|
|
@ -61,3 +61,7 @@
|
||||||
#ifndef AP_RCPROTOCOL_FASTSBUS_ENABLED
|
#ifndef AP_RCPROTOCOL_FASTSBUS_ENABLED
|
||||||
#define AP_RCPROTOCOL_FASTSBUS_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && AP_RCPROTOCOL_SBUS_ENABLED
|
#define AP_RCPROTOCOL_FASTSBUS_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && AP_RCPROTOCOL_SBUS_ENABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef AP_RCPROTOCOL_GHST_ENABLED
|
||||||
|
#define AP_RCPROTOCOL_GHST_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED
|
||||||
|
#endif
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue