forked from Archive/PX4-Autopilot
drivers/rc_input: port to linux for testing
This commit is contained in:
parent
6184f4691a
commit
e3de7e62ea
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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) {
|
||||
|
|
Loading…
Reference in New Issue