aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/px4io/px4io.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/px4io/px4io.cpp')
-rw-r--r--src/drivers/px4io/px4io.cpp161
1 files changed, 103 insertions, 58 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 78d1d3e63..5fff1feac 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -241,7 +241,8 @@ private:
volatile int _task; ///<worker task id
volatile bool _task_should_exit; ///<worker terminate flag
- int _mavlink_fd; ///<mavlink file descriptor
+ 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.
perf_counter_t _perf_update; ///<local performance counter
@@ -254,6 +255,7 @@ private:
int _t_actuator_armed; ///< system armed control topic
int _t_vehicle_control_mode;///< vehicle control mode topic
int _t_param; ///< parameter update topic
+ int _t_vehicle_command; ///< vehicle command topic
/* advertised topics */
orb_advert_t _to_input_rc; ///< rc inputs from io
@@ -274,6 +276,7 @@ private:
bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
+
/**
* Trampoline to the worker task
*/
@@ -389,7 +392,7 @@ private:
/**
* Send mixer definition text to IO
*/
- int mixer_send(const char *buf, unsigned buflen);
+ int mixer_send(const char *buf, unsigned buflen, unsigned retries=3);
/**
* Handle a status update from IO.
@@ -409,6 +412,13 @@ private:
*/
int io_handle_alarms(uint16_t alarms);
+ /**
+ * Handle issuing dsm bind ioctl to px4io.
+ *
+ * @param dsmMode 0:dsm2, 1:dsmx
+ */
+ void dsm_bind_ioctl(int dsmMode);
+
};
@@ -433,6 +443,7 @@ PX4IO::PX4IO(device::Device *interface) :
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
+ _thread_mavlink_fd(-1),
_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
_status(0),
_alarms(0),
@@ -440,6 +451,7 @@ PX4IO::PX4IO(device::Device *interface) :
_t_actuator_armed(-1),
_t_vehicle_control_mode(-1),
_t_param(-1),
+ _t_vehicle_command(-1),
_to_input_rc(0),
_to_actuators_effective(0),
_to_outputs(0),
@@ -710,10 +722,10 @@ void
PX4IO::task_main()
{
hrt_abstime last_poll_time = 0;
- int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
log("starting");
+ _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
@@ -732,16 +744,20 @@ PX4IO::task_main()
_t_param = orb_subscribe(ORB_ID(parameter_update));
orb_set_interval(_t_param, 500); /* 2Hz update rate max. */
+ _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
+ orb_set_interval(_t_param, 1000); /* 1Hz update rate max. */
+
if ((_t_actuators < 0) ||
(_t_actuator_armed < 0) ||
(_t_vehicle_control_mode < 0) ||
- (_t_param < 0)) {
+ (_t_param < 0) ||
+ (_t_vehicle_command < 0)) {
log("subscription(s) failed");
goto out;
}
/* poll descriptor */
- pollfd fds[4];
+ pollfd fds[5];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_actuator_armed;
@@ -750,8 +766,10 @@ PX4IO::task_main()
fds[2].events = POLLIN;
fds[3].fd = _t_param;
fds[3].events = POLLIN;
+ fds[4].fd = _t_vehicle_command;
+ fds[4].events = POLLIN;
- debug("ready");
+ log("ready");
/* lock against the ioctl handler */
lock();
@@ -791,6 +809,16 @@ PX4IO::task_main()
if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN))
io_set_arming_state();
+ /* if we have a vehicle command, handle it */
+ if (fds[4].revents & POLLIN) {
+ struct vehicle_command_s cmd;
+ orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
+ // Check for a DSM pairing command
+ if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) {
+ dsm_bind_ioctl((int)cmd.param2);
+ }
+ }
+
/*
* If it's time for another tick of the polling status machine,
* try it now.
@@ -825,20 +853,11 @@ PX4IO::task_main()
int32_t dsm_bind_val;
param_t dsm_bind_param;
- // See if bind parameter has been set, and reset it to 0
+ // See if bind parameter has been set, and reset it to -1
param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
- if (dsm_bind_val) {
- if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
- if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
- mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
- ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
- } else {
- mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected");
- }
- } else {
- mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected");
- }
- dsm_bind_val = 0;
+ if (dsm_bind_val >= 0) {
+ dsm_bind_ioctl(dsm_bind_val);
+ dsm_bind_val = -1;
param_set(dsm_bind_param, &dsm_bind_val);
}
@@ -1145,6 +1164,23 @@ PX4IO::io_handle_status(uint16_t status)
return ret;
}
+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%c rx", (dsmMode == 0) ? '2' : 'x');
+ ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES);
+ } else {
+ mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected");
+ }
+ } else {
+ mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected");
+ }
+}
+
+
int
PX4IO::io_handle_alarms(uint16_t alarms)
{
@@ -1432,61 +1468,70 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
}
int
-PX4IO::mixer_send(const char *buf, unsigned buflen)
+PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
{
- uint8_t frame[_max_transfer];
- px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
- unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
- msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
- msg->action = F2I_MIXER_ACTION_RESET;
+ uint8_t frame[_max_transfer];
do {
- unsigned count = buflen;
- if (count > max_len)
- count = max_len;
+ px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+ unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
- if (count > 0) {
- memcpy(&msg->text[0], buf, count);
- buf += count;
- buflen -= count;
- }
+ msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
+ msg->action = F2I_MIXER_ACTION_RESET;
- /*
- * We have to send an even number of bytes. This
- * will only happen on the very last transfer of a
- * mixer, and we are guaranteed that there will be
- * space left to round up as _max_transfer will be
- * even.
- */
- unsigned total_len = sizeof(px4io_mixdata) + count;
- if (total_len % 1) {
- msg->text[count] = '\0';
- total_len++;
- }
+ do {
+ unsigned count = buflen;
- int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+ if (count > max_len)
+ count = max_len;
- if (ret) {
- log("mixer send error %d", ret);
- return ret;
- }
+ if (count > 0) {
+ memcpy(&msg->text[0], buf, count);
+ buf += count;
+ buflen -= count;
+ }
- msg->action = F2I_MIXER_ACTION_APPEND;
+ /*
+ * We have to send an even number of bytes. This
+ * will only happen on the very last transfer of a
+ * mixer, and we are guaranteed that there will be
+ * space left to round up as _max_transfer will be
+ * even.
+ */
+ unsigned total_len = sizeof(px4io_mixdata) + count;
+ if (total_len % 1) {
+ msg->text[count] = '\0';
+ total_len++;
+ }
- } while (buflen > 0);
+ int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+
+ if (ret) {
+ log("mixer send error %d", ret);
+ return ret;
+ }
+
+ msg->action = F2I_MIXER_ACTION_APPEND;
+
+ } while (buflen > 0);
+
+ retries--;
+
+ log("mixer sent");
+
+ } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)));
/* check for the mixer-OK flag */
if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
- debug("mixer upload OK");
mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
return 0;
- } else {
- debug("mixer rejected by IO");
- mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
}
+ log("mixer rejected by IO");
+ mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
+
/* load must have failed for some reason */
return -EINVAL;
}
@@ -1999,9 +2044,9 @@ bind(int argc, char *argv[])
errx(0, "needs argument, use dsm2 or dsmx");
if (!strcmp(argv[2], "dsm2"))
- pulses = 3;
+ pulses = DSM2_BIND_PULSES;
else if (!strcmp(argv[2], "dsmx"))
- pulses = 7;
+ pulses = DSMX_BIND_PULSES;
else
errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-31 20:50:02 +0000