From 6e24bbbaaff73c53ddc5a82a289ac2d747fed0fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 23 Jul 2018 18:56:10 +0200 Subject: [PATCH] fmu: add CRSF RC and Telemetry support - Telemetry is only enabled on omnibus, since on Pixhawk it seems we cannot write to the RC UART due to how the board is wired - For the Telemetry the UART needs to be opened RW --- msg/input_rc.msg | 1 + src/drivers/px4fmu/CMakeLists.txt | 1 + src/drivers/px4fmu/crsf_telemetry.cpp | 207 ++++++++++++++++++++++++++ src/drivers/px4fmu/crsf_telemetry.h | 91 +++++++++++ src/drivers/px4fmu/fmu.cpp | 72 ++++++++- src/lib/rc/dsm.cpp | 2 +- 6 files changed, 367 insertions(+), 7 deletions(-) create mode 100644 src/drivers/px4fmu/crsf_telemetry.cpp create mode 100644 src/drivers/px4fmu/crsf_telemetry.h diff --git a/msg/input_rc.msg b/msg/input_rc.msg index 4df163fb49..1a7de29afd 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -12,6 +12,7 @@ uint8 RC_INPUT_SOURCE_PX4FMU_ST24 = 10 uint8 RC_INPUT_SOURCE_PX4FMU_SUMD = 11 uint8 RC_INPUT_SOURCE_PX4FMU_DSM = 12 uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 13 +uint8 RC_INPUT_SOURCE_PX4FMU_CRSF = 14 uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. diff --git a/src/drivers/px4fmu/CMakeLists.txt b/src/drivers/px4fmu/CMakeLists.txt index 1f36779a24..33c261497f 100644 --- a/src/drivers/px4fmu/CMakeLists.txt +++ b/src/drivers/px4fmu/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( COMPILE_FLAGS SRCS fmu.cpp + crsf_telemetry.cpp DEPENDS circuit_breaker mixer diff --git a/src/drivers/px4fmu/crsf_telemetry.cpp b/src/drivers/px4fmu/crsf_telemetry.cpp new file mode 100644 index 0000000000..1d218f0206 --- /dev/null +++ b/src/drivers/px4fmu/crsf_telemetry.cpp @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "crsf_telemetry.h" +#include + +CRSFTelemetry::CRSFTelemetry(int uart_fd) + : _vehicle_gps_position_sub(orb_subscribe(ORB_ID(vehicle_gps_position))), + _battery_status_sub(orb_subscribe(ORB_ID(battery_status))), + _vehicle_attitude_sub(orb_subscribe(ORB_ID(vehicle_attitude))), + _vehicle_status_sub(orb_subscribe(ORB_ID(vehicle_status))), + _uart_fd(uart_fd) +{ +} + +CRSFTelemetry::~CRSFTelemetry() +{ + orb_unsubscribe(_vehicle_gps_position_sub); + orb_unsubscribe(_battery_status_sub); + orb_unsubscribe(_vehicle_attitude_sub); + orb_unsubscribe(_vehicle_status_sub); +} + +bool CRSFTelemetry::update(const hrt_abstime &now) +{ + const int update_rate_hz = 10; + + if (now - _last_update <= 1_s / (update_rate_hz * num_data_types)) { + return false; + } + + bool sent = false; + + switch (_next_type) { + case 0: + sent = send_battery(); + break; + + case 1: + sent = send_gps(); + break; + + case 2: + sent = send_attitude(); + break; + + case 3: + sent = send_flight_mode(); + break; + } + + _last_update = now; + _next_type = (_next_type + 1) % num_data_types; + + return sent; +} + +bool CRSFTelemetry::send_battery() +{ + battery_status_s battery_status; + + if (orb_copy(ORB_ID(battery_status), _battery_status_sub, &battery_status) != 0) { + return false; + } + + 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; + return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining); +} + +bool CRSFTelemetry::send_gps() +{ + vehicle_gps_position_s vehicle_gps_position; + + if (orb_copy(ORB_ID(vehicle_gps_position), _vehicle_gps_position_sub, &vehicle_gps_position) != 0) { + return false; + } + + 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; + + return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed, + gps_heading, altitude, num_satellites); +} + +bool CRSFTelemetry::send_attitude() +{ + vehicle_attitude_s vehicle_attitude; + + if (orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude) != 0) { + return false; + } + + 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; + return crsf_send_telemetry_attitude(_uart_fd, pitch, roll, yaw); +} + +bool CRSFTelemetry::send_flight_mode() +{ + vehicle_status_s vehicle_status; + + if (orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status) != 0) { + return false; + } + + 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_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: + 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: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: + 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; + + case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: + flight_mode = "Rattitude"; + break; + } + + return crsf_send_telemetry_flight_mode(_uart_fd, flight_mode); +} + diff --git a/src/drivers/px4fmu/crsf_telemetry.h b/src/drivers/px4fmu/crsf_telemetry.h new file mode 100644 index 0000000000..3a0e989e6f --- /dev/null +++ b/src/drivers/px4fmu/crsf_telemetry.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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_telemetry.h + * + * @author Beat Küng + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +using namespace time_literals; + +/** + * High-level class that handles sending of CRSF telemetry data + */ +class CRSFTelemetry +{ +public: + /** + * @param uart_fd file descriptor for the UART to use. It is expected to be configured + * already. + */ + CRSFTelemetry(int uart_fd); + + ~CRSFTelemetry(); + + /** + * Send telemetry data. Call this regularly (i.e. at 100Hz), it will automatically + * limit the sending rate. + * @return true if new data sent + */ + bool update(const hrt_abstime &now); + +private: + bool send_battery(); + bool send_gps(); + bool send_attitude(); + bool send_flight_mode(); + + int _vehicle_gps_position_sub; + int _battery_status_sub; + int _vehicle_attitude_sub; + int _vehicle_status_sub; + + hrt_abstime _last_update{0}; + + static constexpr int num_data_types{4}; ///< number of different telemetry data types + int _next_type{0}; + + int _uart_fd; +}; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 5180e58a67..896c2c4a8b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -71,6 +72,8 @@ #include #include +#include "crsf_telemetry.h" + #ifdef HRT_PPM_CHANNEL # include #endif @@ -197,16 +200,18 @@ private: RC_SCAN_SBUS, RC_SCAN_DSM, RC_SCAN_SUMD, - RC_SCAN_ST24 + RC_SCAN_ST24, + RC_SCAN_CRSF }; enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS; - char const *RC_SCAN_STRING[5] = { + char const *RC_SCAN_STRING[6] = { "PPM", "SBUS", "DSM", "SUMD", - "ST24" + "ST24", + "CRSF" }; enum class MotorOrdering : int32_t { @@ -282,6 +287,8 @@ private: bool _airmode; ///< multicopter air-mode MotorOrdering _motor_ordering; + CRSFTelemetry *_crsf_telemetry; + perf_counter_t _perf_control_latency; static bool arm_nothrottle() @@ -397,6 +404,7 @@ PX4FMU::PX4FMU(bool run_as_task) : _thr_mdl_fac(0.0f), _airmode(false), _motor_ordering(MotorOrdering::PX4), + _crsf_telemetry(nullptr), _perf_control_latency(perf_alloc(PC_ELAPSED, "fmu control latency")) { for (unsigned i = 0; i < _max_actuators; i++) { @@ -472,6 +480,10 @@ PX4FMU::~PX4FMU() unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); perf_free(_perf_control_latency); + + if (_crsf_telemetry) { + delete (_crsf_telemetry); + } } int @@ -1751,14 +1763,60 @@ PX4FMU::cycle() // disable CPPM input by mapping it away from the timer capture input px4_arch_unconfiggpio(GPIO_PPM_IN); // Scan the next protocol - set_rc_scan_state(RC_SCAN_SBUS); + set_rc_scan_state(RC_SCAN_CRSF); } #else // skip PPM if it's not supported - set_rc_scan_state(RC_SCAN_SBUS); + set_rc_scan_state(RC_SCAN_CRSF); #endif // HRT_PPM_CHANNEL + break; + + case RC_SCAN_CRSF: + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // Configure serial port for CRSF + crsf_config(_rcs_fd); + rc_io_invert(false, RC_UXART_BASE); + + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + // parse new data + if (newBytes > 0) { + rc_updated = crsf_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, + input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (rc_updated) { + // we have a new CRSF frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF; + fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, false, false, 0); + + // Enable CRSF Telemetry only on the Omnibus, because on Pixhawk (-related) boards + // we cannot write to the RC UART + // It might work on FMU-v5. Or another option is to use a different UART port +#ifdef CONFIG_ARCH_BOARD_OMNIBUS_F4SD + + if (!_rc_scan_locked && !_crsf_telemetry) { + _crsf_telemetry = new CRSFTelemetry(_rcs_fd); + } + +#endif + + _rc_scan_locked = true; + + if (_crsf_telemetry) { + _crsf_telemetry->update(_cycle_timestamp); + } + } + } + + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_SBUS); + } + break; } @@ -3496,6 +3554,7 @@ In addition it does the RC input parsing and auto-selecting the method. Supporte - DSM - SUMD - ST24 +- TBS Crossfire (CRSF) The module is configured via mode_* commands. This defines which of the first N pins the driver should occupy. By using mode_pwm4 for example, pins 5 and 6 can be used by the camera trigger driver or by a PWM rangefinder @@ -3571,7 +3630,8 @@ int PX4FMU::print_status() PX4_INFO("Max update rate: %i Hz", _current_update_rate); } - PX4_INFO("RC scan state: %s", RC_SCAN_STRING[_rc_scan_state]); + PX4_INFO("RC scan state: %s, locked: %s", RC_SCAN_STRING[_rc_scan_state], _rc_scan_locked ? "yes" : "no"); + PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no"); #ifdef RC_SERIAL_PORT PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames()); #endif diff --git a/src/lib/rc/dsm.cpp b/src/lib/rc/dsm.cpp index 8483182965..6821e69ebd 100644 --- a/src/lib/rc/dsm.cpp +++ b/src/lib/rc/dsm.cpp @@ -294,7 +294,7 @@ dsm_init(const char *device) { if (dsm_fd < 0) { - dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + dsm_fd = open(device, O_RDWR | O_NONBLOCK); } dsm_proto_init();