fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 05:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

px4fmu move RC input to new rc_input driver

Browse Source
This commit is contained in:
Daniel Agar
2018-07-30 10:32:56 -04:00
parent 4e05f26659
commit 658237f36a
21 changed files with 1027 additions and 640 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/CMakeLists.txt
-2
View File
@@ -37,10 +37,8 @@ px4_add_module(
COMPILE_FLAGS
SRCS
fmu.cpp
crsf_telemetry.cpp
DEPENDS
circuit_breaker
mixer
rc
pwm_limit
)
src/drivers/px4fmu/crsf_telemetry.cpp
-207
View File
@@ -1,207 +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.
*
****************************************************************************/
#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);
}
src/drivers/px4fmu/crsf_telemetry.h
-91
View File
@@ -1,91 +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_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;
};
src/drivers/px4fmu/fmu.cpp
+3 -622
View File
@@ -47,12 +47,6 @@
#include <drivers/drv_input_capture.h>
#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>
#include <lib/rc/sumd.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_log.h>
@@ -65,36 +59,15 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/adc_report.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
#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
#define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */
static constexpr uint8_t CYCLE_COUNT = 10; /* safety switch must be held for 1 second to activate */
static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
#if defined(PX4_CPU_UUID_WORD32_FORMAT)
# define CPU_UUID_FORMAT PX4_CPU_UUID_WORD32_FORMAT
#else
# define CPU_UUID_FORMAT "%0X"
#endif
#if defined(PX4_CPU_UUID_WORD32_SEPARATOR)
# define CPU_UUID_SEPARATOR PX4_CPU_UUID_WORD32_SEPARATOR
#else
# define CPU_UUID_SEPARATOR " "
#endif
/*
* Define the various LED flash sequences for each system state.
*/
@@ -109,7 +82,6 @@ enum PortMode {
PORT_MODE_UNSET = 0,
PORT_FULL_GPIO,
PORT_FULL_PWM,
PORT_RC_IN,
PORT_PWM6,
PORT_PWM4,
PORT_PWM3,
@@ -195,34 +167,12 @@ public:
void update_pwm_trims();
private:
enum RC_SCAN {
RC_SCAN_PPM = 0,
RC_SCAN_SBUS,
RC_SCAN_DSM,
RC_SCAN_SUMD,
RC_SCAN_ST24,
RC_SCAN_CRSF
};
enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS;
char const *RC_SCAN_STRING[6] = {
"PPM",
"SBUS",
"DSM",
"SUMD",
"ST24",
"CRSF"
};
enum class MotorOrdering : int32_t {
PX4 = 0,
Betaflight = 1
};
hrt_abstime _rc_scan_begin = 0;
bool _rc_scan_locked = false;
bool _report_lock = true;
hrt_abstime _cycle_timestamp = 0;
hrt_abstime _last_safety_check = 0;
hrt_abstime _time_last_mix = 0;
@@ -236,19 +186,13 @@ private:
unsigned _current_update_rate;
bool _run_as_task;
static struct work_s _work;
int _vehicle_cmd_sub;
int _armed_sub;
int _param_sub;
int _adc_sub;
struct rc_input_values _rc_in;
float _analog_rc_rssi_volt;
bool _analog_rc_rssi_stable;
orb_advert_t _to_input_rc;
orb_advert_t _outputs_pub;
unsigned _num_outputs;
int _class_instance;
int _rcs_fd;
uint8_t _rcs_buf[SBUS_BUFFER_SIZE];
bool _throttle_armed;
bool _pwm_on;
@@ -265,9 +209,6 @@ private:
pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
unsigned _poll_fds_num;
uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
uint16_t _raw_rc_count;
static pwm_limit_t _pwm_limit;
static actuator_armed_s _armed;
uint16_t _failsafe_pwm[_max_actuators];
@@ -287,8 +228,6 @@ private:
bool _airmode; ///< multicopter air-mode
MotorOrdering _motor_ordering;
CRSFTelemetry *_crsf_telemetry;
perf_counter_t _perf_control_latency;
static bool arm_nothrottle()
@@ -336,13 +275,6 @@ private:
PX4FMU(const PX4FMU &) = delete;
PX4FMU operator=(const PX4FMU &) = delete;
void fill_rc_in(uint16_t raw_rc_count_local,
uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
hrt_abstime now, bool frame_drop, bool failsafe,
unsigned frame_drops, int rssi);
void set_rc_scan_state(RC_SCAN _rc_scan_state);
void rc_io_invert(bool invert, uint32_t uxart_base);
void safety_check_button(void);
void flash_safety_button(void);
@@ -370,18 +302,11 @@ PX4FMU::PX4FMU(bool run_as_task) :
_pwm_alt_rate_channels(0),
_current_update_rate(0),
_run_as_task(run_as_task),
_vehicle_cmd_sub(-1),
_armed_sub(-1),
_param_sub(-1),
_adc_sub(-1),
_rc_in{},
_analog_rc_rssi_volt(-1.0f),
_analog_rc_rssi_stable(false),
_to_input_rc(nullptr),
_outputs_pub(nullptr),
_num_outputs(0),
_class_instance(0),
_rcs_fd(-1),
_throttle_armed(false),
_pwm_on(false),
_pwm_mask(0),
@@ -390,7 +315,6 @@ PX4FMU::PX4FMU(bool run_as_task) :
_groups_required(0),
_groups_subscribed(0),
_poll_fds_num(0),
_raw_rc_count(0),
_failsafe_pwm{0},
_disarmed_pwm{0},
_reverse_pwm_mask(0),
@@ -404,7 +328,6 @@ 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++) {
@@ -432,16 +355,6 @@ PX4FMU::PX4FMU(bool run_as_task) :
_armed.force_failsafe = false;
_armed.in_esc_calibration_mode = false;
// rc input, published to ORB
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
// initialize it as RC lost
_rc_in.rc_lost = true;
// initialize raw_rc values and count
for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
_raw_rc_values[i] = UINT16_MAX;
}
// If there is no safety button, disable it on boot.
#ifndef GPIO_BTN_SAFETY
_safety_off = true;
@@ -462,7 +375,6 @@ PX4FMU::~PX4FMU()
orb_unsubscribe(_armed_sub);
orb_unsubscribe(_param_sub);
orb_unadvertise(_to_input_rc);
orb_unadvertise(_outputs_pub);
orb_unadvertise(_to_safety);
orb_unadvertise(_to_mixer_status);
@@ -470,20 +382,12 @@ PX4FMU::~PX4FMU()
/* make sure servos are off */
up_pwm_servo_deinit();
#ifdef RC_SERIAL_PORT
dsm_deinit();
#endif
/* note - someone else is responsible for restoring the GPIO config */
/* clean up the alternate device node */
unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
perf_free(_perf_control_latency);
if (_crsf_telemetry) {
delete (_crsf_telemetry);
}
}
int
@@ -516,30 +420,10 @@ PX4FMU::init()
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_param_sub = orb_subscribe(ORB_ID(parameter_update));
_adc_sub = orb_subscribe(ORB_ID(adc_report));
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
#ifdef RC_SERIAL_PORT
# ifdef RF_RADIO_POWER_CONTROL
// power radio on
RF_RADIO_POWER_CONTROL(true);
# endif
_vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
// dsm_init sets some file static variables and returns a file descriptor
_rcs_fd = dsm_init(RC_SERIAL_PORT);
// assume SBUS input and immediately switch it to
// so that if Single wire mode on TX there will be only
// a short contention
sbus_config(_rcs_fd, board_supports_single_wire(RC_UXART_BASE));
# ifdef GPIO_PPM_IN
// disable CPPM input by mapping it away from the timer capture input
px4_arch_unconfiggpio(GPIO_PPM_IN);
# endif
#endif
// Getting initial parameter values
update_params();
@@ -1113,85 +997,6 @@ PX4FMU::capture_callback(uint32_t chan_index,
fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, overflow);
}
void
PX4FMU::fill_rc_in(uint16_t raw_rc_count_local,
uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
hrt_abstime now, bool frame_drop, bool failsafe,
unsigned frame_drops, int rssi = -1)
{
// fill rc_in struct for publishing
_rc_in.channel_count = raw_rc_count_local;
if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
}
unsigned valid_chans = 0;
for (unsigned i = 0; i < _rc_in.channel_count; i++) {
_rc_in.values[i] = raw_rc_values_local[i];
if (raw_rc_values_local[i] != UINT16_MAX) {
valid_chans++;
}
// once filled, reset values back to default
_raw_rc_values[i] = UINT16_MAX;
}
_rc_in.timestamp = now;
_rc_in.timestamp_last_signal = _rc_in.timestamp;
_rc_in.rc_ppm_frame_length = 0;
/* fake rssi if no value was provided */
if (rssi == -1) {
/* set RSSI if analog RSSI input is present */
if (_analog_rc_rssi_stable) {
float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
if (rssi_analog > 100.0f) {
rssi_analog = 100.0f;
}
if (rssi_analog < 0.0f) {
rssi_analog = 0.0f;
}
_rc_in.rssi = rssi_analog;
} else {
_rc_in.rssi = 255;
}
} else {
_rc_in.rssi = rssi;
}
if (valid_chans == 0) {
_rc_in.rssi = 0;
}
_rc_in.rc_failsafe = failsafe;
_rc_in.rc_lost = (valid_chans == 0);
_rc_in.rc_lost_frame_count = frame_drops;
_rc_in.rc_total_frame_count = 0;
}
#ifdef RC_SERIAL_PORT
void PX4FMU::set_rc_scan_state(RC_SCAN newState)
{
// PX4_WARN("RCscan: %s failed, trying %s", PX4FMU::RC_SCAN_STRING[_rc_scan_state], PX4FMU::RC_SCAN_STRING[newState]);
_rc_scan_begin = 0;
_rc_scan_state = newState;
}
void PX4FMU::rc_io_invert(bool invert, uint32_t uxart_base)
{
INVERT_RC_INPUT(invert, uxart_base);
}
#endif
void
PX4FMU::update_pwm_out_state(bool on)
{
@@ -1228,7 +1033,7 @@ PX4FMU::cycle()
_current_update_rate = 0;
}
int poll_timeout = 5; // needs to be small enough so that we don't miss RC input data
int poll_timeout = 5;
if (!_run_as_task) {
/*
@@ -1491,358 +1296,12 @@ PX4FMU::cycle()
update_pwm_out_state(pwm_on);
}
#ifdef RC_SERIAL_PORT
/* vehicle command */
orb_check(_vehicle_cmd_sub, &updated);
if (updated) {
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub, &cmd);
// Check for a pairing command
if ((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
if (!_armed.armed) {
if ((int)cmd.param1 == 0) {
// DSM binding command
int dsm_bind_mode = (int)cmd.param2;
int dsm_bind_pulses = 0;
if (dsm_bind_mode == 0) {
dsm_bind_pulses = DSM2_BIND_PULSES;
} else if (dsm_bind_mode == 1) {
dsm_bind_pulses = DSMX_BIND_PULSES;
} else {
dsm_bind_pulses = DSMX8_BIND_PULSES;
}
ioctl(nullptr, DSM_BIND_START, dsm_bind_pulses);
}
} else {
PX4_WARN("system armed, bind request rejected");
}
}
}
#endif
orb_check(_param_sub, &updated);
if (updated) {
this->update_params();
}
/* update ADC sampling */
#ifdef ADC_RC_RSSI_CHANNEL
orb_check(_adc_sub, &updated);
if (updated) {
struct adc_report_s adc;
orb_copy(ORB_ID(adc_report), _adc_sub, &adc);
const unsigned adc_chans = sizeof(adc.channel_id) / sizeof(adc.channel_id[0]);
for (unsigned i = 0; i < adc_chans; i++) {
if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
if (_analog_rc_rssi_volt < 0.0f) {
_analog_rc_rssi_volt = adc.channel_value[i];
}
_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc.channel_value[i] * 0.005f;
/* only allow this to be used if we see a high RSSI once */
if (_analog_rc_rssi_volt > 2.5f) {
_analog_rc_rssi_stable = true;
}
}
}
}
#endif
bool rc_updated = false;
#ifdef RC_SERIAL_PORT
// This block scans for a supported serial RC input and locks onto the first one found
// Scan for 300 msec, then switch protocol
constexpr hrt_abstime rc_scan_max = 300 * 1000;
bool sbus_failsafe, sbus_frame_drop;
unsigned frame_drops;
bool dsm_11_bit;
if (_report_lock && _rc_scan_locked) {
_report_lock = false;
//PX4_WARN("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
}
// read all available data from the serial RC input UART
int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE);
switch (_rc_scan_state) {
case RC_SCAN_SBUS:
if (_rc_scan_begin == 0) {
_rc_scan_begin = _cycle_timestamp;
// Configure serial port for SBUS
sbus_config(_rcs_fd, board_supports_single_wire(RC_UXART_BASE));
rc_io_invert(true, RC_UXART_BASE);
} else if (_rc_scan_locked
|| _cycle_timestamp - _rc_scan_begin < rc_scan_max) {
// parse new data
if (newBytes > 0) {
rc_updated = sbus_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
if (rc_updated) {
// we have a new SBUS frame. Publish it.
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp,
sbus_frame_drop, sbus_failsafe, frame_drops);
_rc_scan_locked = true;
}
}
} else {
// Scan the next protocol
set_rc_scan_state(RC_SCAN_DSM);
}
break;
case RC_SCAN_DSM:
if (_rc_scan_begin == 0) {
_rc_scan_begin = _cycle_timestamp;
// // Configure serial port for DSM
dsm_config(_rcs_fd);
rc_io_invert(false, RC_UXART_BASE);
} else if (_rc_scan_locked
|| _cycle_timestamp - _rc_scan_begin < rc_scan_max) {
if (newBytes > 0) {
int8_t dsm_rssi;
// parse new data
rc_updated = dsm_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
&dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
if (rc_updated) {
// we have a new DSM frame. Publish it.
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp,
false, false, frame_drops, dsm_rssi);
_rc_scan_locked = true;
}
}
} else {
// Scan the next protocol
set_rc_scan_state(RC_SCAN_ST24);
}
break;
case RC_SCAN_ST24:
if (_rc_scan_begin == 0) {
_rc_scan_begin = _cycle_timestamp;
// Configure serial port for DSM
dsm_config(_rcs_fd);
rc_io_invert(false, RC_UXART_BASE);
} else if (_rc_scan_locked
|| _cycle_timestamp - _rc_scan_begin < rc_scan_max) {
if (newBytes > 0) {
// parse new data
uint8_t st24_rssi, lost_count;
rc_updated = false;
for (unsigned i = 0; i < (unsigned)newBytes; i++) {
/* set updated flag if one complete packet was parsed */
st24_rssi = RC_INPUT_RSSI_MAX;
rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
}
// The st24 will keep outputting RC channels and RSSI even if RC has been lost.
// The only way to detect RC loss is therefore to look at the lost_count.
if (rc_updated) {
if (lost_count == 0) {
// we have a new ST24 frame. Publish it.
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp,
false, false, frame_drops, st24_rssi);
_rc_scan_locked = true;
} else {
// if the lost count > 0 means that there is an RC loss
_rc_in.rc_lost = true;
}
}
}
} else {
// Scan the next protocol
set_rc_scan_state(RC_SCAN_SUMD);
}
break;
case RC_SCAN_SUMD:
if (_rc_scan_begin == 0) {
_rc_scan_begin = _cycle_timestamp;
// Configure serial port for DSM
dsm_config(_rcs_fd);
rc_io_invert(false, RC_UXART_BASE);
} else if (_rc_scan_locked
|| _cycle_timestamp - _rc_scan_begin < rc_scan_max) {
if (newBytes > 0) {
// parse new data
uint8_t sumd_rssi, rx_count;
bool sumd_failsafe;
rc_updated = false;
for (unsigned i = 0; i < (unsigned)newBytes; i++) {
/* set updated flag if one complete packet was parsed */
sumd_rssi = RC_INPUT_RSSI_MAX;
rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
}
if (rc_updated) {
// we have a new SUMD frame. Publish it.
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp,
false, sumd_failsafe, frame_drops, sumd_rssi);
_rc_scan_locked = true;
}
}
} else {
// Scan the next protocol
set_rc_scan_state(RC_SCAN_PPM);
}
break;
case RC_SCAN_PPM:
// skip PPM if it's not supported
#ifdef HRT_PPM_CHANNEL
if (_rc_scan_begin == 0) {
_rc_scan_begin = _cycle_timestamp;
// Configure timer input pin for CPPM
px4_arch_configgpio(GPIO_PPM_IN);
rc_io_invert(false, RC_UXART_BASE);
} else if (_rc_scan_locked || _cycle_timestamp - _rc_scan_begin < rc_scan_max) {
// see if we have new PPM input data
if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
// we have a new PPM frame. Publish it.
rc_updated = true;
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp, false, false, 0);
_rc_scan_locked = true;
_rc_in.rc_ppm_frame_length = ppm_frame_length;
_rc_in.timestamp_last_signal = ppm_last_valid_decode;
}
} else {
// 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_CRSF);
}
#else // skip PPM if it's not supported
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;
}
#else // RC_SERIAL_PORT not defined
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
// we have a new PPM frame. Publish it.
rc_updated = true;
fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp, false, false, 0);
_rc_in.rc_ppm_frame_length = ppm_frame_length;
_rc_in.timestamp_last_signal = ppm_last_valid_decode;
}
#endif // HRT_PPM_CHANNEL
#endif // RC_SERIAL_PORT
if (rc_updated) {
int instance = _class_instance;
orb_publish_auto(ORB_ID(input_rc), &_to_input_rc, &_rc_in, &instance, ORB_PRIO_DEFAULT);
} else if (!rc_updated && ((hrt_absolute_time() - _rc_in.timestamp_last_signal) > 1000 * 1000)) {
_rc_scan_locked = false;
}
if (_run_as_task) {
if (should_exit()) {
break;
@@ -2581,41 +2040,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
#ifdef SPEKTRUM_POWER
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
PX4_INFO("DSM_BIND_START: DSM%s RX", (arg == 0) ? "2" : ((arg == 1) ? "-X" : "-X8"));
if (arg == DSM2_BIND_PULSES ||
arg == DSMX_BIND_PULSES ||
arg == DSMX8_BIND_PULSES) {
dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0);
dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0);
usleep(500000);
dsm_bind(DSM_CMD_BIND_POWER_UP, 0);
usleep(72000);
irqstate_t flags = px4_enter_critical_section();
dsm_bind(DSM_CMD_BIND_SEND_PULSES, arg);
px4_leave_critical_section(flags);
usleep(50000);
dsm_bind(DSM_CMD_BIND_REINIT_UART, 0);
ret = OK;
} else {
PX4_ERR("DSM bind failed");
ret = -EINVAL;
}
break;
#endif
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
@@ -3074,10 +2498,6 @@ PX4FMU::fmu_new_mode(PortMode new_mode)
#endif
break;
case PORT_RC_IN:
servo_mode = PX4FMU::MODE_NONE;
break;
case PORT_PWM1:
/* select 2-pin PWM mode */
servo_mode = PX4FMU::MODE_1PWM;
@@ -3144,22 +2564,6 @@ PX4FMU::fmu_new_mode(PortMode new_mode)
namespace
{
void
bind_spektrum()
{
int fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0) {
PX4_ERR("open fail");
return;
}
/* specify 11ms DSMX. RX will automatically fall back to 22ms or DSM2 if necessary */
ioctl(fd, DSM_BIND_START, DSMX8_BIND_PULSES);
close(fd);
}
int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz)
{
return PX4FMU::set_i2c_bus_clock(bus, clock_hz);
@@ -3414,11 +2818,6 @@ int PX4FMU::custom_command(int argc, char *argv[])
PortMode new_mode = PORT_MODE_UNSET;
const char *verb = argv[0];
if (!strcmp(verb, "bind")) {
bind_spektrum();
return 0;
}
/* does not operate on a FMU instance */
if (!strcmp(verb, "i2c")) {
if (argc > 2) {
@@ -3478,9 +2877,6 @@ int PX4FMU::custom_command(int argc, char *argv[])
if (!strcmp(verb, "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
} else if (!strcmp(verb, "mode_rcin")) {
new_mode = PORT_RC_IN;
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
@@ -3493,7 +2889,6 @@ int PX4FMU::custom_command(int argc, char *argv[])
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
} else if (!strcmp(verb, "mode_pwm4")) {
new_mode = PORT_PWM4;
@@ -3548,13 +2943,6 @@ This module is responsible for driving the output and reading the input pins. Fo
px4io driver is used for main ones.
It listens on the actuator_controls topics, does the mixing and writes the PWM outputs.
In addition it does the RC input parsing and auto-selecting the method. Supported methods are:
- PPM
- SBUS
- 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
@@ -3589,7 +2977,6 @@ mixer files.
PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start the fmu if not running already");
PRINT_MODULE_USAGE_COMMAND("mode_gpio");
PRINT_MODULE_USAGE_COMMAND_DESCR("mode_rcin", "Only do RC input, no PWM outputs");
PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "Select all available pins as PWM");
#if defined(BOARD_HAS_PWM)
PRINT_MODULE_USAGE_COMMAND("mode_pwm1");
@@ -3604,7 +2991,6 @@ mixer files.
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
PRINT_MODULE_USAGE_COMMAND("mode_pwm6");
#endif
PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a DSM bind command (module must be running)");
PRINT_MODULE_USAGE_COMMAND_DESCR("sensor_reset", "Do a sensor reset (SPI bus)");
PRINT_MODULE_USAGE_ARG("<ms>", "Delay time in ms between reset and re-enabling", true);
@@ -3630,11 +3016,6 @@ int PX4FMU::print_status()
PX4_INFO("Max update rate: %i Hz", _current_update_rate);
}
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
const char *mode_str = nullptr;
switch (_mode) {