diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index e2485368f9..b20902efee 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -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 diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index e1fc912e8b..35118c1834 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -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 diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 5deeae1a8c..312f7633ce 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -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 diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index ecbffda85b..cb67c801e1 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -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 diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index f7e0e925ac..f8400a57e8 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -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 diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index d6c55ec15a..90b384a5d1 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -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 diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index 271d893c05..366de5ad6f 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -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 diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 2e135801b4..086ba00492 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -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 diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index 575aa84bf4..2baf757dc3 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -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 diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index e851280fbf..106f6decd4 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -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) { diff --git a/src/lib/rc/sbus.cpp b/src/lib/rc/sbus.cpp index 191c9936b3..469dfcd20a 100644 --- a/src/lib/rc/sbus.cpp +++ b/src/lib/rc/sbus.cpp @@ -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 #include #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) {