dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1205 from PX4/forcefail 

Force failsafe
This commit is contained in:
Lorenz Meier 2014-07-19 21:22:33 +02:00
parent 322089a390 74f31618f2
commit 8688cad8e6
10 changed files with 80 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mavlink/include/mavlink/v1.0

@ -1 +1 @@
Subproject commit d1ebe85eb6bb06d0078f3e0b8144adb131263628
Subproject commit 04b1ad5b284d5e916858ca9f928e93d97bbf6ad9

3
src/drivers/drv_pwm_output.h
View File

@ -202,6 +202,9 @@ ORB_DECLARE(output_pwm);
/** force safety switch off (to disable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
/*
*
*

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

@ -1149,6 +1149,12 @@ PX4IO::io_set_arming_state()
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
if (armed.force_failsafe) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
@ -2002,7 +2008,7 @@ PX4IO::print_status(bool extended_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%s\n",
printf("arming 0x%04x%s%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"),
@ -2010,7 +2016,9 @@ PX4IO::print_status(bool extended_status)
((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" : ""));
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "")
);
#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),
@ -2222,6 +2230,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_FAILSAFE:
/* force failsafe mode instantly */
if (arg == 0) {
/* clear force failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0);
} else {
/* set force failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
}
break;
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */

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

@ -546,24 +546,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
#if 0
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
//XXX: to enable the parachute, a param needs to be set
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd->param1 > 0.5f) {
//XXX update state machine?
armed_local->force_failsafe = true;
warnx("forcing failsafe");
} else {
/* reject parachute depoyment not armed */
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
armed_local->force_failsafe = false;
warnx("disabling failsafe");
}
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
#endif
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;

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

@ -111,7 +111,7 @@ mixer_tick(void)
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
}
/* default to failsafe mixing */
/* default to failsafe mixing - it will be forced below if flag is set */
source = MIX_FAILSAFE;
/*
@ -154,6 +154,13 @@ mixer_tick(void)
}
}
/*
* Check if we should force failsafe - and do it if we have to
*/
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
source = MIX_FAILSAFE;
}
/*
* Set failsafe status flag depending on mixing source
*/

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

@ -179,6 +179,7 @@
#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_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */
#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 */
@ -253,6 +254,10 @@ enum { /* DSM bind states */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */
#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */
/* Debug and test page - not used in normal operation */
#define PX4IO_PAGE_TEST 127
#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */

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

@ -189,7 +189,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN)
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)

1
src/modules/uORB/topics/actuator_armed.h
View File

@ -56,6 +56,7 @@ struct actuator_armed_s {
bool armed; /**< Set to true if system is armed */
bool ready_to_arm; /**< Set to true if system is ready to be armed */
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */
};
/**

1
src/modules/uORB/topics/vehicle_command.h
View File

@ -76,6 +76,7 @@ enum VEHICLE_CMD {
VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */
VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */
VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */

22
src/systemcmds/pwm/pwm.c
View File

@ -635,8 +635,28 @@ pwm_main(int argc, char *argv[])
}
exit(0);
} else if (!strcmp(argv[1], "forcefail")) {
if (argc < 3) {
errx(1, "arg missing [on|off]");
} else {
if (!strcmp(argv[2], "on")) {
/* force failsafe */
ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 1);
} else {
/* force failsafe */
ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0);
}
usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info");
if (ret != OK) {
warnx("FAILED setting forcefail %s", argv[2]);
}
}
exit(0);
}
usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail");
return 0;
}