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 # all available magnetometer drivers
magnetometer/hmc5883 magnetometer/hmc5883
pwm_out_sim pwm_out_sim
rc_input
#telemetry # all available telemetry drivers #telemetry # all available telemetry drivers
DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers
mpu9250 mpu9250

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@@ -79,6 +79,7 @@ sleep 1
linux_sbus start -d /dev/ttyS4 -c 16 linux_sbus start -d /dev/ttyS4 -c 16
# DSM2 port is mapped to /dev/ttyS4 (default for linux_sbus) # 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 # 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 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 linux_sbus start -d /dev/ttyS4 -c 16
# DSM2 port is mapped to /dev/ttyS4 (default for linux_sbus) # 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 # 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 #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 mavlink stream -d /dev/ttyPS1 -s ATTITUDE -r 50
linux_sbus start -d /dev/ttyS2 -c 10 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 linux_pwm_out start -p ocpoc_mmap -m ROMFS/px4fmu_common/mixers/ocpoc_quad_x.main.mix
logger start -t -b 200 logger start -t -b 200
mavlink boot_complete mavlink boot_complete

View File

@@ -59,21 +59,15 @@ RCInput::RCInput(const char *device) :
_raw_rc_values[i] = UINT16_MAX; _raw_rc_values[i] = UINT16_MAX;
} }
#ifdef RC_SERIAL_PORT
if (device) { if (device) {
strncpy(_device, device, sizeof(_device)); strncpy(_device, device, sizeof(_device));
_device[sizeof(_device) - 1] = '\0'; _device[sizeof(_device) - 1] = '\0';
} }
#endif
} }
RCInput::~RCInput() RCInput::~RCInput()
{ {
#ifdef RC_SERIAL_PORT
dsm_deinit(); dsm_deinit();
#endif
delete _crsf_telemetry; delete _crsf_telemetry;
@@ -84,12 +78,10 @@ RCInput::~RCInput()
int int
RCInput::init() RCInput::init()
{ {
#ifdef RC_SERIAL_PORT #ifdef RF_RADIO_POWER_CONTROL
# ifdef RF_RADIO_POWER_CONTROL
// power radio on // power radio on
RF_RADIO_POWER_CONTROL(true); RF_RADIO_POWER_CONTROL(true);
# endif #endif // RF_RADIO_POWER_CONTROL
// dsm_init sets some file static variables and returns a file descriptor // dsm_init sets some file static variables and returns a file descriptor
_rcs_fd = dsm_init(_device); _rcs_fd = dsm_init(_device);
@@ -99,18 +91,20 @@ RCInput::init()
} }
if (board_rc_swap_rxtx(_device)) { if (board_rc_swap_rxtx(_device)) {
#if defined(TIOCSSWAP)
ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED); ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
#endif // TIOCSSWAP
} }
// assume SBUS input and immediately switch it to // assume SBUS input and immediately switch it to
// so that if Single wire mode on TX there will be only // so that if Single wire mode on TX there will be only
// a short contention // a short contention
sbus_config(_rcs_fd, board_rc_singlewire(_device)); 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 // disable CPPM input by mapping it away from the timer capture input
px4_arch_unconfiggpio(GPIO_PPM_IN); px4_arch_unconfiggpio(GPIO_PPM_IN);
# endif #endif // GPIO_PPM_IN
#endif
return 0; return 0;
} }
@@ -123,7 +117,10 @@ RCInput::task_spawn(int argc, char *argv[])
int myoptind = 1; int myoptind = 1;
int ch; int ch;
const char *myoptarg = nullptr; 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) { while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) { switch (ch) {
@@ -146,6 +143,11 @@ RCInput::task_spawn(int argc, char *argv[])
return -1; return -1;
} }
if (device == nullptr) {
PX4_ERR("valid device required");
return PX4_ERROR;
}
RCInput *instance = new RCInput(device); RCInput *instance = new RCInput(device);
if (instance == nullptr) { if (instance == nullptr) {
@@ -226,7 +228,6 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
_rc_in.rc_total_frame_count = 0; _rc_in.rc_total_frame_count = 0;
} }
#ifdef RC_SERIAL_PORT
void RCInput::set_rc_scan_state(RC_SCAN newState) 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]); 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), // First check if the board provides a board-specific inversion method (e.g. via GPIO),
// and if not use an IOCTL // and if not use an IOCTL
if (!board_rc_invert_input(_device, invert)) { if (!board_rc_invert_input(_device, invert)) {
#if defined(TIOCSINVERT)
ioctl(_rcs_fd, TIOCSINVERT, invert ? (SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX) : 0); ioctl(_rcs_fd, TIOCSINVERT, invert ? (SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX) : 0);
#endif // TIOCSINVERT
} }
} }
#endif
void RCInput::Run() void RCInput::Run()
{ {
@@ -329,7 +331,6 @@ void RCInput::Run()
bool rc_updated = false; bool rc_updated = false;
#ifdef RC_SERIAL_PORT
// This block scans for a supported serial RC input and locks onto the first one found // This block scans for a supported serial RC input and locks onto the first one found
// Scan for 300 msec, then switch protocol // Scan for 300 msec, then switch protocol
constexpr hrt_abstime rc_scan_max = 300_ms; constexpr hrt_abstime rc_scan_max = 300_ms;
@@ -590,21 +591,6 @@ void RCInput::Run()
break; 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); perf_end(_cycle_perf);
if (rc_updated) { if (rc_updated) {

View File

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