diff --git a/msg/input_rc.msg b/msg/input_rc.msg index 8857e4e6b8..8f333e7f66 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -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. diff --git a/src/drivers/rc_input/CMakeLists.txt b/src/drivers/rc_input/CMakeLists.txt index 1ad8262481..6c58fb2605 100644 --- a/src/drivers/rc_input/CMakeLists.txt +++ b/src/drivers/rc_input/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( SRCS RCInput.cpp crsf_telemetry.cpp + ghst_telemetry.cpp MODULE_CONFIG module.yaml DEPENDS diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 1e37018c21..8fcabd9f8a 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -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; diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index 9d17b2abd8..f965aa764b 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -60,6 +61,7 @@ #include #include "crsf_telemetry.h" +#include "ghst_telemetry.h" #ifdef HRT_PPM_CHANNEL # include @@ -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; diff --git a/src/drivers/rc_input/ghst_telemetry.cpp b/src/drivers/rc_input/ghst_telemetry.cpp new file mode 100644 index 0000000000..ab9da12698 --- /dev/null +++ b/src/drivers/rc_input/ghst_telemetry.cpp @@ -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 + * @author Juraj Ciberlin + */ + +#include "ghst_telemetry.h" +#include + +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); +} diff --git a/src/drivers/rc_input/ghst_telemetry.h b/src/drivers/rc_input/ghst_telemetry.h new file mode 100644 index 0000000000..0bbba0e0d5 --- /dev/null +++ b/src/drivers/rc_input/ghst_telemetry.h @@ -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 + * @author Juraj Ciberlin + */ + +#pragma once + +#include +#include +#include + +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; +}; diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt index d4162502b2..bcba0a41a0 100644 --- a/src/lib/rc/CMakeLists.txt +++ b/src/lib/rc/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(rc crsf.cpp + ghst.cpp st24.cpp sumd.cpp sbus.cpp diff --git a/src/lib/rc/common_rc.cpp b/src/lib/rc/common_rc.cpp index 2a271c8d0e..6cda37b9c3 100644 --- a/src/lib/rc/common_rc.cpp +++ b/src/lib/rc/common_rc.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; +} diff --git a/src/lib/rc/common_rc.h b/src/lib/rc/common_rc.h index 85e55b6eab..54b7f0046e 100644 --- a/src/lib/rc/common_rc.h +++ b/src/lib/rc/common_rc.h @@ -4,6 +4,7 @@ #include #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); diff --git a/src/lib/rc/crsf.cpp b/src/lib/rc/crsf.cpp index 85831421ba..383a0fbe71 100644 --- a/src/lib/rc/crsf.cpp +++ b/src/lib/rc/crsf.cpp @@ -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 diff --git a/src/lib/rc/ghst.cpp b/src/lib/rc/ghst.cpp new file mode 100644 index 0000000000..c8b2febc82 --- /dev/null +++ b/src/lib/rc/ghst.cpp @@ -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 + * @author Juraj Ciberlin + */ + +#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 +#include +#include +#include + +// 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; +} diff --git a/src/lib/rc/ghst.h b/src/lib/rc/ghst.h new file mode 100644 index 0000000000..c1a7a0c0a0 --- /dev/null +++ b/src/lib/rc/ghst.h @@ -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 + * @author Juraj Ciberlin + */ + +#pragma once + +#include +#include + +__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