dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Ghost protocol 

ghst: add ghost protocol
This commit is contained in:
jciberlin 2021-02-14 17:59:22 +01:00 committed by Lorenz Meier
parent 711a69854b
commit 64342f568d
12 changed files with 781 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/input_rc.msg
View File

@ -15,6 +15,7 @@ uint8 RC_INPUT_SOURCE_PX4FMU_SUMD = 11
uint8 RC_INPUT_SOURCE_PX4FMU_DSM = 12
uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 13
uint8 RC_INPUT_SOURCE_PX4FMU_CRSF = 14
uint8 RC_INPUT_SOURCE_PX4FMU_GHST = 15
uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.

1
src/drivers/rc_input/CMakeLists.txt
View File

@ -37,6 +37,7 @@ px4_add_module(
SRCS
RCInput.cpp
crsf_telemetry.cpp
ghst_telemetry.cpp
MODULE_CONFIG
module.yaml
DEPENDS

52
src/drivers/rc_input/RCInput.cpp
View File

@ -72,6 +72,7 @@ RCInput::~RCInput()
dsm_deinit();
delete _crsf_telemetry;
delete _ghst_telemetry;
perf_free(_cycle_perf);
perf_free(_publish_interval_perf);
@ -625,6 +626,53 @@ void RCInput::Run()
}
}
} else {
// Scan the next protocol
set_rc_scan_state(RC_SCAN_GHST);
}
break;
case RC_SCAN_GHST:
if (_rc_scan_begin == 0) {
_rc_scan_begin = cycle_timestamp;
// Configure serial port for GHST
ghst_config(_rcs_fd);
rc_io_invert(false);
} else if (_rc_scan_locked
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
// parse new data
if (newBytes > 0) {
int8_t ghst_rssi = -1;
rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
if (rc_updated) {
// we have a new GHST frame. Publish it.
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
// Enable GHST Telemetry only on the Omnibus, because on Pixhawk (-related) boards
// we cannot write to the RC UART
// It might work on FMU-v5. Or another option is to use a different UART port
#ifdef CONFIG_ARCH_BOARD_OMNIBUS_F4SD
if (!_rc_scan_locked && !_ghst_telemetry) {
_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
}
#endif /* CONFIG_ARCH_BOARD_OMNIBUS_F4SD */
_rc_scan_locked = true;
if (_ghst_telemetry) {
_ghst_telemetry->update(cycle_timestamp);
}
}
}
} else {
// Scan the next protocol
set_rc_scan_state(RC_SCAN_SBUS);
@ -732,6 +780,10 @@ int RCInput::print_status()
PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no");
break;
case RC_SCAN_GHST:
PX4_INFO("GHST Telemetry: %s", _ghst_telemetry ? "yes" : "no");
break;
case RC_SCAN_SBUS:
PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
break;

11
src/drivers/rc_input/RCInput.hpp
View File

@ -41,6 +41,7 @@
#include <drivers/drv_rc_input.h>
#include <lib/perf/perf_counter.h>
#include <lib/rc/crsf.h>
#include <lib/rc/ghst.h>
#include <lib/rc/dsm.h>
#include <lib/rc/sbus.h>
#include <lib/rc/st24.h>
@ -60,6 +61,7 @@
#include <uORB/topics/vehicle_command.h>
#include "crsf_telemetry.h"
#include "ghst_telemetry.h"
#ifdef HRT_PPM_CHANNEL
# include <systemlib/ppm_decode.h>
@ -94,16 +96,18 @@ private:
RC_SCAN_DSM,
RC_SCAN_SUMD,
RC_SCAN_ST24,
RC_SCAN_CRSF
RC_SCAN_CRSF,
RC_SCAN_GHST
} _rc_scan_state{RC_SCAN_SBUS};
static constexpr char const *RC_SCAN_STRING[6] {
static constexpr char const *RC_SCAN_STRING[7] {
"PPM",
"SBUS",
"DSM",
"SUMD",
"ST24",
"CRSF"
"CRSF",
"GHST"
};
void Run() override;
@ -159,6 +163,7 @@ private:
uint16_t _raw_rc_count{};
CRSFTelemetry *_crsf_telemetry{nullptr};
GHSTTelemetry *_ghst_telemetry{nullptr};
perf_counter_t _cycle_perf;
perf_counter_t _publish_interval_perf;

85
src/drivers/rc_input/ghst_telemetry.cpp Normal file
View File

@ -0,0 +1,85 @@
/****************************************************************************
*
* 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_telemetry.cpp
*
* IRC Ghost (Immersion RC Ghost) telemetry.
*
* @author Igor Misic <igy1000mb@gmail.com>
* @author Juraj Ciberlin <jciberlin1@gmail.com>
*/
#include "ghst_telemetry.h"
#include <lib/rc/ghst.h>
GHSTTelemetry::GHSTTelemetry(int uart_fd) :
_uart_fd(uart_fd)
{
}
bool GHSTTelemetry::update(const hrt_abstime &now)
{
const int update_rate_hz = 10;
if (now - _last_update <= 1_s / (update_rate_hz * num_data_types)) {
return false;
}
bool sent = false;
switch (_next_type) {
case 0:
sent = send_battery_status();
break;
}
_last_update = now;
_next_type = (_next_type + 1) % num_data_types;
return sent;
}
bool GHSTTelemetry::send_battery_status()
{
battery_status_s battery_status;
if (!_battery_status_sub.update(&battery_status)) {
return false;
}
uint16_t voltage = battery_status.voltage_filtered_v * 10;
uint16_t current = battery_status.current_filtered_a * 10;
uint16_t fuel = battery_status.discharged_mah;
return ghst_send_telemetry_battery(_uart_fd, voltage, current, fuel);
}

83
src/drivers/rc_input/ghst_telemetry.h Normal file
View File

@ -0,0 +1,83 @@
/****************************************************************************
*
* 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_telemetry.cpp
*
* IRC Ghost (Immersion RC Ghost) telemetry.
*
* @author Igor Misic <igy1000mb@gmail.com>
* @author Juraj Ciberlin <jciberlin1@gmail.com>
*/
#pragma once
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <drivers/drv_hrt.h>
using namespace time_literals;
/**
* High-level class that handles sending of GHST telemetry data
*/
class GHSTTelemetry
{
public:
/**
* @param uart_fd file descriptor for the UART to use. It is expected to be configured
* already.
*/
GHSTTelemetry(int uart_fd);
~GHSTTelemetry() = default;
/**
* Send telemetry data. Call this regularly (i.e. at 100Hz), it will automatically
* limit the sending rate.
* @return true if new data sent
*/
bool update(const hrt_abstime &now);
private:
bool send_battery_status();
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
hrt_abstime _last_update{0};
static constexpr int num_data_types{1}; // number of different telemetry data types
int _next_type{0};
int _uart_fd;
};

1
src/lib/rc/CMakeLists.txt
View File

@ -33,6 +33,7 @@
add_library(rc
crsf.cpp
ghst.cpp
st24.cpp
sumd.cpp
sbus.cpp

27
src/lib/rc/common_rc.cpp
View File

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

5
src/lib/rc/common_rc.h
View File

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

29
src/lib/rc/crsf.cpp
View File

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

377
src/lib/rc/ghst.cpp Normal file
View File

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

141
src/lib/rc/ghst.h Normal file
View File

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