mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-03 09:01:35 +08:00
Ghost protocol
ghst: add ghost protocol
This commit is contained in:
jciberlin
@@ -33,6 +33,7 @@
|
||||
|
||||
add_library(rc
|
||||
crsf.cpp
|
||||
ghst.cpp
|
||||
st24.cpp
|
||||
sumd.cpp
|
||||
sbus.cpp
|
||||
|
||||
@@ -2,3 +2,30 @@
|
||||
#include "common_rc.h"
|
||||
|
||||
__EXPORT rc_decode_buf_t rc_decode_buf;
|
||||
|
||||
uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
|
||||
{
|
||||
crc ^= a;
|
||||
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
if (crc & 0x80) {
|
||||
crc = (crc << 1) ^ 0xD5;
|
||||
|
||||
} else {
|
||||
crc = crc << 1;
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
|
||||
for (int i = 0; i < len; ++i) {
|
||||
crc = crc8_dvb_s2(crc, buf[i]);
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
#include <stdint.h>
|
||||
|
||||
#include "crsf.h"
|
||||
#include "ghst.h"
|
||||
#include "dsm.h"
|
||||
#include "sbus.h"
|
||||
#include "st24.h"
|
||||
@@ -13,6 +14,7 @@
|
||||
typedef struct rc_decode_buf_ {
|
||||
union {
|
||||
crsf_frame_t crsf_frame;
|
||||
ghst_frame_t ghst_frame;
|
||||
dsm_decode_t dsm;
|
||||
sbus_frame_t sbus_frame;
|
||||
ReceiverFcPacket _strxpacket;
|
||||
@@ -22,3 +24,6 @@ typedef struct rc_decode_buf_ {
|
||||
#pragma pack(pop)
|
||||
|
||||
extern rc_decode_buf_t rc_decode_buf;
|
||||
|
||||
uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a);
|
||||
uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len);
|
||||
|
||||
@@ -138,8 +138,6 @@ static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced;
|
||||
*/
|
||||
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
|
||||
|
||||
static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a);
|
||||
static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len);
|
||||
uint8_t crsf_frame_CRC(const crsf_frame_t &frame);
|
||||
|
||||
|
||||
@@ -202,33 +200,6 @@ bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t
|
||||
return ret;
|
||||
}
|
||||
|
||||
static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
|
||||
{
|
||||
crc ^= a;
|
||||
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
if (crc & 0x80) {
|
||||
crc = (crc << 1) ^ 0xD5;
|
||||
|
||||
} else {
|
||||
crc = crc << 1;
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
|
||||
for (int i = 0; i < len; ++i) {
|
||||
crc = crc8_dvb_s2(crc, buf[i]);
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
uint8_t crsf_frame_CRC(const crsf_frame_t &frame)
|
||||
{
|
||||
// CRC includes type and payload
|
||||
|
||||
@@ -0,0 +1,377 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ghst.cpp
|
||||
*
|
||||
* RC protocol definition for IRC Ghost (Immersion RC Ghost).
|
||||
*
|
||||
* @author Igor Misic <igy1000mb@gmail.com>
|
||||
* @author Juraj Ciberlin <jciberlin1@gmail.com>
|
||||
*/
|
||||
|
||||
#if 0 // enable non-verbose debugging
|
||||
#define GHST_DEBUG PX4_WARN
|
||||
#else
|
||||
#define GHST_DEBUG(...)
|
||||
#endif
|
||||
|
||||
#if 0 // verbose debugging. Careful when enabling: it leads to too much output, causing dropped bytes
|
||||
#define GHST_VERBOSE PX4_WARN
|
||||
#else
|
||||
#define GHST_VERBOSE(...)
|
||||
#endif
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <termios.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
// TODO: include RSSI dBm to percentage conversion for ghost receiver
|
||||
#include "spektrum_rssi.h"
|
||||
|
||||
#include "ghst.h"
|
||||
#include "common_rc.h"
|
||||
|
||||
#define MIN(a,b) (((a)<(b))?(a):(b))
|
||||
#define MAX(a,b) (((a)>(b))?(a):(b))
|
||||
|
||||
#define GHST_FRAME_PAYLOAD_SIZE_TELEMETRY (10u)
|
||||
#define GHST_FRAME_CRC_SIZE (1)
|
||||
#define GHST_FRAME_TYPE_SIZE (1)
|
||||
#define GHST_TYPE_DATA_CRC_SIZE (12u)
|
||||
|
||||
enum class ghst_parser_state_t : uint8_t {
|
||||
unsynced = 0,
|
||||
synced
|
||||
};
|
||||
|
||||
// only RSSI frame contains value of RSSI, if it is not received, send last received RSSI
|
||||
static int8_t ghst_rssi = -1;
|
||||
|
||||
static ghst_frame_t &ghst_frame = rc_decode_buf.ghst_frame;
|
||||
static uint32_t current_frame_position = 0;
|
||||
static ghst_parser_state_t parser_state = ghst_parser_state_t::unsynced;
|
||||
|
||||
/**
|
||||
* parse the current ghst_frame buffer
|
||||
*/
|
||||
static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels);
|
||||
|
||||
int ghst_config(int uart_fd)
|
||||
{
|
||||
struct termios t;
|
||||
int ret_val;
|
||||
|
||||
// no parity, one stop bit
|
||||
tcgetattr(uart_fd, &t);
|
||||
cfsetspeed(&t, GHST_BAUDRATE);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
ret_val = tcsetattr(uart_fd, TCSANOW, &t);
|
||||
return ret_val;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert from RC to PWM value
|
||||
* @param chan_value channel value in [172, 1811]
|
||||
* @return PWM channel value in [1000, 2000]
|
||||
*/
|
||||
static uint16_t convert_channel_value(unsigned chan_value);
|
||||
|
||||
|
||||
bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values,
|
||||
int8_t *rssi, uint16_t *num_values, uint16_t max_channels)
|
||||
{
|
||||
bool success = false;
|
||||
uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame;
|
||||
|
||||
while (len > 0) {
|
||||
|
||||
// fill in the ghst_buffer, as much as we can
|
||||
const unsigned current_len = MIN(len, sizeof(ghst_frame_t) - current_frame_position);
|
||||
memcpy(ghst_frame_ptr + current_frame_position, frame, current_len);
|
||||
current_frame_position += current_len;
|
||||
|
||||
// protection to guarantee parsing progress
|
||||
if (current_len == 0) {
|
||||
GHST_DEBUG("========== parser bug: no progress (%i) ===========", len);
|
||||
|
||||
for (unsigned i = 0; i < current_frame_position; ++i) {
|
||||
GHST_DEBUG("ghst_frame_ptr[%i]: 0x%x", i, (int)ghst_frame_ptr[i]);
|
||||
}
|
||||
|
||||
// reset the parser
|
||||
current_frame_position = 0;
|
||||
parser_state = ghst_parser_state_t::unsynced;
|
||||
return false;
|
||||
}
|
||||
|
||||
len -= current_len;
|
||||
frame += current_len;
|
||||
|
||||
if (ghst_parse_buffer(values, rssi, num_values, max_channels)) {
|
||||
success = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
uint8_t ghst_frame_CRC(const ghst_frame_t &frame)
|
||||
{
|
||||
uint8_t crc = crc8_dvb_s2(0, frame.type);
|
||||
|
||||
for (int i = 0; i < frame.header.length - GHST_FRAME_CRC_SIZE - GHST_FRAME_TYPE_SIZE; ++i) {
|
||||
crc = crc8_dvb_s2(crc, frame.payload[i]);
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
static uint16_t convert_channel_value(unsigned chan_value)
|
||||
{
|
||||
/*
|
||||
* RC PWM
|
||||
* min 172 -> 988us
|
||||
* mid 992 -> 1500us
|
||||
* max 1811 -> 2012us
|
||||
*/
|
||||
static constexpr float scale = (2012.f - 988.f) / (1811.f - 172.f);
|
||||
static constexpr float offset = 988.f - 172.f * scale;
|
||||
return (scale * chan_value) + offset;
|
||||
}
|
||||
|
||||
static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels)
|
||||
{
|
||||
uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame;
|
||||
|
||||
if (parser_state == ghst_parser_state_t::unsynced) {
|
||||
// there is no sync yet, try to find an RC packet by searching for a matching frame length and type
|
||||
for (unsigned i = 1; i < current_frame_position - 1; ++i) {
|
||||
if ((ghst_frame_ptr[i + 1] >= (uint8_t)ghstFrameType::frameTypeFirst) &&
|
||||
(ghst_frame_ptr[i + 1] <= (uint8_t)ghstFrameType::frameTypeLast)) {
|
||||
if (ghst_frame_ptr[i] == GHST_TYPE_DATA_CRC_SIZE) {
|
||||
parser_state = ghst_parser_state_t::synced;
|
||||
unsigned frame_offset = i - 1;
|
||||
GHST_VERBOSE("RC channels found at offset %i", frame_offset);
|
||||
|
||||
// move the rest of the buffer to the beginning
|
||||
if (frame_offset != 0) {
|
||||
memmove(ghst_frame_ptr, ghst_frame_ptr + frame_offset, current_frame_position - frame_offset);
|
||||
current_frame_position -= frame_offset;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (parser_state != ghst_parser_state_t::synced) {
|
||||
if (current_frame_position >= sizeof(ghst_frame_t)) {
|
||||
// discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start)
|
||||
current_frame_position = 3;
|
||||
|
||||
for (unsigned i = 0; i < current_frame_position; ++i) {
|
||||
ghst_frame_ptr[i] = ghst_frame_ptr[sizeof(ghst_frame_t) - current_frame_position + i];
|
||||
}
|
||||
|
||||
GHST_VERBOSE("Discarding buffer");
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
if (current_frame_position < 3) {
|
||||
// wait until we have the address, length and type
|
||||
return false;
|
||||
}
|
||||
|
||||
// now we have at least the header and the type
|
||||
|
||||
const unsigned current_frame_length = ghst_frame.header.length + sizeof(ghst_frame_header_t);
|
||||
|
||||
if (current_frame_length > sizeof(ghst_frame_t) || current_frame_length < 4) {
|
||||
// frame too long or bogus (frame length should be longer than 4, at least 1 address, 1 length, 1 type, 1 data, 1 crc)
|
||||
// discard everything and go into unsynced state
|
||||
current_frame_position = 0;
|
||||
parser_state = ghst_parser_state_t::unsynced;
|
||||
GHST_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, ghst_frame.type);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (current_frame_position < current_frame_length) {
|
||||
// we do not have the full frame yet -> wait for more data
|
||||
GHST_VERBOSE("waiting for more data (%i < %i)", current_frame_position, current_frame_length);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ret = false;
|
||||
|
||||
// now we have the full frame
|
||||
|
||||
if ((ghst_frame.type >= (uint8_t)ghstFrameType::frameTypeFirst) &&
|
||||
(ghst_frame.type <= (uint8_t)ghstFrameType::frameTypeLast) &&
|
||||
(ghst_frame.header.length == GHST_TYPE_DATA_CRC_SIZE)) {
|
||||
const uint8_t crc = ghst_frame.payload[ghst_frame.header.length - 2];
|
||||
|
||||
if (crc == ghst_frame_CRC(ghst_frame)) {
|
||||
const ghstPayloadData_t *const rcChannels = (ghstPayloadData_t *)&ghst_frame.payload;
|
||||
*num_values = MIN(max_channels, 16);
|
||||
|
||||
// all frames contain data from chan1to4
|
||||
if (max_channels > 0) { values[0] = convert_channel_value(rcChannels->chan1to4.chan1 >> 1); }
|
||||
|
||||
if (max_channels > 1) { values[1] = convert_channel_value(rcChannels->chan1to4.chan2 >> 1); }
|
||||
|
||||
if (max_channels > 2) { values[2] = convert_channel_value(rcChannels->chan1to4.chan3 >> 1); }
|
||||
|
||||
if (max_channels > 3) { values[3] = convert_channel_value(rcChannels->chan1to4.chan4 >> 1); }
|
||||
|
||||
if (ghst_frame.type == (uint8_t)ghstFrameType::frameType5to8) {
|
||||
if (max_channels > 4) { values[4] = convert_channel_value(rcChannels->chanA << 3); }
|
||||
|
||||
if (max_channels > 5) { values[5] = convert_channel_value(rcChannels->chanB << 3); }
|
||||
|
||||
if (max_channels > 6) { values[6] = convert_channel_value(rcChannels->chanC << 3); }
|
||||
|
||||
if (max_channels > 7) { values[7] = convert_channel_value(rcChannels->chanD << 3); }
|
||||
|
||||
} else if (ghst_frame.type == (uint8_t)ghstFrameType::frameType9to12) {
|
||||
if (max_channels > 8) { values[8] = convert_channel_value(rcChannels->chanA << 3); }
|
||||
|
||||
if (max_channels > 9) { values[9] = convert_channel_value(rcChannels->chanB << 3); }
|
||||
|
||||
if (max_channels > 10) { values[10] = convert_channel_value(rcChannels->chanC << 3); }
|
||||
|
||||
if (max_channels > 11) { values[11] = convert_channel_value(rcChannels->chanD << 3); }
|
||||
|
||||
} else if (ghst_frame.type == (uint8_t)ghstFrameType::frameType13to16) {
|
||||
if (max_channels > 12) { values[12] = convert_channel_value(rcChannels->chanA << 3); }
|
||||
|
||||
if (max_channels > 13) { values[13] = convert_channel_value(rcChannels->chanB << 3); }
|
||||
|
||||
if (max_channels > 14) { values[14] = convert_channel_value(rcChannels->chanC << 3); }
|
||||
|
||||
if (max_channels > 15) { values[15] = convert_channel_value(rcChannels->chanD << 3); }
|
||||
|
||||
} else if (ghst_frame.type == (uint8_t)ghstFrameType::frameTypeRssi) {
|
||||
const ghstPayloadRssi_t *const rssiValues = (ghstPayloadRssi_t *)&ghst_frame.payload;
|
||||
// TODO: call function for RSSI dBm to percentage conversion for ghost receiver
|
||||
ghst_rssi = spek_dbm_to_percent(rssiValues->rssidBm);
|
||||
}
|
||||
|
||||
else {
|
||||
GHST_DEBUG("Frame type: %i", ghst_frame.type);
|
||||
}
|
||||
|
||||
*rssi = ghst_rssi;
|
||||
|
||||
GHST_VERBOSE("Got Channels");
|
||||
|
||||
ret = true;
|
||||
|
||||
} else {
|
||||
GHST_DEBUG("CRC check failed");
|
||||
}
|
||||
|
||||
} else {
|
||||
GHST_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, ghst_frame.type);
|
||||
}
|
||||
|
||||
// either reset or move the rest of the buffer
|
||||
if (current_frame_position > current_frame_length) {
|
||||
GHST_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length);
|
||||
memmove(ghst_frame_ptr, ghst_frame_ptr + current_frame_length, current_frame_position - current_frame_length);
|
||||
current_frame_position -= current_frame_length;
|
||||
|
||||
} else {
|
||||
current_frame_position = 0;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* write an uint8_t value to a buffer at a given offset and increment the offset
|
||||
*/
|
||||
static inline void write_uint8_t(uint8_t *buf, int &offset, uint8_t value)
|
||||
{
|
||||
buf[offset++] = value;
|
||||
}
|
||||
|
||||
/**
|
||||
* write an uint16_t value to a buffer at a given offset and increment the offset
|
||||
*/
|
||||
static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value)
|
||||
{
|
||||
buf[offset] = value & 0xff;
|
||||
buf[offset + 1] = value >> 8;
|
||||
offset += 2;
|
||||
}
|
||||
|
||||
/**
|
||||
* write frame header
|
||||
*/
|
||||
static inline void write_frame_header(uint8_t *buf, int &offset, ghstTelemetryType type, uint8_t payload_size)
|
||||
{
|
||||
write_uint8_t(buf, offset, (uint8_t)ghstAddress::rxAddress);
|
||||
write_uint8_t(buf, offset, payload_size + GHST_FRAME_CRC_SIZE + GHST_FRAME_TYPE_SIZE);
|
||||
write_uint8_t(buf, offset, (uint8_t)type);
|
||||
}
|
||||
|
||||
/**
|
||||
* write frame CRC
|
||||
*/
|
||||
static inline void write_frame_crc(uint8_t *buf, int &offset, int buf_size)
|
||||
{
|
||||
write_uint8_t(buf, offset, crc8_dvb_s2_buf(buf + 2, buf_size - 3));
|
||||
}
|
||||
|
||||
bool ghst_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, uint16_t fuel)
|
||||
{
|
||||
uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4u]; // address, frame length, type, crc
|
||||
int offset = 0;
|
||||
write_frame_header(buf, offset, ghstTelemetryType::batteryPack, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY);
|
||||
write_uint16_t(buf, offset, voltage);
|
||||
write_uint16_t(buf, offset, current);
|
||||
write_uint16_t(buf, offset, fuel);
|
||||
write_uint8_t(buf, offset, 0x00); // empty
|
||||
write_uint8_t(buf, offset, 0x00); // empty
|
||||
write_uint8_t(buf, offset, 0x00); // empty
|
||||
write_uint8_t(buf, offset, 0x00); // empty
|
||||
write_frame_crc(buf, offset, sizeof(buf));
|
||||
return write(uart_fd, buf, offset) == offset;
|
||||
}
|
||||
@@ -0,0 +1,141 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ghst.h
|
||||
*
|
||||
* RC protocol definition for IRC Ghost (Immersion RC Ghost).
|
||||
*
|
||||
* @author Igor Misic <igy1000mb@gmail.com>
|
||||
* @author Juraj Ciberlin <jciberlin1@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
#define GHST_BAUDRATE (420000u)
|
||||
#define GHST_PAYLOAD_MAX_SIZE (14u)
|
||||
|
||||
enum class ghstAddress {
|
||||
rxAddress = 0x89 // Rx address
|
||||
};
|
||||
|
||||
enum class ghstFrameType {
|
||||
frameTypeFirst = 0x10, // first frame type
|
||||
frameType5to8 = 0x10, // channel 5-8
|
||||
frameType9to12 = 0x11, // channel 9-12
|
||||
frameType13to16 = 0x12, // channel 13-16
|
||||
frameTypeRssi = 0x13, // RSSI frame type, contains LQ, RSSI, RF mode, Tx power
|
||||
frameTypeLast = 0x1f // last frame type
|
||||
};
|
||||
|
||||
enum class ghstTelemetryType {
|
||||
batteryPack = 0x23 // battery pack status frame type
|
||||
};
|
||||
|
||||
struct ghst_frame_header_t {
|
||||
uint8_t device_address; // device address
|
||||
uint8_t length; // length
|
||||
};
|
||||
|
||||
struct ghst_frame_t {
|
||||
ghst_frame_header_t header; // header
|
||||
uint8_t type; // frame type
|
||||
uint8_t payload[GHST_PAYLOAD_MAX_SIZE + 1]; // payload data including 1 byte CRC at the end
|
||||
};
|
||||
|
||||
// Channel data (1-4)
|
||||
typedef struct {
|
||||
unsigned int chan1: 12;
|
||||
unsigned int chan2: 12;
|
||||
unsigned int chan3: 12;
|
||||
unsigned int chan4: 12;
|
||||
} __attribute__((__packed__)) ghstChannelData_t;
|
||||
|
||||
// Payload data
|
||||
typedef struct {
|
||||
ghstChannelData_t chan1to4;
|
||||
unsigned int chanA: 8;
|
||||
unsigned int chanB: 8;
|
||||
unsigned int chanC: 8;
|
||||
unsigned int chanD: 8;
|
||||
} __attribute__((__packed__)) ghstPayloadData_t;
|
||||
|
||||
// Payload data - RSSI frame type
|
||||
typedef struct {
|
||||
ghstChannelData_t chan1to4;
|
||||
unsigned int lq: 8; // link quality
|
||||
unsigned int rssidBm: 8; // RSSI [dBm]
|
||||
unsigned int rfMode: 8; // RF mode
|
||||
int txPowerdBm: 8; // Tx power [dBm]
|
||||
} __attribute__((__packed__)) ghstPayloadRssi_t;
|
||||
|
||||
/**
|
||||
* Configure an UART port to be used for GHST
|
||||
* @param uart_fd UART file descriptor
|
||||
* @return 0 on success, -errno otherwise
|
||||
*/
|
||||
__EXPORT int ghst_config(int uart_fd);
|
||||
|
||||
|
||||
/**
|
||||
* Parse the GHST protocol and extract RC channel data.
|
||||
*
|
||||
* @param now current time
|
||||
* @param frame data to parse
|
||||
* @param len length of frame
|
||||
* @param values output channel values
|
||||
* @param rssi received signal strength indicator
|
||||
* @param num_values set to the number of parsed channels in values
|
||||
* @param max_channels maximum length of values
|
||||
* @return true if channels successfully decoded
|
||||
*/
|
||||
__EXPORT bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values,
|
||||
int8_t *rssi, uint16_t *num_values, uint16_t max_channels);
|
||||
|
||||
|
||||
/**
|
||||
* Send telemetry battery information
|
||||
* @param uart_fd UART file descriptor
|
||||
* @param voltage Voltage [0.1V]
|
||||
* @param current Current [0.1A]
|
||||
* @param fuel drawn mAh
|
||||
* @return true on success
|
||||
*/
|
||||
__EXPORT bool ghst_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, uint16_t fuel);
|
||||
|
||||
__END_DECLS
|
||||
Reference in New Issue
Block a user