Daniel Agar
7 years ago
21 changed files with 1027 additions and 640 deletions
@ -0,0 +1,43 @@
@@ -0,0 +1,43 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2015 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_input |
||||
MAIN rc_input |
||||
STACK_MAIN 1200 |
||||
COMPILE_FLAGS |
||||
SRCS |
||||
RCInput.cpp |
||||
crsf_telemetry.cpp |
||||
DEPENDS |
||||
rc |
||||
) |
||||
@ -0,0 +1,799 @@
@@ -0,0 +1,799 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2012-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 "RCInput.hpp" |
||||
|
||||
#include "crsf_telemetry.h" |
||||
|
||||
using namespace time_literals; |
||||
|
||||
#define SCHEDULE_INTERVAL 4000 /**< The schedule interval in usec (250 Hz) */ |
||||
|
||||
#if defined(SPEKTRUM_POWER) |
||||
static bool bind_spektrum(int arg); |
||||
#endif /* SPEKTRUM_POWER */ |
||||
|
||||
work_s RCInput::_work = {}; |
||||
|
||||
RCInput::RCInput(bool run_as_task) |
||||
{ |
||||
// 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; |
||||
} |
||||
} |
||||
|
||||
RCInput::~RCInput() |
||||
{ |
||||
orb_unadvertise(_to_input_rc); |
||||
|
||||
orb_unsubscribe(_adc_sub); |
||||
orb_unsubscribe(_vehicle_cmd_sub); |
||||
|
||||
#ifdef RC_SERIAL_PORT |
||||
dsm_deinit(); |
||||
#endif |
||||
|
||||
if (_crsf_telemetry) { |
||||
delete (_crsf_telemetry); |
||||
} |
||||
} |
||||
|
||||
int |
||||
RCInput::init() |
||||
{ |
||||
_adc_sub = orb_subscribe(ORB_ID(adc_report)); |
||||
|
||||
#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 |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
int |
||||
RCInput::task_spawn(int argc, char *argv[]) |
||||
{ |
||||
bool run_as_task = false; |
||||
bool error_flag = false; |
||||
|
||||
int myoptind = 1; |
||||
int ch; |
||||
const char *myoptarg = nullptr; |
||||
|
||||
while ((ch = px4_getopt(argc, argv, "t", &myoptind, &myoptarg)) != EOF) { |
||||
switch (ch) { |
||||
case 't': |
||||
run_as_task = true; |
||||
break; |
||||
|
||||
case '?': |
||||
error_flag = true; |
||||
break; |
||||
|
||||
default: |
||||
PX4_WARN("unrecognized flag"); |
||||
error_flag = true; |
||||
break; |
||||
} |
||||
} |
||||
|
||||
if (error_flag) { |
||||
return -1; |
||||
} |
||||
|
||||
|
||||
if (!run_as_task) { |
||||
|
||||
/* schedule a cycle to start things */ |
||||
int ret = work_queue(HPWORK, &_work, (worker_t)&RCInput::cycle_trampoline, nullptr, 0); |
||||
|
||||
if (ret < 0) { |
||||
return ret; |
||||
} |
||||
|
||||
_task_id = task_id_is_work_queue; |
||||
|
||||
} else { |
||||
|
||||
/* start the IO interface task */ |
||||
|
||||
_task_id = px4_task_spawn_cmd("rc_input", |
||||
SCHED_DEFAULT, |
||||
SCHED_PRIORITY_SLOW_DRIVER, |
||||
1000, |
||||
(px4_main_t)&run_trampoline, |
||||
nullptr); |
||||
|
||||
if (_task_id < 0) { |
||||
_task_id = -1; |
||||
return -errno; |
||||
} |
||||
} |
||||
|
||||
return PX4_OK; |
||||
} |
||||
|
||||
void |
||||
RCInput::cycle_trampoline(void *arg) |
||||
{ |
||||
RCInput *dev = reinterpret_cast<RCInput *>(arg); |
||||
|
||||
// check if the trampoline is called for the first time
|
||||
if (!dev) { |
||||
dev = new RCInput(false); |
||||
|
||||
if (!dev) { |
||||
PX4_ERR("alloc failed"); |
||||
return; |
||||
} |
||||
|
||||
if (dev->init() != 0) { |
||||
PX4_ERR("init failed"); |
||||
delete dev; |
||||
return; |
||||
} |
||||
|
||||
_object = dev; |
||||
} |
||||
|
||||
dev->cycle(); |
||||
} |
||||
|
||||
void |
||||
RCInput::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 RCInput::set_rc_scan_state(RC_SCAN newState) |
||||
{ |
||||
// PX4_WARN("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
|
||||
_rc_scan_begin = 0; |
||||
_rc_scan_state = newState; |
||||
} |
||||
|
||||
void RCInput::rc_io_invert(bool invert, uint32_t uxart_base) |
||||
{ |
||||
INVERT_RC_INPUT(invert, uxart_base); |
||||
} |
||||
#endif |
||||
|
||||
void |
||||
RCInput::run() |
||||
{ |
||||
if (init() != 0) { |
||||
PX4_ERR("init failed"); |
||||
exit_and_cleanup(); |
||||
return; |
||||
} |
||||
|
||||
cycle(); |
||||
} |
||||
|
||||
void |
||||
RCInput::cycle() |
||||
{ |
||||
while (true) { |
||||
|
||||
_cycle_timestamp = hrt_absolute_time(); |
||||
|
||||
#if defined(SPEKTRUM_POWER) |
||||
/* vehicle command */ |
||||
bool vehicle_command_updated = false; |
||||
orb_check(_vehicle_cmd_sub, &vehicle_command_updated); |
||||
|
||||
if (vehicle_command_updated) { |
||||
vehicle_command_s vcmd = {}; |
||||
orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub, &vcmd); |
||||
|
||||
// Check for a pairing command
|
||||
if ((unsigned int)vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) { |
||||
if (!_rc_scan_locked /* !_armed.armed */) { // TODO: add armed check?
|
||||
if ((int)vcmd.param1 == 0) { |
||||
// DSM binding command
|
||||
int dsm_bind_mode = (int)vcmd.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; |
||||
} |
||||
|
||||
bind_spektrum(dsm_bind_pulses); |
||||
} |
||||
|
||||
} else { |
||||
PX4_WARN("system armed, bind request rejected"); |
||||
} |
||||
} |
||||
} |
||||
|
||||
#endif /* SPEKTRUM_POWER */ |
||||
|
||||
/* update ADC sampling */ |
||||
#ifdef ADC_RC_RSSI_CHANNEL |
||||
bool adc_updated = false; |
||||
orb_check(_adc_sub, &adc_updated); |
||||
|
||||
if (adc_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 /* ADC_RC_RSSI_CHANNEL */ |
||||
|
||||
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_ms; |
||||
|
||||
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]);
|
||||
} |
||||
|
||||
int newBytes = 0; |
||||
|
||||
if (_run_as_task) { |
||||
// TODO: needs work (poll _rcs_fd)
|
||||
// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
|
||||
// then update priority to SCHED_PRIORITY_FAST_DRIVER
|
||||
// read all available data from the serial RC input UART
|
||||
newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); |
||||
|
||||
} else { |
||||
// read all available data from the serial RC input UART
|
||||
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 /* CONFIG_ARCH_BOARD_OMNIBUS_F4SD */ |
||||
|
||||
_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; |
||||
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) > 1_s)) { |
||||
_rc_scan_locked = false; |
||||
} |
||||
|
||||
if (_run_as_task) { |
||||
if (should_exit()) { |
||||
break; |
||||
} |
||||
|
||||
} else { |
||||
if (should_exit()) { |
||||
exit_and_cleanup(); |
||||
|
||||
} else { |
||||
/* schedule next cycle */ |
||||
work_queue(HPWORK, &_work, (worker_t)&RCInput::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL)); |
||||
} |
||||
|
||||
break; |
||||
} |
||||
} |
||||
} |
||||
|
||||
#if defined(SPEKTRUM_POWER) |
||||
bool bind_spektrum(int arg) |
||||
{ |
||||
int ret = PX4_ERROR; |
||||
|
||||
/* specify 11ms DSMX. RX will automatically fall back to 22ms or DSM2 if necessary */ |
||||
|
||||
/* 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; |
||||
} |
||||
|
||||
return (ret == PX4_OK); |
||||
} |
||||
#endif /* SPEKTRUM_POWER */ |
||||
|
||||
RCInput *RCInput::instantiate(int argc, char *argv[]) |
||||
{ |
||||
// No arguments to parse. We also know that we should run as task
|
||||
return new RCInput(true); |
||||
} |
||||
|
||||
int RCInput::custom_command(int argc, char *argv[]) |
||||
{ |
||||
#if defined(SPEKTRUM_POWER) |
||||
const char *verb = argv[0]; |
||||
|
||||
if (!strcmp(verb, "bind")) { |
||||
bind_spektrum(DSMX8_BIND_PULSES); |
||||
return 0; |
||||
} |
||||
|
||||
#endif /* SPEKTRUM_POWER */ |
||||
|
||||
/* start the FMU if not running */ |
||||
if (!is_running()) { |
||||
int ret = RCInput::task_spawn(argc, argv); |
||||
|
||||
if (ret) { |
||||
return ret; |
||||
} |
||||
} |
||||
|
||||
return print_usage("unknown command"); |
||||
} |
||||
|
||||
int RCInput::print_usage(const char *reason) |
||||
{ |
||||
if (reason) { |
||||
PX4_WARN("%s\n", reason); |
||||
} |
||||
|
||||
PRINT_MODULE_DESCRIPTION( |
||||
R"DESCR_STR( |
||||
### Description |
||||
This module does the RC input parsing and auto-selecting the method. Supported methods are: |
||||
- PPM |
||||
- SBUS |
||||
- DSM |
||||
- SUMD |
||||
- ST24 |
||||
- TBS Crossfire (CRSF) |
||||
|
||||
### Implementation |
||||
By default the module runs on the work queue, to reduce RAM usage. It can also be run in its own thread, |
||||
specified via start flag -t, to reduce latency. |
||||
When running on the work queue, it schedules at a fixed frequency. |
||||
)DESCR_STR"); |
||||
|
||||
PRINT_MODULE_USAGE_NAME("rc_input", "driver"); |
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)"); |
||||
PRINT_MODULE_USAGE_PARAM_FLAG('t', "Run as separate task instead of the work queue", true); |
||||
|
||||
#if defined(SPEKTRUM_POWER) |
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a DSM bind command (module must be running)"); |
||||
#endif /* SPEKTRUM_POWER */ |
||||
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
int RCInput::print_status() |
||||
{ |
||||
PX4_INFO("Running %s", (_run_as_task ? "as task" : "on work queue")); |
||||
|
||||
if (!_run_as_task) { |
||||
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 |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
extern "C" __EXPORT int rc_input_main(int argc, char *argv[]); |
||||
|
||||
int |
||||
rc_input_main(int argc, char *argv[]) |
||||
{ |
||||
return RCInput::main(argc, argv); |
||||
} |
||||
@ -0,0 +1,155 @@
@@ -0,0 +1,155 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2012-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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <cfloat> |
||||
|
||||
#include <board_config.h> |
||||
#include <drivers/drv_hrt.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> |
||||
#include <px4_module.h> |
||||
#include <px4_workqueue.h> |
||||
#include <uORB/topics/adc_report.h> |
||||
#include <uORB/topics/input_rc.h> |
||||
#include <uORB/topics/vehicle_command.h> |
||||
|
||||
#include "crsf_telemetry.h" |
||||
|
||||
#ifdef HRT_PPM_CHANNEL |
||||
# include <systemlib/ppm_decode.h> |
||||
#endif |
||||
|
||||
class RCInput : public ModuleBase<RCInput> |
||||
{ |
||||
public: |
||||
|
||||
RCInput(bool run_as_task); |
||||
virtual ~RCInput(); |
||||
|
||||
/** @see ModuleBase */ |
||||
static int task_spawn(int argc, char *argv[]); |
||||
|
||||
/** @see ModuleBase */ |
||||
static RCInput *instantiate(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::run() */ |
||||
void run() override; |
||||
|
||||
/**
|
||||
* run the main loop: if running as task, continuously iterate, otherwise execute only one single cycle |
||||
*/ |
||||
void cycle(); |
||||
|
||||
/** @see ModuleBase::print_status() */ |
||||
int print_status() override; |
||||
|
||||
int init(); |
||||
|
||||
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" |
||||
}; |
||||
|
||||
hrt_abstime _rc_scan_begin{0}; |
||||
bool _rc_scan_locked{false}; |
||||
bool _report_lock{true}; |
||||
|
||||
hrt_abstime _cycle_timestamp{0}; |
||||
|
||||
unsigned _current_update_rate{0}; |
||||
bool _run_as_task{false}; |
||||
|
||||
static struct work_s _work; |
||||
|
||||
int _vehicle_cmd_sub{-1}; |
||||
int _adc_sub{-1}; |
||||
|
||||
rc_input_values _rc_in{}; |
||||
|
||||
float _analog_rc_rssi_volt{-1.0f}; |
||||
bool _analog_rc_rssi_stable{false}; |
||||
|
||||
orb_advert_t _to_input_rc{nullptr}; |
||||
|
||||
int _rcs_fd{-1}; |
||||
|
||||
uint8_t _rcs_buf[SBUS_BUFFER_SIZE] {}; |
||||
|
||||
uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; |
||||
uint16_t _raw_rc_count{}; |
||||
|
||||
CRSFTelemetry *_crsf_telemetry{nullptr}; |
||||
|
||||
static void cycle_trampoline(void *arg); |
||||
int start(); |
||||
|
||||
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); |
||||
|
||||
}; |
||||
Loading…
Reference in new issue