drivers/rc_input: port to linux for testing

This commit is contained in:
Daniel Agar 2020-01-09 15:34:57 -05:00
parent 6184f4691a
commit e3de7e62ea
11 changed files with 30 additions and 34 deletions

View File

@ -32,6 +32,7 @@ px4_add_board(
#magnetometer # all available magnetometer drivers
magnetometer/hmc5883
pwm_out_sim
rc_input
#telemetry # all available telemetry drivers
DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers
mpu9250

View File

@ -57,6 +57,7 @@ px4_add_board(
#magnetometer # all available magnetometer drivers
pwm_out_sim
qshell/posix
rc_input
#telemetry # all available telemetry drivers
MODULES
airspeed_selector

View File

@ -57,6 +57,7 @@ px4_add_board(
#magnetometer # all available magnetometer drivers
pwm_out_sim
qshell/posix
rc_input
#telemetry # all available telemetry drivers
MODULES
airspeed_selector

View File

@ -24,6 +24,7 @@ px4_add_board(
#magnetometer # all available magnetometer drivers
magnetometer/hmc5883
pwm_out_sim
rc_input
#telemetry # all available telemetry drivers
DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers
mpu9250

View File

@ -26,6 +26,7 @@ px4_add_board(
magnetometer/hmc5883
magnetometer/lsm9ds1_mag
pwm_out_sim
rc_input
#telemetry # all available telemetry drivers
DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers
isl29501

View File

@ -23,6 +23,7 @@ px4_add_board(
#magnetometer # all available magnetometer drivers
magnetometer/hmc5883
pwm_out_sim
rc_input
rpi_rc_in
#telemetry # all available telemetry drivers
MODULES

View File

@ -79,6 +79,7 @@ sleep 1
linux_sbus start -d /dev/ttyS4 -c 16
# DSM2 port is mapped to /dev/ttyS4 (default for linux_sbus)
#rc_input start -d /dev/ttyS4
# default: ROMFS/px4fmu_common/mixers/quad_x.main.mix, 8 output channels
linux_pwm_out start -p bbblue_rc -m ROMFS/px4fmu_common/mixers/quad_x.main.mix

View File

@ -78,6 +78,7 @@ sleep 1
linux_sbus start -d /dev/ttyS4 -c 16
# DSM2 port is mapped to /dev/ttyS4 (default for linux_sbus)
#rc_input start -d /dev/ttyS4
# default: ROMFS/px4fmu_common/mixers/quad_x.main.mix, 8 output channels
#linux_pwm_out start -p bbblue_rc -m ROMFS/px4fmu_common/mixers/quad_x.main.mix

View File

@ -40,6 +40,8 @@ mavlink stream -d /dev/ttyPS1 -s HIGHRES_IMU -r 50
mavlink stream -d /dev/ttyPS1 -s ATTITUDE -r 50
linux_sbus start -d /dev/ttyS2 -c 10
#rc_input start -d /dev/ttyS2
linux_pwm_out start -p ocpoc_mmap -m ROMFS/px4fmu_common/mixers/ocpoc_quad_x.main.mix
logger start -t -b 200
mavlink boot_complete

View File

@ -59,21 +59,15 @@ RCInput::RCInput(const char *device) :
_raw_rc_values[i] = UINT16_MAX;
}
#ifdef RC_SERIAL_PORT
if (device) {
strncpy(_device, device, sizeof(_device));
_device[sizeof(_device) - 1] = '\0';
}
#endif
}
RCInput::~RCInput()
{
#ifdef RC_SERIAL_PORT
dsm_deinit();
#endif
delete _crsf_telemetry;
@ -84,12 +78,10 @@ RCInput::~RCInput()
int
RCInput::init()
{
#ifdef RC_SERIAL_PORT
# ifdef RF_RADIO_POWER_CONTROL
#ifdef RF_RADIO_POWER_CONTROL
// power radio on
RF_RADIO_POWER_CONTROL(true);
# endif
#endif // RF_RADIO_POWER_CONTROL
// dsm_init sets some file static variables and returns a file descriptor
_rcs_fd = dsm_init(_device);
@ -99,18 +91,20 @@ RCInput::init()
}
if (board_rc_swap_rxtx(_device)) {
#if defined(TIOCSSWAP)
ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
#endif // TIOCSSWAP
}
// 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_rc_singlewire(_device));
# ifdef GPIO_PPM_IN
#ifdef GPIO_PPM_IN
// disable CPPM input by mapping it away from the timer capture input
px4_arch_unconfiggpio(GPIO_PPM_IN);
# endif
#endif
#endif // GPIO_PPM_IN
return 0;
}
@ -123,7 +117,10 @@ RCInput::task_spawn(int argc, char *argv[])
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
const char *device = RC_SERIAL_PORT;
const char *device = nullptr;
#if defined(RC_SERIAL_PORT)
device = RC_SERIAL_PORT;
#endif // RC_SERIAL_PORT
while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
@ -146,6 +143,11 @@ RCInput::task_spawn(int argc, char *argv[])
return -1;
}
if (device == nullptr) {
PX4_ERR("valid device required");
return PX4_ERROR;
}
RCInput *instance = new RCInput(device);
if (instance == nullptr) {
@ -226,7 +228,6 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
_rc_in.rc_total_frame_count = 0;
}
#ifdef RC_SERIAL_PORT
void RCInput::set_rc_scan_state(RC_SCAN newState)
{
PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
@ -239,10 +240,11 @@ void RCInput::rc_io_invert(bool invert)
// First check if the board provides a board-specific inversion method (e.g. via GPIO),
// and if not use an IOCTL
if (!board_rc_invert_input(_device, invert)) {
#if defined(TIOCSINVERT)
ioctl(_rcs_fd, TIOCSINVERT, invert ? (SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX) : 0);
#endif // TIOCSINVERT
}
}
#endif
void RCInput::Run()
{
@ -329,7 +331,6 @@ void RCInput::Run()
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;
@ -590,21 +591,6 @@ void RCInput::Run()
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
perf_end(_cycle_perf);
if (rc_updated) {

View File

@ -56,7 +56,7 @@ using namespace time_literals;
#define SBUS_DEBUG_LEVEL 0 /* Set debug output level */
#if defined(__PX4_POSIX_OCPOC)
#if defined(__PX4_LINUX)
#include <sys/ioctl.h>
#include <linux/serial_core.h>
#endif
@ -152,7 +152,7 @@ sbus_init(const char *device, bool singlewire)
int
sbus_config(int sbus_fd, bool singlewire)
{
#if defined(__PX4_POSIX_OCPOC)
#if defined(__PX4_LINUX)
struct termios options;
if (tcgetattr(sbus_fd, &options) != 0) {