From 77fdce9f3cab39f56a8026d2c061fd032c2eb08d Mon Sep 17 00:00:00 2001 From: chris1seto Date: Fri, 23 Sep 2022 16:19:25 -0700 Subject: [PATCH] drivers/rc/crsf_rc: update standalone driver for CRSF and direct support for lq/rssi (#20084) Co-authored-by: Chris Seto --- .../crazyflie/syslink/syslink_main.cpp | 3 + .../navio_sysfs_rc_in/NavioSysRCInput.cpp | 3 + msg/input_rc.msg | 3 + src/drivers/px4io/px4io.cpp | 3 + src/drivers/rc/crsf_rc/CMakeLists.txt | 8 +- src/drivers/rc/crsf_rc/Crc8.cpp | 29 + src/drivers/rc/crsf_rc/Crc8.hpp | 7 + src/drivers/rc/crsf_rc/CrsfParser.cpp | 352 ++++++++++++ src/drivers/rc/crsf_rc/CrsfParser.hpp | 91 +++ src/drivers/rc/crsf_rc/CrsfRc.cpp | 521 ++++++++++++------ src/drivers/rc/crsf_rc/CrsfRc.hpp | 49 +- src/drivers/rc/crsf_rc/QueueBuffer.cpp | 163 ++++++ src/drivers/rc/crsf_rc/QueueBuffer.hpp | 63 +++ src/drivers/rc/crsf_rc/crsf.cpp | 507 ----------------- src/drivers/rc/crsf_rc/crsf.h | 112 ---- src/drivers/rc_input/RCInput.cpp | 3 + src/drivers/rpi_rc_in/rpi_rc_in.cpp | 2 + .../snapdragon_spektrum_rc/spektrum_rc.cpp | 3 + src/modules/mavlink/mavlink_receiver.cpp | 3 + .../simulator_mavlink/SimulatorMavlink.cpp | 3 + 20 files changed, 1135 insertions(+), 793 deletions(-) create mode 100644 src/drivers/rc/crsf_rc/Crc8.cpp create mode 100644 src/drivers/rc/crsf_rc/Crc8.hpp create mode 100644 src/drivers/rc/crsf_rc/CrsfParser.cpp create mode 100644 src/drivers/rc/crsf_rc/CrsfParser.hpp create mode 100644 src/drivers/rc/crsf_rc/QueueBuffer.cpp create mode 100644 src/drivers/rc/crsf_rc/QueueBuffer.hpp delete mode 100644 src/drivers/rc/crsf_rc/crsf.cpp delete mode 100644 src/drivers/rc/crsf_rc/crsf.h diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index d2faf8d2d9..909d84175d 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -548,6 +548,9 @@ Syslink::handle_raw(syslink_message_t *sys) rc.values[3] = cmd->thrust * 1000 / USHRT_MAX + 1000; rc.values[4] = 1000; // Dummy channel as px4 needs at least 5 + rc.link_quality = -1; + rc.rssi_dbm = NAN; + _rc_pub.publish(rc); } else if (c->port == CRTP_PORT_MAVLINK) { diff --git a/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp index be17b084b3..3c8691d811 100644 --- a/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp +++ b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp @@ -152,6 +152,9 @@ void NavioSysRCInput::Run() data.timestamp_last_signal = timestamp_sample; data.channel_count = CHANNELS; data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; + data.link_quality = -1; + data.rssi_dbm = NAN; + data.timestamp = hrt_absolute_time(); _input_rc_pub.publish(data); diff --git a/msg/input_rc.msg b/msg/input_rc.msg index 68addbfdf1..db4b3de233 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -35,3 +35,6 @@ uint16 rc_ppm_frame_length # Length of a single PPM frame. Zero for non-PPM sys uint8 input_source # Input source uint16[18] values # measured pulse widths for each of the supported channels + +int8 link_quality # link quality. Percentage 0-100%. -1 = invalid +float32 rssi_dbm # Actual rssi in units of dBm. NaN = invalid \ No newline at end of file diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index fba6a2bd67..2036f2c2d0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1144,6 +1144,9 @@ int PX4IO::io_publish_raw_rc() if (input_rc.input_source != input_rc_s::RC_INPUT_SOURCE_UNKNOWN) { + input_rc.link_quality = -1; + input_rc.rssi_dbm = NAN; + _to_input_rc.publish(input_rc); } diff --git a/src/drivers/rc/crsf_rc/CMakeLists.txt b/src/drivers/rc/crsf_rc/CMakeLists.txt index aaa85e7c54..ceb7531079 100644 --- a/src/drivers/rc/crsf_rc/CMakeLists.txt +++ b/src/drivers/rc/crsf_rc/CMakeLists.txt @@ -35,10 +35,14 @@ px4_add_module( MAIN crsf_rc COMPILE_FLAGS SRCS - crsf.cpp - crsf.h CrsfRc.cpp CrsfRc.hpp + QueueBuffer.cpp + QueueBuffer.hpp + CrsfParser.cpp + CrsfParser.hpp + Crc8.hpp + Crc8.cpp MODULE_CONFIG module.yaml diff --git a/src/drivers/rc/crsf_rc/Crc8.cpp b/src/drivers/rc/crsf_rc/Crc8.cpp new file mode 100644 index 0000000000..e912409fa4 --- /dev/null +++ b/src/drivers/rc/crsf_rc/Crc8.cpp @@ -0,0 +1,29 @@ +#include +#include +#include "Crc8.hpp" + +static uint8_t crc8_lut[256]; + +void Crc8Init(const uint8_t poly) +{ + for (int idx = 0; idx < 256; ++idx) { + uint8_t crc = idx; + + for (int shift = 0; shift < 8; ++shift) { + crc = (crc << 1) ^ ((crc & 0x80) ? poly : 0); + } + + crc8_lut[idx] = crc & 0xff; + } +} + +uint8_t Crc8Calc(const uint8_t *data, uint8_t size) +{ + uint8_t crc = 0; + + while (size--) { + crc = crc8_lut[crc ^ *data++]; + } + + return crc; +} \ No newline at end of file diff --git a/src/drivers/rc/crsf_rc/Crc8.hpp b/src/drivers/rc/crsf_rc/Crc8.hpp new file mode 100644 index 0000000000..fa8a8884b9 --- /dev/null +++ b/src/drivers/rc/crsf_rc/Crc8.hpp @@ -0,0 +1,7 @@ +#pragma once + +#include +#include + +void Crc8Init(const uint8_t poly); +uint8_t Crc8Calc(const uint8_t *data, uint8_t size); diff --git a/src/drivers/rc/crsf_rc/CrsfParser.cpp b/src/drivers/rc/crsf_rc/CrsfParser.cpp new file mode 100644 index 0000000000..db086d6d9c --- /dev/null +++ b/src/drivers/rc/crsf_rc/CrsfParser.cpp @@ -0,0 +1,352 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 CrsfParser.cpp +* +* Parser for incoming CRSF packets +* +* @author Chris Seto +*/ + +#include +#include +#include +#include +#include "QueueBuffer.hpp" +#include "CrsfParser.hpp" +#include "Crc8.hpp" + +#define CRSF_CHANNEL_VALUE_MIN 172 +#define CRSF_CHANNEL_VALUE_MAX 1811 +#define CRSF_CHANNEL_VALUE_SPAN (CRSF_CHANNEL_VALUE_MAX - CRSF_CHANNEL_VALUE_MIN) +#define CRSF_MAX_PACKET_LEN 64 +#define CRSF_HEADER 0xc8 + +enum CRSF_PAYLOAD_SIZE { + CRSF_PAYLOAD_SIZE_GPS = 15, + CRSF_PAYLOAD_SIZE_BATTERY = 8, + CRSF_PAYLOAD_SIZE_LINK_STATISTICS = 10, + CRSF_PAYLOAD_SIZE_RC_CHANNELS = 22, + CRSF_PAYLOAD_SIZE_ATTITUDE = 6, +}; + +enum CRSF_PACKET_TYPE { + CRSF_PACKET_TYPE_GPS = 0x02, + CRSF_PACKET_TYPE_BATTERY_SENSOR = 0x08, + CRSF_PACKET_TYPE_LINK_STATISTICS = 0x14, + CRSF_PACKET_TYPE_OPENTX_SYNC = 0x10, + CRSF_PACKET_TYPE_RADIO_ID = 0x3A, + CRSF_PACKET_TYPE_RC_CHANNELS_PACKED = 0x16, + CRSF_PACKET_TYPE_ATTITUDE = 0x1E, + CRSF_PACKET_TYPE_FLIGHT_MODE = 0x21, + // Extended Header Frames, range: 0x28 to 0x96 + CRSF_PACKET_TYPE_DEVICE_PING = 0x28, + CRSF_PACKET_TYPE_DEVICE_INFO = 0x29, + CRSF_PACKET_TYPE_PARAMETER_SETTINGS_ENTRY = 0x2B, + CRSF_PACKET_TYPE_PARAMETER_READ = 0x2C, + CRSF_PACKET_TYPE_PARAMETER_WRITE = 0x2D, + CRSF_PACKET_TYPE_COMMAND = 0x32, + // MSP commands + CRSF_PACKET_TYPE_MSP_REQ = 0x7A, // response request using msp sequence as command + CRSF_PACKET_TYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary + CRSF_PACKET_TYPE_MSP_WRITE = 0x7C, // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit) +}; + +enum CRSF_ADDRESS { + CRSF_ADDRESS_BROADCAST = 0x00, + CRSF_ADDRESS_USB = 0x10, + CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80, + CRSF_ADDRESS_RESERVED1 = 0x8A, + CRSF_ADDRESS_CURRENT_SENSOR = 0xC0, + CRSF_ADDRESS_GPS = 0xC2, + CRSF_ADDRESS_TBS_BLACKBOX = 0xC4, + CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8, + CRSF_ADDRESS_RESERVED2 = 0xCA, + CRSF_ADDRESS_RACE_TAG = 0xCC, + CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA, + CRSF_ADDRESS_CRSF_RECEIVER = 0xEC, + CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE, +}; + +#define HEADER_SIZE 1 +#define PACKET_SIZE_SIZE 1 +#define PACKET_TYPE_SIZE 1 +#define PACKET_SIZE_TYPE_SIZE 2 +#define CRC_SIZE 1 + +enum PARSER_STATE { + PARSER_STATE_HEADER, + PARSER_STATE_SIZE_TYPE, + PARSER_STATE_PAYLOAD, + PARSER_STATE_CRC, +}; + +typedef struct { + uint8_t packet_type; + uint32_t packet_size; + bool (*processor)(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet); +} CrsfPacketDescriptor_t; + +static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet); +static bool ProcessLinkStatistics(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet); + +#define CRSF_PACKET_DESCRIPTOR_COUNT 2 +static const CrsfPacketDescriptor_t crsf_packet_descriptors[CRSF_PACKET_DESCRIPTOR_COUNT] = { + {CRSF_PACKET_TYPE_RC_CHANNELS_PACKED, CRSF_PAYLOAD_SIZE_RC_CHANNELS, ProcessChannelData}, + {CRSF_PACKET_TYPE_LINK_STATISTICS, CRSF_PAYLOAD_SIZE_LINK_STATISTICS, ProcessLinkStatistics}, +}; + +static enum PARSER_STATE parser_state = PARSER_STATE_HEADER; +static uint32_t working_index = 0; +static uint32_t working_segment_size = HEADER_SIZE; + +#define RX_QUEUE_BUFFER_SIZE 200 +static QueueBuffer_t rx_queue; +static uint8_t rx_queue_buffer[RX_QUEUE_BUFFER_SIZE]; +static uint8_t process_buffer[CRSF_MAX_PACKET_LEN]; +static CrsfPacketDescriptor_t *working_descriptor = NULL; + +static CrsfPacketDescriptor_t *FindCrsfDescriptor(const enum CRSF_PACKET_TYPE packet_type); + +void CrsfParser_Init(void) +{ + QueueBuffer_Init(&rx_queue, rx_queue_buffer, RX_QUEUE_BUFFER_SIZE); +} + +static float ConstrainF(const float x, const float min, const float max) +{ + if (x < min) { + return min; + + } else if (x > max) { + return max; + } + + return x; +} + +static float MapF(const float x, const float in_min, const float in_max, const float out_min, const float out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet) +{ + uint32_t raw_channels[CRSF_CHANNEL_COUNT]; + uint32_t i; + + new_packet->message_type = CRSF_MESSAGE_TYPE_RC_CHANNELS; + + // Decode channel data + raw_channels[0] = (data[0] | data[1] << 8) & 0x07FF; + raw_channels[1] = (data[1] >> 3 | data[2] << 5) & 0x07FF; + raw_channels[2] = (data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF; + raw_channels[3] = (data[4] >> 1 | data[5] << 7) & 0x07FF; + raw_channels[4] = (data[5] >> 4 | data[6] << 4) & 0x07FF; + raw_channels[5] = (data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF; + raw_channels[6] = (data[8] >> 2 | data[9] << 6) & 0x07FF; + raw_channels[7] = (data[9] >> 5 | data[10] << 3) & 0x07FF; + raw_channels[8] = (data[11] | data[12] << 8) & 0x07FF; + raw_channels[9] = (data[12] >> 3 | data[13] << 5) & 0x07FF; + raw_channels[10] = (data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF; + raw_channels[11] = (data[15] >> 1 | data[16] << 7) & 0x07FF; + raw_channels[12] = (data[16] >> 4 | data[17] << 4) & 0x07FF; + raw_channels[13] = (data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF; + raw_channels[14] = (data[19] >> 2 | data[20] << 6) & 0x07FF; + raw_channels[15] = (data[20] >> 5 | data[21] << 3) & 0x07FF; + + for (i = 0; i < CRSF_CHANNEL_COUNT; i++) { + raw_channels[i] = ConstrainF(raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX); + new_packet->channel_data.channels[i] = MapF((float)raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX, + 1000.0f, 2000.0f); + } + + return true; +} + +static bool ProcessLinkStatistics(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet) +{ + new_packet->message_type = CRSF_MESSAGE_TYPE_LINK_STATISTICS; + + new_packet->link_statistics.uplink_rssi_1 = data[0]; + new_packet->link_statistics.uplink_rssi_2 = data[1]; + new_packet->link_statistics.uplink_link_quality = data[2]; + new_packet->link_statistics.uplink_snr = data[3]; + new_packet->link_statistics.active_antenna = data[4]; + new_packet->link_statistics.rf_mode = data[5]; + new_packet->link_statistics.uplink_tx_power = data[6]; + new_packet->link_statistics.downlink_rssi = data[7]; + new_packet->link_statistics.downlink_link_quality = data[8]; + new_packet->link_statistics.downlink_snr = data[9]; + + return true; +} + +static CrsfPacketDescriptor_t *FindCrsfDescriptor(const enum CRSF_PACKET_TYPE packet_type) +{ + uint32_t i; + + for (i = 0; i < CRSF_PACKET_DESCRIPTOR_COUNT; i++) { + if (crsf_packet_descriptors[i].packet_type == packet_type) { + return (CrsfPacketDescriptor_t *)&crsf_packet_descriptors[i]; + } + } + + return NULL; +} + +bool CrsfParser_LoadBuffer(const uint8_t *buffer, const uint32_t size) +{ + return QueueBuffer_AppendBuffer(&rx_queue, buffer, size); +} + +uint32_t CrsfParser_FreeQueueSize(void) +{ + return RX_QUEUE_BUFFER_SIZE - QueueBuffer_Count(&rx_queue); +} + +// 0xC8 [packet len] [packet type] [data] [crc] +bool CrsfParser_TryParseCrsfPacket(CrsfPacket_t *const new_packet, CrsfParserStatistics_t *const parser_statistics) +{ + uint32_t buffer_count; + uint8_t working_byte; + uint8_t packet_size; + uint8_t packet_type; + bool valid_packet = false; + + buffer_count = QueueBuffer_Count(&rx_queue); + + // Iterate through the buffer to parse the message out + while ((working_index < buffer_count) && (buffer_count - working_index) >= working_segment_size) { + switch (parser_state) { + // Header + case PARSER_STATE_HEADER: + if (QueueBuffer_Get(&rx_queue, &working_byte)) { + if (working_byte == CRSF_HEADER) { + parser_state = PARSER_STATE_SIZE_TYPE; + working_segment_size = PACKET_SIZE_TYPE_SIZE; + working_index = 0; + buffer_count = QueueBuffer_Count(&rx_queue); + continue; + + } else { + parser_statistics->disposed_bytes++; + } + } + + working_index = 0; + working_segment_size = HEADER_SIZE; + break; + + // Packet size type + case PARSER_STATE_SIZE_TYPE: + QueueBuffer_Peek(&rx_queue, working_index++, &packet_size); + QueueBuffer_Peek(&rx_queue, working_index++, &packet_type); + + working_descriptor = FindCrsfDescriptor((enum CRSF_PACKET_TYPE)packet_type); + + // If we know what this packet is... + if (working_descriptor != NULL) { + // Validate length + if (packet_size != working_descriptor->packet_size + PACKET_SIZE_TYPE_SIZE) { + parser_statistics->invalid_known_packet_sizes++; + parser_state = PARSER_STATE_HEADER; + working_segment_size = HEADER_SIZE; + working_index = 0; + buffer_count = QueueBuffer_Count(&rx_queue); + continue; + } + + working_segment_size = working_descriptor->packet_size; + + } else { + // We don't know what this packet is, so we'll let the parser continue + // just so that we can dequeue it in one shot + working_segment_size = packet_size + PACKET_SIZE_TYPE_SIZE; + } + + parser_state = PARSER_STATE_PAYLOAD; + break; + + // Full packet content + case PARSER_STATE_PAYLOAD: + working_index += working_segment_size; + working_segment_size = CRC_SIZE; + parser_state = PARSER_STATE_CRC; + break; + + // CRC + case PARSER_STATE_CRC: + // Fetch the suspected packet as a contingous block of memory + QueueBuffer_PeekBuffer(&rx_queue, 0, process_buffer, working_index + CRC_SIZE); + + // Verify checksum + if (Crc8Calc(process_buffer + PACKET_SIZE_SIZE, working_index - PACKET_SIZE_SIZE) == process_buffer[working_index]) { + if (working_descriptor != NULL) { + if (working_descriptor->processor != NULL) { + if (working_descriptor->processor(process_buffer + PACKET_SIZE_TYPE_SIZE, working_index - PACKET_SIZE_TYPE_SIZE, + new_packet)) { + parser_statistics->crcs_valid_known_packets++; + valid_packet = true; + } + } + + } else { + // No working_descriptor at this point means unknown packet + parser_statistics->crcs_valid_unknown_packets++; + } + + // Remove the sucessfully processed data from the queue + QueueBuffer_Dequeue(&rx_queue, working_index + CRC_SIZE); + + } else { + parser_statistics->crcs_invalid++; + } + + working_index = 0; + working_segment_size = HEADER_SIZE; + parser_state = PARSER_STATE_HEADER; + + if (valid_packet) { + return true; + } + + break; + } + + buffer_count = QueueBuffer_Count(&rx_queue); + } + + return false; +} diff --git a/src/drivers/rc/crsf_rc/CrsfParser.hpp b/src/drivers/rc/crsf_rc/CrsfParser.hpp new file mode 100644 index 0000000000..7b36876b40 --- /dev/null +++ b/src/drivers/rc/crsf_rc/CrsfParser.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 CrsfParser.hpp +* +* Parser for incoming CRSF packets +* +* @author Chris Seto +*/ + +#pragma once + +#include +#include + +#define CRSF_CHANNEL_COUNT 16 + +struct CrsfChannelData_t { + float channels[CRSF_CHANNEL_COUNT]; +}; + +struct CrsfLinkStatistics_t { + uint8_t uplink_rssi_1; + uint8_t uplink_rssi_2; + uint8_t uplink_link_quality; + int8_t uplink_snr; + uint8_t active_antenna; + uint8_t rf_mode; + uint8_t uplink_tx_power; + uint8_t downlink_rssi; + uint8_t downlink_link_quality; + int8_t downlink_snr; +}; + +struct CrsfParserStatistics_t { + uint32_t disposed_bytes; + uint32_t crcs_valid_known_packets; + uint32_t crcs_valid_unknown_packets; + uint32_t crcs_invalid; + uint32_t invalid_known_packet_sizes; +}; + +enum CRSF_MESSAGE_TYPE { + CRSF_MESSAGE_TYPE_RC_CHANNELS, + CRSF_MESSAGE_TYPE_LINK_STATISTICS, +}; + +typedef struct { + CRSF_MESSAGE_TYPE message_type; + + union { + CrsfChannelData_t channel_data; + CrsfLinkStatistics_t link_statistics; + }; +} CrsfPacket_t; + +void CrsfParser_Init(void); +bool CrsfParser_LoadBuffer(const uint8_t *buffer, const uint32_t size); +uint32_t CrsfParser_FreeQueueSize(void); +bool CrsfParser_TryParseCrsfPacket(CrsfPacket_t *const new_packet, CrsfParserStatistics_t *const parser_statistics); diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index afaf544af8..f5489b35db 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -32,12 +32,21 @@ ****************************************************************************/ #include "CrsfRc.hpp" +#include "CrsfParser.hpp" +#include "Crc8.hpp" #include #include +#include +#include +#include +#include + using namespace time_literals; +#define CRSF_BAUDRATE 420000 + CrsfRc::CrsfRc(const char *device) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)) @@ -81,34 +90,27 @@ int CrsfRc::task_spawn(int argc, char *argv[]) } if (error_flag) { - return -1; + return PX4_ERROR; } - if (device_name && (access(device_name, R_OK | W_OK) == 0)) { - CrsfRc *instance = new CrsfRc(device_name); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return PX4_ERROR; - } - - _object.store(instance); - _task_id = task_id_is_work_queue; - - instance->ScheduleNow(); - - return PX4_OK; - - } else { - if (device_name) { - PX4_ERR("invalid device (-d) %s", device_name); - - } else { - PX4_ERR("valid device required"); - } + if (!device_name) { + PX4_ERR("Valid device required"); + return PX4_ERROR; } - return PX4_ERROR; + CrsfRc *instance = new CrsfRc(device_name); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleNow(); + + return PX4_OK; } void CrsfRc::Run() @@ -125,183 +127,359 @@ void CrsfRc::Run() _rc_fd = ::open(_device, O_RDWR | O_NONBLOCK); if (_rc_fd >= 0) { - // no parity, one stop bit - struct termios t {}; + struct termios t; + tcgetattr(_rc_fd, &t); cfsetspeed(&t, CRSF_BAUDRATE); - t.c_cflag &= ~(CSTOPB | PARENB); + t.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + t.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + t.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + t.c_oflag = 0; tcsetattr(_rc_fd, TCSANOW, &t); + + if (board_rc_swap_rxtx(_device)) { +#if defined(TIOCSSWAP) + ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED); +#endif // TIOCSSWAP + } + + if (board_rc_singlewire(_device)) { + _is_singlewire = true; +#if defined(TIOCSSINGLEWIRE) + ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); +#endif // TIOCSSINGLEWIRE + } + + PX4_INFO("Crsf serial opened sucessfully"); + + if (_is_singlewire) { + PX4_INFO("Crsf serial is single wire. Telemetry disabled"); + } + + tcflush(_rc_fd, TCIOFLUSH); + + Crc8Init(0xd5); } + + _input_rc.rssi_dbm = NAN; + _input_rc.link_quality = -1; + + CrsfParser_Init(); + + } - // poll with 3 second timeout + // poll with 100mS timeout pollfd fds[1]; fds[0].fd = _rc_fd; fds[0].events = POLLIN; - int ret = ::poll(fds, 1, 3000); + int ret = ::poll(fds, 1, 100); if (ret < 0) { PX4_ERR("poll error"); // try again with delay - ScheduleDelayed(500_ms); + ScheduleDelayed(100_ms); return; } const hrt_abstime time_now_us = hrt_absolute_time(); perf_count_interval(_cycle_interval_perf, time_now_us); - // read all available data from the serial RC input UART + // Read all available data from the serial RC input UART int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE); if (new_bytes > 0) { _bytes_rx += new_bytes; - // parse new data - uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; - uint16_t raw_rc_count = 0; - bool rc_updated = crsf::crsf_parse(time_now_us, &_rcs_buf[0], new_bytes, &raw_rc_values[0], &raw_rc_count, - input_rc_s::RC_INPUT_MAX_CHANNELS); + // Load new bytes into the CRSF parser buffer + CrsfParser_LoadBuffer(_rcs_buf, new_bytes); - if (rc_updated) { - if (!_rc_locked) { - _rc_locked = true; - PX4_INFO("RC input locked"); - } + // Scan the parse buffer for messages, one at a time + CrsfPacket_t new_crsf_packet; - // we have a new CRSF frame. Publish it. - input_rc_s input_rc{}; - input_rc.timestamp_last_signal = time_now_us; - input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS); - input_rc.rssi = -1; // TODO - input_rc.rc_lost = (raw_rc_count == 0); - input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF; + while (CrsfParser_TryParseCrsfPacket(&new_crsf_packet, &_packet_parser_statistics)) { + switch (new_crsf_packet.message_type) { + case CRSF_MESSAGE_TYPE_RC_CHANNELS: + _input_rc.timestamp_last_signal = time_now_us; + _last_packet_seen = time_now_us; - for (int i = 0; i < input_rc.channel_count; i++) { - input_rc.values[i] = raw_rc_values[i]; - } - - input_rc.timestamp = hrt_absolute_time(); - _input_rc_pub.publish(input_rc); - perf_count(_publish_interval_perf); - _rc_valid = input_rc.timestamp; - - if (_param_rc_crsf_tel_en.get() && (input_rc.timestamp > _telemetry_update_last + 100_ms)) { - switch (_next_type) { - case 0: - battery_status_s battery_status; - - if (_battery_status_sub.update(&battery_status)) { - uint16_t voltage = battery_status.voltage_filtered_v * 10; - uint16_t current = battery_status.current_filtered_a * 10; - int fuel = battery_status.discharged_mah; - uint8_t remaining = battery_status.remaining * 100; - crsf::crsf_send_telemetry_battery(_rc_fd, voltage, current, fuel, remaining); - } - - break; - - case 1: - sensor_gps_s vehicle_gps_position; - - if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) { - int32_t latitude = vehicle_gps_position.lat; - int32_t longitude = vehicle_gps_position.lon; - uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f; - uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f; - uint16_t altitude = vehicle_gps_position.alt + 1000; - uint8_t num_satellites = vehicle_gps_position.satellites_used; - crsf::crsf_send_telemetry_gps(_rc_fd, latitude, longitude, groundspeed, gps_heading, altitude, num_satellites); - } - - break; - - case 2: - vehicle_attitude_s vehicle_attitude; - - if (_vehicle_attitude_sub.update(&vehicle_attitude)) { - matrix::Eulerf attitude = matrix::Quatf(vehicle_attitude.q); - int16_t pitch = attitude(1) * 1e4f; - int16_t roll = attitude(0) * 1e4f; - int16_t yaw = attitude(2) * 1e4f; - crsf::crsf_send_telemetry_attitude(_rc_fd, pitch, roll, yaw); - } - - break; - - case 3: - vehicle_status_s vehicle_status; - - if (_vehicle_status_sub.update(&vehicle_status)) { - const char *flight_mode = "(unknown)"; - - switch (vehicle_status.nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: - flight_mode = "Manual"; - break; - - case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - flight_mode = "Altitude"; - break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: - flight_mode = "Position"; - break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - flight_mode = "Return"; - break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - flight_mode = "Mission"; - break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: - case vehicle_status_s::NAVIGATION_STATE_DESCEND: - case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: - case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: - case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: - flight_mode = "Auto"; - break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: - flight_mode = "Acro"; - break; - - case vehicle_status_s::NAVIGATION_STATE_TERMINATION: - flight_mode = "Terminate"; - break; - - case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: - flight_mode = "Offboard"; - break; - - case vehicle_status_s::NAVIGATION_STATE_STAB: - flight_mode = "Stabilized"; - break; - } - - crsf::crsf_send_telemetry_flight_mode(_rc_fd, flight_mode); - } - - break; + for (int i = 0; i < CRSF_CHANNEL_COUNT; i++) { + _input_rc.values[i] = new_crsf_packet.channel_data.channels[i]; } - _telemetry_update_last = input_rc.timestamp; - _next_type = (_next_type + 1) % num_data_types; + break; + + case CRSF_MESSAGE_TYPE_LINK_STATISTICS: + _last_packet_seen = time_now_us; + _input_rc.rssi_dbm = -(float)new_crsf_packet.link_statistics.uplink_rssi_1; + _input_rc.link_quality = new_crsf_packet.link_statistics.uplink_link_quality; + break; + + default: + break; } } + + if (_param_rc_crsf_tel_en.get() && !_is_singlewire + && (_input_rc.timestamp > _telemetry_update_last + 100_ms)) { + switch (_next_type) { + case 0: + battery_status_s battery_status; + + if (_battery_status_sub.update(&battery_status)) { + uint16_t voltage = battery_status.voltage_filtered_v * 10; + uint16_t current = battery_status.current_filtered_a * 10; + int fuel = battery_status.discharged_mah; + uint8_t remaining = battery_status.remaining * 100; + this->SendTelemetryBattery(voltage, current, fuel, remaining); + } + + break; + + case 1: + sensor_gps_s sensor_gps; + + if (_vehicle_gps_position_sub.update(&sensor_gps)) { + int32_t latitude = sensor_gps.lat; + int32_t longitude = sensor_gps.lon; + uint16_t groundspeed = sensor_gps.vel_d_m_s / 3.6f * 10.f; + uint16_t gps_heading = math::degrees(sensor_gps.cog_rad) * 100.f; + uint16_t altitude = sensor_gps.alt + 1000; + uint8_t num_satellites = sensor_gps.satellites_used; + this->SendTelemetryGps(latitude, longitude, groundspeed, gps_heading, altitude, num_satellites); + } + + break; + + case 2: + vehicle_attitude_s vehicle_attitude; + + if (_vehicle_attitude_sub.update(&vehicle_attitude)) { + matrix::Eulerf attitude = matrix::Quatf(vehicle_attitude.q); + int16_t pitch = attitude(1) * 1e4f; + int16_t roll = attitude(0) * 1e4f; + int16_t yaw = attitude(2) * 1e4f; + this->SendTelemetryAttitude(pitch, roll, yaw); + } + + break; + + case 3: + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + const char *flight_mode = "(unknown)"; + + switch (vehicle_status.nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + flight_mode = "Manual"; + break; + + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + flight_mode = "Altitude"; + break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + flight_mode = "Position"; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + flight_mode = "Return"; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + flight_mode = "Mission"; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + flight_mode = "Auto"; + break; + + /*case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + flight_mode = "Failure"; + break;*/ + + case vehicle_status_s::NAVIGATION_STATE_ACRO: + flight_mode = "Acro"; + break; + + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + flight_mode = "Terminate"; + break; + + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + flight_mode = "Offboard"; + break; + + case vehicle_status_s::NAVIGATION_STATE_STAB: + flight_mode = "Stabilized"; + break; + + default: + flight_mode = "Unknown"; + } + + this->SendTelemetryFlightMode(flight_mode); + } + + break; + } + + _telemetry_update_last = _input_rc.timestamp; + _next_type = (_next_type + 1) % num_data_types; + } } - if (!_rc_locked && (hrt_elapsed_time(&_rc_valid) > 5_s)) { - ::close(_rc_fd); - _rc_fd = -1; - _rc_locked = false; - ScheduleDelayed(200_ms); + // If no communication + if (time_now_us - _last_packet_seen > 100_ms) { + // Invalidate link statistics + _input_rc.rssi_dbm = NAN; + _input_rc.link_quality = -1; + } + + // If we have not gotten RC updates specifically + if (time_now_us - _input_rc.timestamp_last_signal > 50_ms) { + _input_rc.rc_lost = 1; + _input_rc.rc_failsafe = 1; } else { - ScheduleDelayed(4_ms); + _input_rc.rc_lost = 0; + _input_rc.rc_failsafe = 0; } + + _input_rc.channel_count = CRSF_CHANNEL_COUNT; + _input_rc.rssi = -1; + _input_rc.rc_ppm_frame_length = 0; + _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF; + _input_rc.timestamp = hrt_absolute_time(); + _input_rc_pub.publish(_input_rc); + + perf_count(_publish_interval_perf); + + ScheduleDelayed(4_ms); +} + +/** + * 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) +{ + // Big endian + buf[offset] = value >> 8; + buf[offset + 1] = value & 0xff; + offset += 2; +} + +/** + * write an uint24_t value to a buffer at a given offset and increment the offset + */ +static inline void write_uint24_t(uint8_t *buf, int &offset, int value) +{ + // Big endian + buf[offset] = value >> 16; + buf[offset + 1] = (value >> 8) & 0xff; + buf[offset + 2] = value & 0xff; + offset += 3; +} + +/** + * write an int32_t value to a buffer at a given offset and increment the offset + */ +static inline void write_int32_t(uint8_t *buf, int &offset, int32_t value) +{ + // Big endian + buf[offset] = value >> 24; + buf[offset + 1] = (value >> 16) & 0xff; + buf[offset + 2] = (value >> 8) & 0xff; + buf[offset + 3] = value & 0xff; + offset += 4; +} + +void CrsfRc::WriteFrameHeader(uint8_t *buf, int &offset, const crsf_frame_type_t type, const uint8_t payload_size) +{ + write_uint8_t(buf, offset, 0xc8); // this got changed from the address to the sync byte + write_uint8_t(buf, offset, payload_size + 2); + write_uint8_t(buf, offset, (uint8_t)type); +} + +void CrsfRc::WriteFrameCrc(uint8_t *buf, int &offset, const int buf_size) +{ + // CRC does not include the address and length + write_uint8_t(buf, offset, Crc8Calc(buf + 2, buf_size - 3)); +} + +bool CrsfRc::SendTelemetryBattery(const uint16_t voltage, const uint16_t current, const int fuel, + const uint8_t remaining) +{ + uint8_t buf[(uint8_t)crsf_payload_size_t::battery_sensor + 4]; + int offset = 0; + WriteFrameHeader(buf, offset, crsf_frame_type_t::battery_sensor, (uint8_t)crsf_payload_size_t::battery_sensor); + write_uint16_t(buf, offset, voltage); + write_uint16_t(buf, offset, current); + write_uint24_t(buf, offset, fuel); + write_uint8_t(buf, offset, remaining); + WriteFrameCrc(buf, offset, sizeof(buf)); + return write(_rc_fd, buf, offset) == offset; +} + +bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed, + const uint16_t gps_heading, const uint16_t altitude, const uint8_t num_satellites) +{ + uint8_t buf[(uint8_t)crsf_payload_size_t::gps + 4]; + int offset = 0; + WriteFrameHeader(buf, offset, crsf_frame_type_t::gps, (uint8_t)crsf_payload_size_t::gps); + write_int32_t(buf, offset, latitude); + write_int32_t(buf, offset, longitude); + write_uint16_t(buf, offset, groundspeed); + write_uint16_t(buf, offset, gps_heading); + write_uint16_t(buf, offset, altitude); + write_uint8_t(buf, offset, num_satellites); + WriteFrameCrc(buf, offset, sizeof(buf)); + return write(_rc_fd, buf, offset) == offset; +} + +bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw) +{ + uint8_t buf[(uint8_t)crsf_payload_size_t::attitude + 4]; + int offset = 0; + WriteFrameHeader(buf, offset, crsf_frame_type_t::attitude, (uint8_t)crsf_payload_size_t::attitude); + write_uint16_t(buf, offset, pitch); + write_uint16_t(buf, offset, roll); + write_uint16_t(buf, offset, yaw); + WriteFrameCrc(buf, offset, sizeof(buf)); + return write(_rc_fd, buf, offset) == offset; +} + +bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode) +{ + const int max_length = 16; + int length = strlen(flight_mode) + 1; + + if (length > max_length) { + length = max_length; + } + + uint8_t buf[max_length + 4]; + int offset = 0; + WriteFrameHeader(buf, offset, crsf_frame_type_t::flight_mode, length); + memcpy(buf + offset, flight_mode, length); + offset += length; + buf[offset - 1] = 0; // ensure null-terminated string + WriteFrameCrc(buf, offset, length + 4); + return write(_rc_fd, buf, offset) == offset; } int CrsfRc::print_status() @@ -311,15 +489,22 @@ int CrsfRc::print_status() PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx); } - PX4_INFO("RC state: %s", _rc_locked ? "found" : "searching for signal"); + if (_is_singlewire) { + PX4_INFO("Telemetry disabled: Singlewire RC port"); - if (_rc_locked) { + } else { PX4_INFO("Telemetry: %s", _param_rc_crsf_tel_en.get() ? "yes" : "no"); } perf_print_counter(_cycle_interval_perf); perf_print_counter(_publish_interval_perf); + PX4_INFO("Disposed bytes: %li", _packet_parser_statistics.disposed_bytes); + PX4_INFO("Valid known packet CRCs: %li", _packet_parser_statistics.crcs_valid_known_packets); + PX4_INFO("Valid unknown packet CRCs: %li", _packet_parser_statistics.crcs_valid_unknown_packets); + PX4_INFO("Invalid CRCs: %li", _packet_parser_statistics.crcs_invalid); + PX4_INFO("Invalid known packet sizes: %li", _packet_parser_statistics.invalid_known_packet_sizes); + return 0; } @@ -337,7 +522,7 @@ int CrsfRc::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This module does the CRSF RC input parsing. +This module parses the CRSF RC uplink protocol and generates CRSF downlink telemetry data )DESCR_STR"); diff --git a/src/drivers/rc/crsf_rc/CrsfRc.hpp b/src/drivers/rc/crsf_rc/CrsfRc.hpp index 3218c2acfd..6603e01ea3 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.hpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.hpp @@ -33,7 +33,7 @@ #pragma once -#include "crsf.h" // old parser TODO: +#include "CrsfParser.hpp" #include #include @@ -74,18 +74,31 @@ public: private: void Run() override; - hrt_abstime _rc_valid{0}; - bool _rc_locked{false}; - uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; + input_rc_s _input_rc{}; + + bool SendTelemetryBattery(const uint16_t voltage, const uint16_t current, const int fuel, const uint8_t remaining); + + bool SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed, + const uint16_t gps_heading, const uint16_t altitude, const uint8_t num_satellites); + + bool SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw); + + bool SendTelemetryFlightMode(const char *flight_mode); + int _rc_fd{-1}; char _device[20] {}; ///< device / serial port path + bool _is_singlewire{false}; static constexpr size_t RC_MAX_BUFFER_SIZE{64}; uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {}; uint32_t _bytes_rx{0}; + hrt_abstime _last_packet_seen{0}; + + CrsfParserStatistics_t _packet_parser_statistics{0}; + // telemetry hrt_abstime _telemetry_update_last{0}; static constexpr int num_data_types{4}; ///< number of different telemetry data types @@ -95,6 +108,34 @@ private: uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + enum class crsf_frame_type_t : uint8_t { + gps = 0x02, + battery_sensor = 0x08, + link_statistics = 0x14, + rc_channels_packed = 0x16, + attitude = 0x1E, + flight_mode = 0x21, + + // Extended Header Frames, range: 0x28 to 0x96 + device_ping = 0x28, + device_info = 0x29, + parameter_settings_entry = 0x2B, + parameter_read = 0x2C, + parameter_write = 0x2D, + command = 0x32 + }; + + enum class crsf_payload_size_t : uint8_t { + gps = 15, + battery_sensor = 8, + link_statistics = 10, + rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes. + attitude = 6, + }; + + void WriteFrameHeader(uint8_t *buf, int &offset, const crsf_frame_type_t type, const uint8_t payload_size); + void WriteFrameCrc(uint8_t *buf, int &offset, const int buf_size); + perf_counter_t _cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; perf_counter_t _publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")}; diff --git a/src/drivers/rc/crsf_rc/QueueBuffer.cpp b/src/drivers/rc/crsf_rc/QueueBuffer.cpp new file mode 100644 index 0000000000..68aeb41a7a --- /dev/null +++ b/src/drivers/rc/crsf_rc/QueueBuffer.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 QueueBuffer.cpp +* +* A very lightweight QueueBuffer implemtnation +* +* @author Chris Seto +*/ + +#include +#include +#include +#include "QueueBuffer.hpp" + +void QueueBuffer_Init(QueueBuffer_t *const q, uint8_t *buffer, const uint32_t buffer_size) +{ + q->buffer = buffer; + q->buffer_size = buffer_size; + + q->head = 0; + q->tail = 0; + q->count = 0; +} + +uint32_t QueueBuffer_Count(const QueueBuffer_t *q) +{ + return q->count; +} + +void QueueBuffer_Append(QueueBuffer_t *const q, const uint8_t x) +{ + // Append the data and make the new tail index + q->buffer[q->tail] = x; + q->tail = ((q->tail + 1) % q->buffer_size); + + // Check if we can expand the count any more + if (q->count < q->buffer_size) { + q->count++; + } +} + +bool QueueBuffer_AppendBuffer(QueueBuffer_t *const q, const uint8_t *x, const uint32_t append_size) +{ + uint32_t buffer_end_size; + + buffer_end_size = q->buffer_size - q->tail; + + // Check if we can even put this buffer into the queu + if (q->count + append_size > q->buffer_size) { + return false; + } + + // Check if there is enough space on the end of the queue to put the entire buffer + if (buffer_end_size >= append_size) { + // We can write the entire append buffer in one shot + memcpy((void *)(q->buffer + q->tail), (void *)x, append_size); + + } else { + memcpy((void *)(q->buffer + q->tail), (void *)x, buffer_end_size); + memcpy((void *)(q->buffer), (void *)(x + buffer_end_size), append_size - buffer_end_size); + } + + // Append to the tail + q->tail = ((q->tail + append_size) % q->buffer_size); + q->count += append_size; + + return true; +} + +bool QueueBuffer_IsEmpty(const QueueBuffer_t *q) +{ + return (q->count == 0); +} + +bool QueueBuffer_Get(QueueBuffer_t *const q, uint8_t *const x) +{ + if (q->count == 0) { + return false; + } + + *x = q->buffer[q->head]; + q->head = ((q->head + 1) % q->buffer_size); + q->count--; + + return true; +} + +void QueueBuffer_Dequeue(QueueBuffer_t *const q, const uint32_t n) +{ + if (n > q->count) { + return; + } + + q->count -= n; + q->head = (q->head + n) % q->buffer_size; +} + +bool QueueBuffer_Peek(const QueueBuffer_t *q, const uint32_t index, uint8_t *const x) +{ + if (index >= q->count) { + return false; + } + + *x = q->buffer[(q->head + index) % q->buffer_size]; + return true; +} + +bool QueueBuffer_PeekBuffer(const QueueBuffer_t *q, const uint32_t index, uint8_t *buffer, const uint32_t size) +{ + uint32_t copy_start; + + copy_start = q->head + index; + + // Check to see if this amount of sizegth exists at the index + if (index + size > q->count) { + return false; + } + + // Check if we can do this in a single shot + if (copy_start + size <= q->buffer_size) { + memcpy((void *)buffer, (void *)(q->buffer + copy_start), size); + + } else { + // Double shot copy + uint32_t copy1Size = (q->buffer_size - copy_start); + memcpy((void *)buffer, (void *)(q->buffer + copy_start), copy1Size); + memcpy((void *)(buffer + copy1Size), (void *)(q->buffer), (size - copy1Size)); + } + + return true; +} \ No newline at end of file diff --git a/src/drivers/rc/crsf_rc/QueueBuffer.hpp b/src/drivers/rc/crsf_rc/QueueBuffer.hpp new file mode 100644 index 0000000000..af035401a6 --- /dev/null +++ b/src/drivers/rc/crsf_rc/QueueBuffer.hpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 QueueBuffer.cpp + * + * A very lightweight QueueBuffer implemtnation + * + * @author Chris Seto + */ + +#pragma once + +#include +#include + +typedef struct { + uint8_t *buffer; + uint32_t buffer_size; + uint32_t count; + uint32_t head; + uint32_t tail; +} QueueBuffer_t; + +void QueueBuffer_Init(QueueBuffer_t *const q, uint8_t *buffer, const uint32_t buffer_size); +uint32_t QueueBuffer_Count(const QueueBuffer_t *q); +void QueueBuffer_Append(QueueBuffer_t *const q, const uint8_t x); +bool QueueBuffer_AppendBuffer(QueueBuffer_t *const q, const uint8_t *x, const uint32_t append_size); +bool QueueBuffer_IsEmpty(const QueueBuffer_t *q); +bool QueueBuffer_Get(QueueBuffer_t *const q, uint8_t *const x); +void QueueBuffer_Dequeue(QueueBuffer_t *const q, const uint32_t n); +bool QueueBuffer_Peek(const QueueBuffer_t *q, const uint32_t index, uint8_t *const x); +bool QueueBuffer_PeekBuffer(const QueueBuffer_t *q, const uint32_t index, uint8_t *buffer, const uint32_t size); \ No newline at end of file diff --git a/src/drivers/rc/crsf_rc/crsf.cpp b/src/drivers/rc/crsf_rc/crsf.cpp deleted file mode 100644 index ee556ca51b..0000000000 --- a/src/drivers/rc/crsf_rc/crsf.cpp +++ /dev/null @@ -1,507 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2022 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. - * - ****************************************************************************/ - -#if 0 // enable non-verbose debugging -#define CRSF_DEBUG PX4_WARN -#else -#define CRSF_DEBUG(...) -#endif - -#if 0 // verbose debugging. Careful when enabling: it leads to too much output, causing dropped bytes -#define CRSF_VERBOSE PX4_WARN -#else -#define CRSF_VERBOSE(...) -#endif - -#include -#include -#include -#include - -#include "crsf.h" - -namespace crsf -{ - -#define CRSF_FRAME_SIZE_MAX 30 // the actual maximum length is 64, but we're only interested in RC channels and want to minimize buffer size -#define CRSF_PAYLOAD_SIZE_MAX (CRSF_FRAME_SIZE_MAX-4) - -struct crsf_frame_header_t { - uint8_t device_address; ///< @see crsf_address_t - uint8_t length; ///< length of crsf_frame_t (including CRC) minus sizeof(crsf_frame_header_t) -}; - -struct crsf_frame_t { - crsf_frame_header_t header; - uint8_t type; ///< @see crsf_frame_type_t - uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; ///< payload data including 1 byte CRC at end -}; - -#define MIN(a,b) (((a)<(b))?(a):(b)) -#define MAX(a,b) (((a)>(b))?(a):(b)) - -#define CRSF_SYNC_BYTE 0xC8 - -enum class crsf_frame_type_t : uint8_t { - gps = 0x02, - battery_sensor = 0x08, - link_statistics = 0x14, - rc_channels_packed = 0x16, - attitude = 0x1E, - flight_mode = 0x21, - - // Extended Header Frames, range: 0x28 to 0x96 - device_ping = 0x28, - device_info = 0x29, - parameter_settings_entry = 0x2B, - parameter_read = 0x2C, - parameter_write = 0x2D, - command = 0x32 -}; - -enum class crsf_payload_size_t : uint8_t { - gps = 15, - battery_sensor = 8, - link_statistics = 10, - rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes. - attitude = 6, -}; - - -enum class crsf_address_t : uint8_t { - broadcast = 0x00, - usb = 0x10, - tbs_core_pnp_pro = 0x80, - reserved1 = 0x8A, - current_sensor = 0xC0, - gps = 0xC2, - tbs_blackbox = 0xC4, - flight_controller = 0xC8, - reserved2 = 0xCA, - race_tag = 0xCC, - radio_transmitter = 0xEA, - crsf_receiver = 0xEC, - crsf_transmitter = 0xEE -}; - -#pragma pack(push, 1) -struct crsf_payload_RC_channels_packed_t { - // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes - unsigned chan0 : 11; - unsigned chan1 : 11; - unsigned chan2 : 11; - unsigned chan3 : 11; - unsigned chan4 : 11; - unsigned chan5 : 11; - unsigned chan6 : 11; - unsigned chan7 : 11; - unsigned chan8 : 11; - unsigned chan9 : 11; - unsigned chan10 : 11; - unsigned chan11 : 11; - unsigned chan12 : 11; - unsigned chan13 : 11; - unsigned chan14 : 11; - unsigned chan15 : 11; -}; - -#pragma pack(pop) - -enum class crsf_parser_state_t : uint8_t { - unsynced = 0, - synced -}; - -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; -} - -static crsf_frame_t crsf_frame{}; -static unsigned current_frame_position = 0; -static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced; - -/** - * parse the current crsf_frame buffer - */ -static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels); - -uint8_t crsf_frame_CRC(const crsf_frame_t &frame); - -/** - * 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 crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, - uint16_t *num_values, uint16_t max_channels) -{ - bool ret = false; - uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame; - - while (len > 0) { - - // fill in the crsf_buffer, as much as we can - const unsigned current_len = MIN(len, sizeof(crsf_frame_t) - current_frame_position); - memcpy(crsf_frame_ptr + current_frame_position, frame, current_len); - current_frame_position += current_len; - - // protection to guarantee parsing progress - if (current_len == 0) { - CRSF_DEBUG("========== parser bug: no progress (%i) ===========", len); - - for (unsigned i = 0; i < current_frame_position; ++i) { - CRSF_DEBUG("crsf_frame_ptr[%i]: 0x%x", i, (int)crsf_frame_ptr[i]); - } - - // reset the parser - current_frame_position = 0; - parser_state = crsf_parser_state_t::unsynced; - return false; - } - - len -= current_len; - frame += current_len; - - if (crsf_parse_buffer(values, num_values, max_channels)) { - ret = true; - } - } - - - return ret; -} - -uint8_t crsf_frame_CRC(const crsf_frame_t &frame) -{ - // CRC includes type and payload - uint8_t crc = crc8_dvb_s2(0, frame.type); - - for (int i = 0; i < frame.header.length - 2; ++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 crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels) -{ - uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame; - - if (parser_state == crsf_parser_state_t::unsynced) { - // there is no sync byte, 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 (crsf_frame_ptr[i] == (uint8_t)crsf_payload_size_t::rc_channels + 2 && - crsf_frame_ptr[i + 1] == (uint8_t)crsf_frame_type_t::rc_channels_packed) { - parser_state = crsf_parser_state_t::synced; - unsigned frame_offset = i - 1; - CRSF_VERBOSE("RC channels found at offset %i", frame_offset); - - // move the rest of the buffer to the beginning - if (frame_offset != 0) { - memmove(crsf_frame_ptr, crsf_frame_ptr + frame_offset, current_frame_position - frame_offset); - current_frame_position -= frame_offset; - } - - break; - } - } - } - - if (parser_state != crsf_parser_state_t::synced) { - if (current_frame_position >= sizeof(crsf_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) { - crsf_frame_ptr[i] = crsf_frame_ptr[sizeof(crsf_frame_t) - current_frame_position + i]; - } - - CRSF_VERBOSE("Discarding buffer"); - } - - return false; - } - - - if (current_frame_position < 3) { - // wait until we have the header & type - return false; - } - - // Now we have at least the header and the type - - const unsigned current_frame_length = crsf_frame.header.length + sizeof(crsf_frame_header_t); - - if (current_frame_length > sizeof(crsf_frame_t) || current_frame_length < 4) { - // frame too long or bogus -> discard everything and go into unsynced state - current_frame_position = 0; - parser_state = crsf_parser_state_t::unsynced; - CRSF_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, crsf_frame.type); - return false; - } - - if (current_frame_position < current_frame_length) { - // we don't have the full frame yet -> wait for more data - CRSF_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 (crsf_frame.type == (uint8_t)crsf_frame_type_t::rc_channels_packed && - crsf_frame.header.length == (uint8_t)crsf_payload_size_t::rc_channels + 2) { - const uint8_t crc = crsf_frame.payload[crsf_frame.header.length - 2]; - - if (crc == crsf_frame_CRC(crsf_frame)) { - const crsf_payload_RC_channels_packed_t *const rc_channels = - (crsf_payload_RC_channels_packed_t *)&crsf_frame.payload; - *num_values = MIN(max_channels, 16); - - if (max_channels > 0) { values[0] = convert_channel_value(rc_channels->chan0); } - - if (max_channels > 1) { values[1] = convert_channel_value(rc_channels->chan1); } - - if (max_channels > 2) { values[2] = convert_channel_value(rc_channels->chan2); } - - if (max_channels > 3) { values[3] = convert_channel_value(rc_channels->chan3); } - - if (max_channels > 4) { values[4] = convert_channel_value(rc_channels->chan4); } - - if (max_channels > 5) { values[5] = convert_channel_value(rc_channels->chan5); } - - if (max_channels > 6) { values[6] = convert_channel_value(rc_channels->chan6); } - - if (max_channels > 7) { values[7] = convert_channel_value(rc_channels->chan7); } - - if (max_channels > 8) { values[8] = convert_channel_value(rc_channels->chan8); } - - if (max_channels > 9) { values[9] = convert_channel_value(rc_channels->chan9); } - - if (max_channels > 10) { values[10] = convert_channel_value(rc_channels->chan10); } - - if (max_channels > 11) { values[11] = convert_channel_value(rc_channels->chan11); } - - if (max_channels > 12) { values[12] = convert_channel_value(rc_channels->chan12); } - - if (max_channels > 13) { values[13] = convert_channel_value(rc_channels->chan13); } - - if (max_channels > 14) { values[14] = convert_channel_value(rc_channels->chan14); } - - if (max_channels > 15) { values[15] = convert_channel_value(rc_channels->chan15); } - - CRSF_VERBOSE("Got Channels"); - - ret = true; - - } else { - CRSF_DEBUG("CRC check failed"); - } - - } else { - CRSF_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, crsf_frame.type); - // We could check the CRC here and reset the parser into unsynced state if it fails. - // But in practise it's robust even without that. - } - - // Either reset or move the rest of the buffer - if (current_frame_position > current_frame_length) { - CRSF_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length); - memmove(crsf_frame_ptr, crsf_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) -{ - // Big endian - buf[offset] = value >> 8; - buf[offset + 1] = value & 0xff; - offset += 2; -} -/** - * write an uint24_t value to a buffer at a given offset and increment the offset - */ -static inline void write_uint24_t(uint8_t *buf, int &offset, int value) -{ - // Big endian - buf[offset] = value >> 16; - buf[offset + 1] = (value >> 8) & 0xff; - buf[offset + 2] = value & 0xff; - offset += 3; -} - -/** - * write an int32_t value to a buffer at a given offset and increment the offset - */ -static inline void write_int32_t(uint8_t *buf, int &offset, int32_t value) -{ - // Big endian - buf[offset] = value >> 24; - buf[offset + 1] = (value >> 16) & 0xff; - buf[offset + 2] = (value >> 8) & 0xff; - buf[offset + 3] = value & 0xff; - offset += 4; -} - -static inline void write_frame_header(uint8_t *buf, int &offset, crsf_frame_type_t type, uint8_t payload_size) -{ - write_uint8_t(buf, offset, CRSF_SYNC_BYTE); // this got changed from the address to the sync byte - write_uint8_t(buf, offset, payload_size + 2); - write_uint8_t(buf, offset, (uint8_t)type); -} -static inline void write_frame_crc(uint8_t *buf, int &offset, int buf_size) -{ - // CRC does not include the address and length - write_uint8_t(buf, offset, crc8_dvb_s2_buf(buf + 2, buf_size - 3)); - - // check correctness of buffer size (only needed during development) - //if (buf_size != offset) { PX4_ERR("frame size mismatch (%i != %i)", buf_size, offset); } -} - -bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining) -{ - uint8_t buf[(uint8_t)crsf_payload_size_t::battery_sensor + 4]; - int offset = 0; - write_frame_header(buf, offset, crsf_frame_type_t::battery_sensor, (uint8_t)crsf_payload_size_t::battery_sensor); - write_uint16_t(buf, offset, voltage); - write_uint16_t(buf, offset, current); - write_uint24_t(buf, offset, fuel); - write_uint8_t(buf, offset, remaining); - write_frame_crc(buf, offset, sizeof(buf)); - return write(uart_fd, buf, offset) == offset; -} - -bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed, - uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites) -{ - uint8_t buf[(uint8_t)crsf_payload_size_t::gps + 4]; - int offset = 0; - write_frame_header(buf, offset, crsf_frame_type_t::gps, (uint8_t)crsf_payload_size_t::gps); - write_int32_t(buf, offset, latitude); - write_int32_t(buf, offset, longitude); - write_uint16_t(buf, offset, groundspeed); - write_uint16_t(buf, offset, gps_heading); - write_uint16_t(buf, offset, altitude); - write_uint8_t(buf, offset, num_satellites); - write_frame_crc(buf, offset, sizeof(buf)); - return write(uart_fd, buf, offset) == offset; -} - -bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw) -{ - uint8_t buf[(uint8_t)crsf_payload_size_t::attitude + 4]; - int offset = 0; - write_frame_header(buf, offset, crsf_frame_type_t::attitude, (uint8_t)crsf_payload_size_t::attitude); - write_uint16_t(buf, offset, pitch); - write_uint16_t(buf, offset, roll); - write_uint16_t(buf, offset, yaw); - write_frame_crc(buf, offset, sizeof(buf)); - return write(uart_fd, buf, offset) == offset; -} - -bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode) -{ - const int max_length = 16; - int length = strlen(flight_mode) + 1; - - if (length > max_length) { - length = max_length; - } - - uint8_t buf[max_length + 4]; - int offset = 0; - write_frame_header(buf, offset, crsf_frame_type_t::flight_mode, length); - memcpy(buf + offset, flight_mode, length); - offset += length; - buf[offset - 1] = 0; // ensure null-terminated string - write_frame_crc(buf, offset, length + 4); - return write(uart_fd, buf, offset) == offset; -} - -}; // namespace crsf diff --git a/src/drivers/rc/crsf_rc/crsf.h b/src/drivers/rc/crsf_rc/crsf.h deleted file mode 100644 index e3dab435ed..0000000000 --- a/src/drivers/rc/crsf_rc/crsf.h +++ /dev/null @@ -1,112 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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 crsf.h - * - * RC protocol definition for CSRF (TBS Crossfire). - * It is an uninverted protocol at 420000 baudrate. - * - * RC channels come in at 150Hz. - * - * @author Beat Küng - */ - -#pragma once - -#include -#include - -namespace crsf -{ - -#define CRSF_BAUDRATE 420000 - -/** - * Parse the CRSF 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, each in range [1000, 2000] - * @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 - */ -bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, 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 - * @param remaining battery remaining [%] - * @return true on success - */ -bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining); - -/** - * Send telemetry GPS information - * @param uart_fd UART file descriptor - * @param latitude latitude [degree * 1e7] - * @param longitude longitude [degree * 1e7] - * @param groundspeed Ground speed [km/h * 10] - * @param gps_heading GPS heading [degree * 100] - * @param altitude Altitude [meters + 1000m offset] - * @param num_satellites number of satellites used - * @return true on success - */ -bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed, - uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites); - -/** - * Send telemetry Attitude information - * @param uart_fd UART file descriptor - * @param pitch Pitch angle [rad * 1e4] - * @param roll Roll angle [rad * 1e4] - * @param yaw Yaw angle [rad * 1e4] - * @return true on success - */ -bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw); - -/** - * Send telemetry Flight Mode information - * @param uart_fd UART file descriptor - * @param flight_mode Flight Mode string (max length = 15) - * @return true on success - */ -bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode); - -}; // namespace crsf diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index f13ede5679..80c7d7afd1 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -753,6 +753,9 @@ void RCInput::Run() if (rc_updated) { perf_count(_publish_interval_perf); + _rc_in.link_quality = -1; + _rc_in.rssi_dbm = NAN; + _to_input_rc.publish(_rc_in); } else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) { diff --git a/src/drivers/rpi_rc_in/rpi_rc_in.cpp b/src/drivers/rpi_rc_in/rpi_rc_in.cpp index d565bac27d..6e52a274c8 100644 --- a/src/drivers/rpi_rc_in/rpi_rc_in.cpp +++ b/src/drivers/rpi_rc_in/rpi_rc_in.cpp @@ -124,6 +124,8 @@ void RcInput::_measure(void) _data.rc_failsafe = false; _data.rc_lost = false; _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; + _data.link_quality = -1; + _data.rssi_dbm = NAN; _rcinput_pub.publish(_data); } diff --git a/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp b/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp index 228b422500..ee39c6d970 100644 --- a/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp +++ b/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp @@ -204,6 +204,9 @@ void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_ input_rc.rc_lost = (valid_chans == 0); input_rc.rc_lost_frame_count = frame_drops; input_rc.rc_total_frame_count = 0; + + input_rc.link_quality = -1; + input_rc.rssi_dbm = NAN; } int start(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 38efbdecdc..8335d87fc6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2126,6 +2126,9 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) } } + rc.link_quality = -1; + rc.rssi_dbm = NAN; + // publish uORB message _rc_pub.publish(rc); } diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index cd496f44cb..450b782848 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -913,6 +913,9 @@ void SimulatorMavlink::handle_message_rc_channels(const mavlink_message_t *msg) rc_input.values[16] = rc_channels.chan17_raw; rc_input.values[17] = rc_channels.chan18_raw; + rc_input.link_quality = -1; + rc_input.rssi_dbm = NAN; + rc_input.timestamp = hrt_absolute_time(); // publish message