diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-16 16:22:38 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-16 16:22:38 +0200 |
commit | 52d15ac7b1a90ededd0079ec17f0bc709947d7de (patch) | |
tree | 0773a9a9252d810c692fc0759feb1910ac801b89 /src/drivers | |
parent | 0dc240688dcae83e34f9001072ac151f4a3df140 (diff) | |
parent | 6113be111e84a57715f3f3bfe81077bf1b267e52 (diff) | |
download | px4-firmware-52d15ac7b1a90ededd0079ec17f0bc709947d7de.tar.gz px4-firmware-52d15ac7b1a90ededd0079ec17f0bc709947d7de.tar.bz2 px4-firmware-52d15ac7b1a90ededd0079ec17f0bc709947d7de.zip |
Merged public master
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/drv_pwm_output.h | 5 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 63 |
2 files changed, 36 insertions, 32 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 52a667403..ec9d4ca09 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -118,11 +118,8 @@ ORB_DECLARE(output_pwm); /** start DSM bind */ #define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) -/** stop DSM bind */ -#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8) - /** Power up DSM receiver */ -#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9) +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ae5d6ce19..ed98ff0a5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -195,6 +195,12 @@ private: bool _dsm_vcc_ctl; /** + * System armed + */ + + bool _system_armed; + + /** * Trampoline to the worker task */ static void task_main_trampoline(int argc, char *argv[]); @@ -363,7 +369,8 @@ PX4IO::PX4IO(device::Device *interface) : _battery_amp_bias(0), _battery_mamphour_total(0), _battery_last_timestamp(0), - _dsm_vcc_ctl(false) + _dsm_vcc_ctl(false), + _system_armed(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -580,9 +587,11 @@ void PX4IO::task_main() { hrt_abstime last_poll_time = 0; + int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); log("starting"); + /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. @@ -690,6 +699,25 @@ PX4IO::task_main() */ if (fds[3].revents & POLLIN) { parameter_update_s pupdate; + int32_t dsm_bind_val; + param_t dsm_bind_param; + + // See if bind parameter has been set, and reset it to 0 + param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); + if (dsm_bind_val) { + if (!_system_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; + param_set(dsm_bind_param, &dsm_bind_val); + } /* copy to reset the notification */ orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); @@ -756,6 +784,8 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; + _system_armed = vstatus.flag_system_armed; + if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { @@ -1452,16 +1482,11 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) 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); - usleep(1000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(100000); + usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); - break; - - case DSM_BIND_STOP: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); - usleep(500000); break; case DSM_BIND_POWER_UP: @@ -1736,30 +1761,12 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - errx(1, "failed opening console"); - warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); - warnx("Press CTRL-C or 'c' when done."); g_dev->ioctl(nullptr, DSM_BIND_START, pulses); - for (;;) { - usleep(500000L); - /* Check if user wants to quit */ - char c; - if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { - warnx("Done\n"); - g_dev->ioctl(nullptr, DSM_BIND_STOP, 0); - g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); - close(console); - exit(0); - } - } - } + exit(0); + } void |