forked from Archive/PX4-Autopilot
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
This commit is contained in:
Beat Küng
Lorenz Meier
parent
04dbd40723
commit
6e24bbbaaf
|
|
@ -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.
|
||||
|
||||
|
|
|
|||
|
|
@ -37,6 +37,7 @@ px4_add_module(
|
|||
COMPILE_FLAGS
|
||||
SRCS
|
||||
fmu.cpp
|
||||
crsf_telemetry.cpp
|
||||
DEPENDS
|
||||
circuit_breaker
|
||||
mixer
|
||||
|
|
|
|||
|
|
@ -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 <lib/rc/crsf.h>
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -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 <beat-kueng@gmx.net>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
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;
|
||||
};
|
||||
|
|
@ -48,6 +48,7 @@
|
|||
#include <drivers/drv_mixer.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <lib/rc/crsf.h>
|
||||
#include <lib/rc/dsm.h>
|
||||
#include <lib/rc/sbus.h>
|
||||
#include <lib/rc/st24.h>
|
||||
|
|
@ -71,6 +72,8 @@
|
|||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
#include "crsf_telemetry.h"
|
||||
|
||||
#ifdef HRT_PPM_CHANNEL
|
||||
# include <systemlib/ppm_decode.h>
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue