forked from Archive/PX4-Autopilot
commit
57216ac6fe
|
@ -160,7 +160,7 @@ ORB_DECLARE(output_pwm);
|
|||
|
||||
#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
|
||||
#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
|
||||
#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
|
||||
#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
|
||||
|
||||
/** power up DSM receiver */
|
||||
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
|
||||
|
|
|
@ -244,8 +244,7 @@ private:
|
|||
volatile int _task; ///< worker task id
|
||||
volatile bool _task_should_exit; ///< worker terminate flag
|
||||
|
||||
int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
|
||||
int _thread_mavlink_fd; ///< mavlink file descriptor for thread.
|
||||
int _mavlink_fd; ///< mavlink file descriptor.
|
||||
|
||||
perf_counter_t _perf_update; ///<local performance counter for status updates
|
||||
perf_counter_t _perf_write; ///<local performance counter for PWM control writes
|
||||
|
@ -474,7 +473,6 @@ PX4IO::PX4IO(device::Device *interface) :
|
|||
_task(-1),
|
||||
_task_should_exit(false),
|
||||
_mavlink_fd(-1),
|
||||
_thread_mavlink_fd(-1),
|
||||
_perf_update(perf_alloc(PC_ELAPSED, "io update")),
|
||||
_perf_write(perf_alloc(PC_ELAPSED, "io write")),
|
||||
_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
|
||||
|
@ -507,9 +505,6 @@ PX4IO::PX4IO(device::Device *interface) :
|
|||
/* we need this potentially before it could be set in task_main */
|
||||
g_dev = this;
|
||||
|
||||
/* open MAVLink text channel */
|
||||
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
_debug_enabled = false;
|
||||
_servorail_status.rssi_v = 0;
|
||||
}
|
||||
|
@ -785,7 +780,7 @@ PX4IO::task_main()
|
|||
hrt_abstime poll_last = 0;
|
||||
hrt_abstime orb_check_last = 0;
|
||||
|
||||
_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
|
||||
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
/*
|
||||
* Subscribe to the appropriate PWM output topic based on whether we are the
|
||||
|
@ -880,6 +875,10 @@ PX4IO::task_main()
|
|||
/* run at 5Hz */
|
||||
orb_check_last = now;
|
||||
|
||||
/* try to claim the MAVLink log FD */
|
||||
if (_mavlink_fd < 0)
|
||||
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
/* check updates on uORB topics and handle it */
|
||||
bool updated = false;
|
||||
|
||||
|
@ -1275,16 +1274,14 @@ void
|
|||
PX4IO::dsm_bind_ioctl(int dsmMode)
|
||||
{
|
||||
if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
|
||||
/* 0: dsm2, 1:dsmx */
|
||||
if ((dsmMode == 0) || (dsmMode == 1)) {
|
||||
mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8"));
|
||||
ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
|
||||
} else {
|
||||
mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected");
|
||||
}
|
||||
mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
|
||||
int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
|
||||
|
||||
if (ret)
|
||||
mavlink_log_critical(_mavlink_fd, "binding failed.");
|
||||
|
||||
} else {
|
||||
mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected");
|
||||
mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected");
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -2115,14 +2112,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
|
|||
break;
|
||||
|
||||
case DSM_BIND_START:
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
|
||||
usleep(500000);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
|
||||
usleep(72000);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
|
||||
usleep(50000);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
|
||||
|
||||
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
|
||||
if (arg == DSM2_BIND_PULSES ||
|
||||
arg == DSMX_BIND_PULSES ||
|
||||
arg == DSMX8_BIND_PULSES) {
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
|
||||
usleep(500000);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
|
||||
usleep(72000);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
|
||||
usleep(50000);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
|
||||
|
||||
ret = OK;
|
||||
} else {
|
||||
ret = -EINVAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case DSM_BIND_POWER_UP:
|
||||
|
@ -2615,7 +2622,7 @@ bind(int argc, char *argv[])
|
|||
#endif
|
||||
|
||||
if (argc < 3)
|
||||
errx(0, "needs argument, use dsm2 or dsmx");
|
||||
errx(0, "needs argument, use dsm2, dsmx or dsmx8");
|
||||
|
||||
if (!strcmp(argv[2], "dsm2"))
|
||||
pulses = DSM2_BIND_PULSES;
|
||||
|
@ -2624,7 +2631,7 @@ bind(int argc, char *argv[])
|
|||
else if (!strcmp(argv[2], "dsmx8"))
|
||||
pulses = DSMX8_BIND_PULSES;
|
||||
else
|
||||
errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
|
||||
errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]);
|
||||
// Test for custom pulse parameter
|
||||
if (argc > 3)
|
||||
pulses = atoi(argv[3]);
|
||||
|
|
|
@ -566,7 +566,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
break;
|
||||
|
||||
case PX4IO_P_SETUP_DSM:
|
||||
dsm_bind(value & 0x0f, (value >> 4) & 7);
|
||||
dsm_bind(value & 0x0f, (value >> 4) & 0xF);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
Loading…
Reference in New Issue