jciberlin
4 years ago
committed by
Lorenz Meier
12 changed files with 781 additions and 32 deletions
@ -0,0 +1,85 @@
@@ -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); |
||||
} |
||||
@ -0,0 +1,83 @@
@@ -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; |
||||
}; |
||||
@ -0,0 +1,377 @@
@@ -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 @@
@@ -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 |
||||
Loading…
Reference in new issue