fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #596 from PX4/lockdown_disable

Browse Source 
Lockdown disable
This commit is contained in:
Lorenz Meier 2014-01-30 01:08:50 -08:00
commit 2f267fbf43
6 changed files with 166 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/drivers/drv_pwm_output.h
View File

@ -193,6 +193,20 @@ ORB_DECLARE(output_pwm);
* split between servos and GPIO */
#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20)
/** set the lockdown override flag to enable outputs in HIL */
#define PWM_SERVO_SET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 21)
/** get the lockdown override flag to enable outputs in HIL */
#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22)
/*
*
*
* WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE!
*
*
*/
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

99
src/drivers/px4io/px4io.cpp
View File

@ -276,6 +276,7 @@ private:
servorail_status_s _servorail_status; ///< servorail status
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
@ -493,6 +494,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),
@ -1050,13 +1052,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;
@ -1893,14 +1901,15 @@ PX4IO::print_status()
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
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),
@ -2097,6 +2106,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);
@ -2754,13 +2771,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
@ -2994,6 +3070,9 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "bind"))
bind(argc, argv);
if (!strcmp(argv[1], "lockdown"))
lockdown(argc, argv);
if (!strcmp(argv[1], "sbus1_out")) {
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()

57
src/modules/commander/commander.cpp
View File

@ -154,6 +154,16 @@ static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
static struct vehicle_status_s status;
/* armed topic */
static struct actuator_armed_s armed;
static struct safety_s safety;
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
/* tasks waiting for low prio thread */
typedef enum {
LOW_PRIO_TASK_NONE = 0,
@ -210,6 +220,9 @@ void print_reject_arm(const char *msg);
void print_status();
int arm();
int disarm();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
/**
@ -277,6 +290,16 @@ int commander_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "arm")) {
arm();
exit(0);
}
if (!strcmp(argv[1], "disarm")) {
disarm();
exit(0);
}
usage("unrecognized command");
exit(1);
}
@ -344,6 +367,30 @@ void print_status()
static orb_advert_t control_mode_pub;
static orb_advert_t status_pub;
int arm()
{
int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}
}
int disarm()
{
int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}
}
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
@ -537,16 +584,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
static struct vehicle_status_s status;
/* armed topic */
static struct actuator_armed_s armed;
static struct safety_s safety;
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */

15
src/modules/px4iofirmware/mixer.cpp
View File

@ -223,14 +223,25 @@ mixer_tick(void)
}
}
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
/* set arming */
bool needs_to_arm = (should_arm || should_always_enable_pwm);
/* check any conditions that prevent arming */
if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
needs_to_arm = false;
}
if (!should_arm && !should_always_enable_pwm) {
needs_to_arm = false;
}
if (needs_to_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
isr_debug(5, "> PWM enabled");
} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
} else if (!needs_to_arm && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;

1
src/modules/px4iofirmware/protocol.h
View File

@ -177,6 +177,7 @@
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */

8
src/modules/px4iofirmware/registers.c
View File

@ -185,7 +185,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@ -498,11 +499,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* lockup of the IO arming state.
*/
// XXX do not reset IO's safety state by FMU for now
// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
// r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
// }
if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}