diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-16 10:57:09 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-16 10:57:09 +0100 |
commit | 978cfdccce9148ebb2282646b609d5eb7c59f487 (patch) | |
tree | 641a87b2d0f21630fee266ce2bc3dccfa6e13bc8 /src/drivers/px4io/px4io.cpp | |
parent | ded8cc6e14c7ec42d2a0e08b83c1510f213bf55d (diff) | |
download | px4-firmware-978cfdccce9148ebb2282646b609d5eb7c59f487.tar.gz px4-firmware-978cfdccce9148ebb2282646b609d5eb7c59f487.tar.bz2 px4-firmware-978cfdccce9148ebb2282646b609d5eb7c59f487.zip |
Teach IO driver how to disable lockdown mode
Diffstat (limited to 'src/drivers/px4io/px4io.cpp')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 99 |
1 files changed, 89 insertions, 10 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ca35d2f2..658bcd8d8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -273,6 +273,7 @@ private: actuator_outputs_s _outputs; ///<mixed outputs bool _primary_pwm_device; ///< true if we are the default PWM output + bool _lockdown_override; ///< allow to override the safety lockdown float _battery_amp_per_volt; ///< current sensor amps/volt float _battery_amp_bias; ///< current sensor bias @@ -489,6 +490,7 @@ PX4IO::PX4IO(device::Device *interface) : _to_servorail(0), _to_safety(0), _primary_pwm_device(false), + _lockdown_override(false), _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), @@ -1043,13 +1045,19 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - if (armed.armed && !armed.lockdown) { + if (armed.armed) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } + if (armed.lockdown && !_lockdown_override) { + set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } else { + clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -1830,14 +1838,15 @@ PX4IO::print_status() /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s%s\n", arming, - ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), - ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), - ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : "")); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), @@ -2034,6 +2043,14 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) *(unsigned *)arg = _max_actuators; break; + case PWM_SERVO_SET_DISABLE_LOCKDOWN: + _lockdown_override = arg; + break; + + case PWM_SERVO_GET_DISABLE_LOCKDOWN: + *(unsigned *)arg = _lockdown_override; + break; + case DSM_BIND_START: io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); usleep(500000); @@ -2657,13 +2674,72 @@ void if_test(unsigned mode) { device::Device *interface = get_interface(); + int result; - int result = interface->ioctl(1, mode); /* XXX magic numbers */ - delete interface; + if (interface) { + result = interface->ioctl(1, mode); /* XXX magic numbers */ + delete interface; + } else { + errx(1, "interface not loaded, exiting"); + } errx(0, "test returned %d", result); } +void +lockdown(int argc, char *argv[]) +{ + if (g_dev != nullptr) { + + if (argc > 2 && !strcmp(argv[2], "disable")) { + + warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?"); + warnx("Press 'y' to enable, any other key to abort."); + + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + hrt_abstime start = hrt_absolute_time(); + const unsigned long timeout = 5000000; + + while (hrt_elapsed_time(&start) < timeout) { + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + if (c != 'y') { + exit(0); + } else if (c == 'y') { + break; + } + } + + usleep(10000); + } + + if (hrt_elapsed_time(&start) > timeout) + errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES."); + + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); + + warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!"); + } else { + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); + warnx("ACTUATORS ARE NOW SAFE IN HIL."); + } + + } else { + errx(1, "driver not loaded, exiting"); + } + exit(0); +} + } /* namespace */ int @@ -2883,6 +2959,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); + if (!strcmp(argv[1], "lockdown")) + lockdown(argc, argv); + out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); } |