diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 1fd07821af..50701d9f89 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -34,6 +34,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_PWM_INPUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_ROBOCLAW=y CONFIG_DRIVERS_SAFETY_BUTTON=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 7447404a5a..c7d5d89b85 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -38,6 +38,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA238=y CONFIG_DRIVERS_PWM_INPUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_ROBOCLAW=y CONFIG_DRIVERS_SAFETY_BUTTON=y diff --git a/src/drivers/rc/Kconfig b/src/drivers/rc/Kconfig new file mode 100644 index 0000000000..f4b771e2ba --- /dev/null +++ b/src/drivers/rc/Kconfig @@ -0,0 +1,9 @@ +menu "RC" + menuconfig COMMON_RC + bool "Common RC" + default n + select DRIVERS_RC_CRSF_RC + ---help--- + Enable default set of magnetometer drivers + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/rc/crsf_rc/CMakeLists.txt b/src/drivers/rc/crsf_rc/CMakeLists.txt new file mode 100644 index 0000000000..aaa85e7c54 --- /dev/null +++ b/src/drivers/rc/crsf_rc/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__rc__crsf_rc + MAIN crsf_rc + COMPILE_FLAGS + SRCS + crsf.cpp + crsf.h + CrsfRc.cpp + CrsfRc.hpp + + MODULE_CONFIG + module.yaml + DEPENDS + rc + ) diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp new file mode 100644 index 0000000000..658c523d75 --- /dev/null +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -0,0 +1,360 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "CrsfRc.hpp" + +#include +#include + +using namespace time_literals; + +CrsfRc::CrsfRc(const char *device) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)) +{ + if (device) { + strncpy(_device, device, sizeof(_device) - 1); + _device[sizeof(_device) - 1] = '\0'; + } +} + +CrsfRc::~CrsfRc() +{ + perf_free(_cycle_interval_perf); + perf_free(_publish_interval_perf); +} + +int CrsfRc::task_spawn(int argc, char *argv[]) +{ + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *device_name = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_name = myoptarg; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return -1; + } + + 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"); + } + } + + return PX4_ERROR; +} + +void CrsfRc::Run() +{ + if (should_exit()) { + ScheduleClear(); + ::close(_rc_fd); + _rc_fd = -1; + exit_and_cleanup(); + return; + } + + if (_rc_fd < 0) { + _rc_fd = ::open(_device, O_RDWR | O_NONBLOCK); + + if (_rc_fd >= 0) { + // no parity, one stop bit + struct termios t {}; + tcgetattr(_rc_fd, &t); + cfsetspeed(&t, CRSF_BAUDRATE); + t.c_cflag &= ~(CSTOPB | PARENB); + tcsetattr(_rc_fd, TCSANOW, &t); + } + } + + // poll with 3 second timeout + pollfd fds[1]; + fds[0].fd = _rc_fd; + fds[0].events = POLLIN; + int ret = ::poll(fds, 1, 3000); + + if (ret < 0) { + PX4_ERR("poll error"); + // try again with delay + ScheduleDelayed(500_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 + 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); + + if (rc_updated) { + if (!_rc_locked) { + _rc_locked = true; + PX4_INFO("RC input locked"); + } + + // 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; + + 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: + vehicle_gps_position_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_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; + } + + crsf::crsf_send_telemetry_flight_mode(_rc_fd, 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); + + } else { + ScheduleDelayed(4_ms); + } +} + +int CrsfRc::print_status() +{ + if (_device[0] != '\0') { + PX4_INFO("UART device: %s", _device); + PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx); + } + + PX4_INFO("RC state: %s", _rc_locked ? "found" : "searching for signal"); + + if (_rc_locked) { + PX4_INFO("Telemetry: %s", _param_rc_crsf_tel_en.get() ? "yes" : "no"); + } + + perf_print_counter(_cycle_interval_perf); + perf_print_counter(_publish_interval_perf); + + return 0; +} + +int CrsfRc::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int CrsfRc::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module does the CRSF RC input parsing. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("crsf_rc", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int crsf_rc_main(int argc, char *argv[]) +{ + return CrsfRc::main(argc, argv); +} diff --git a/src/drivers/rc/crsf_rc/CrsfRc.hpp b/src/drivers/rc/crsf_rc/CrsfRc.hpp new file mode 100644 index 0000000000..da4c51c980 --- /dev/null +++ b/src/drivers/rc/crsf_rc/CrsfRc.hpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "crsf.h" // old parser TODO: + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// telemetry +#include +#include +#include +#include +#include + +class CrsfRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + CrsfRc(const char *device); + ~CrsfRc() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + void Run() override; + + hrt_abstime _rc_valid{0}; + bool _rc_locked{false}; + + uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; + + int _rc_fd{-1}; + char _device[20] {}; ///< device / serial port path + + static constexpr size_t RC_MAX_BUFFER_SIZE{64}; + uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {}; + uint32_t _bytes_rx{0}; + + // telemetry + hrt_abstime _telemetry_update_last{0}; + static constexpr int num_data_types{4}; ///< number of different telemetry data types + int _next_type{0}; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + 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")}; + + DEFINE_PARAMETERS( + (ParamBool) _param_rc_crsf_tel_en + ) +}; diff --git a/src/drivers/rc/crsf_rc/Kconfig b/src/drivers/rc/crsf_rc/Kconfig new file mode 100644 index 0000000000..21096275af --- /dev/null +++ b/src/drivers/rc/crsf_rc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_RC_CRSF_RC + bool "crsf_rc" + default n + ---help--- + Enable support for crsf rc diff --git a/src/drivers/rc/crsf_rc/crsf.cpp b/src/drivers/rc/crsf_rc/crsf.cpp new file mode 100644 index 0000000000..ee556ca51b --- /dev/null +++ b/src/drivers/rc/crsf_rc/crsf.cpp @@ -0,0 +1,507 @@ +/**************************************************************************** + * + * 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 new file mode 100644 index 0000000000..e3dab435ed --- /dev/null +++ b/src/drivers/rc/crsf_rc/crsf.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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/crsf_rc/module.yaml b/src/drivers/rc/crsf_rc/module.yaml new file mode 100644 index 0000000000..fc4ce23a27 --- /dev/null +++ b/src/drivers/rc/crsf_rc/module.yaml @@ -0,0 +1,22 @@ +module_name: CRSF RC Input Driver +serial_config: + - command: "crsf_rc start -d ${SERIAL_DEV}" + port_config_param: + name: RC_CRSF_PRT_CFG + group: Serial + #default: RC + #depends_on_port: RC + description_extended: | + Crossfire RC (CRSF) driver. + +parameters: + - group: RC + definitions: + RC_CRSF_TEL_EN: + description: + short: Crossfire RC telemetry enable + long: | + Crossfire telemetry enable + + type: boolean + default: [0]