forked from Archive/PX4-Autopilot
Teach IO driver how to disable lockdown mode
This commit is contained in:
parent
ded8cc6e14
commit
978cfdccce
|
@ -273,6 +273,7 @@ private:
|
||||||
actuator_outputs_s _outputs; ///<mixed outputs
|
actuator_outputs_s _outputs; ///<mixed outputs
|
||||||
|
|
||||||
bool _primary_pwm_device; ///< true if we are the default PWM output
|
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_per_volt; ///< current sensor amps/volt
|
||||||
float _battery_amp_bias; ///< current sensor bias
|
float _battery_amp_bias; ///< current sensor bias
|
||||||
|
@ -489,6 +490,7 @@ PX4IO::PX4IO(device::Device *interface) :
|
||||||
_to_servorail(0),
|
_to_servorail(0),
|
||||||
_to_safety(0),
|
_to_safety(0),
|
||||||
_primary_pwm_device(false),
|
_primary_pwm_device(false),
|
||||||
|
_lockdown_override(false),
|
||||||
_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
|
_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
|
||||||
_battery_amp_bias(0),
|
_battery_amp_bias(0),
|
||||||
_battery_mamphour_total(0),
|
_battery_mamphour_total(0),
|
||||||
|
@ -1043,13 +1045,19 @@ PX4IO::io_set_arming_state()
|
||||||
uint16_t set = 0;
|
uint16_t set = 0;
|
||||||
uint16_t clear = 0;
|
uint16_t clear = 0;
|
||||||
|
|
||||||
if (armed.armed && !armed.lockdown) {
|
if (armed.armed) {
|
||||||
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
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) {
|
if (armed.ready_to_arm) {
|
||||||
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
||||||
|
|
||||||
|
@ -1830,14 +1838,15 @@ PX4IO::print_status()
|
||||||
/* setup and state */
|
/* setup and state */
|
||||||
printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
|
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);
|
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,
|
||||||
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
|
((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_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_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
|
||||||
((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
|
((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_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
|
||||||
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""));
|
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
|
||||||
|
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""));
|
||||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||||
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
|
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
|
||||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
|
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;
|
*(unsigned *)arg = _max_actuators;
|
||||||
break;
|
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:
|
case DSM_BIND_START:
|
||||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
|
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
|
||||||
usleep(500000);
|
usleep(500000);
|
||||||
|
@ -2657,13 +2674,72 @@ void
|
||||||
if_test(unsigned mode)
|
if_test(unsigned mode)
|
||||||
{
|
{
|
||||||
device::Device *interface = get_interface();
|
device::Device *interface = get_interface();
|
||||||
|
int result;
|
||||||
|
|
||||||
int result = interface->ioctl(1, mode); /* XXX magic numbers */
|
if (interface) {
|
||||||
delete interface;
|
result = interface->ioctl(1, mode); /* XXX magic numbers */
|
||||||
|
delete interface;
|
||||||
|
} else {
|
||||||
|
errx(1, "interface not loaded, exiting");
|
||||||
|
}
|
||||||
|
|
||||||
errx(0, "test returned %d", result);
|
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 */
|
} /* namespace */
|
||||||
|
|
||||||
int
|
int
|
||||||
|
@ -2883,6 +2959,9 @@ px4io_main(int argc, char *argv[])
|
||||||
if (!strcmp(argv[1], "bind"))
|
if (!strcmp(argv[1], "bind"))
|
||||||
bind(argc, argv);
|
bind(argc, argv);
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "lockdown"))
|
||||||
|
lockdown(argc, argv);
|
||||||
|
|
||||||
out:
|
out:
|
||||||
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
|
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue