forked from Archive/PX4-Autopilot
Merge pull request #288 from PX4/failsafe_io
Better failsafe on IO (if FMU and IO fail)
This commit is contained in:
sjwilks
commit
40dbd21e11
|
|
@ -106,7 +106,7 @@ public:
|
|||
* @param rate The rate in Hz actuator outpus are sent to IO.
|
||||
* Min 10 Hz, max 400 Hz
|
||||
*/
|
||||
int set_update_rate(int rate);
|
||||
int set_update_rate(int rate);
|
||||
|
||||
/**
|
||||
* Set the battery current scaling and bias
|
||||
|
|
@ -114,7 +114,15 @@ public:
|
|||
* @param amp_per_volt
|
||||
* @param amp_bias
|
||||
*/
|
||||
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
|
||||
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
|
||||
|
||||
/**
|
||||
* Push failsafe values to IO.
|
||||
*
|
||||
* @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
|
||||
* @param len Number of channels, could up to 8
|
||||
*/
|
||||
int set_failsafe_values(const uint16_t *vals, unsigned len);
|
||||
|
||||
/**
|
||||
* Print the current status of IO
|
||||
|
|
@ -326,11 +334,11 @@ PX4IO::PX4IO() :
|
|||
_to_actuators_effective(0),
|
||||
_to_outputs(0),
|
||||
_to_battery(0),
|
||||
_primary_pwm_device(false),
|
||||
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
|
||||
_battery_amp_bias(0),
|
||||
_battery_mamphour_total(0),
|
||||
_battery_last_timestamp(0),
|
||||
_primary_pwm_device(false)
|
||||
_battery_last_timestamp(0)
|
||||
{
|
||||
/* we need this potentially before it could be set in task_main */
|
||||
g_dev = this;
|
||||
|
|
@ -689,6 +697,19 @@ PX4IO::io_set_control_state()
|
|||
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
|
||||
{
|
||||
uint16_t regs[_max_actuators];
|
||||
|
||||
if (len > _max_actuators)
|
||||
/* fail with error */
|
||||
return E2BIG;
|
||||
|
||||
/* copy values to registers in IO */
|
||||
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO::io_set_arming_state()
|
||||
{
|
||||
|
|
@ -1250,7 +1271,7 @@ PX4IO::print_status()
|
|||
printf("%u bytes free\n",
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
|
||||
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
|
||||
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n",
|
||||
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
|
||||
flags,
|
||||
((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
|
||||
|
|
@ -1262,7 +1283,8 @@ PX4IO::print_status()
|
|||
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"));
|
||||
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : ""));
|
||||
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
|
||||
printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
|
||||
alarms,
|
||||
|
|
@ -1718,6 +1740,41 @@ px4io_main(int argc, char *argv[])
|
|||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "failsafe")) {
|
||||
|
||||
if (argc < 3) {
|
||||
errx(1, "failsafe command needs at least one channel value (ppm)");
|
||||
}
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
|
||||
/* set values for first 8 channels, fill unassigned channels with 1500. */
|
||||
uint16_t failsafe[8];
|
||||
|
||||
for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
|
||||
{
|
||||
/* set channel to commanline argument or to 900 for non-provided channels */
|
||||
if (argc > i + 2) {
|
||||
failsafe[i] = atoi(argv[i+2]);
|
||||
if (failsafe[i] < 800 || failsafe[i] > 2200) {
|
||||
errx(1, "value out of range of 800 < value < 2200. Aborting.");
|
||||
}
|
||||
} else {
|
||||
/* a zero value will result in stopping to output any pulse */
|
||||
failsafe[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
|
||||
|
||||
if (ret != OK)
|
||||
errx(ret, "failed setting failsafe values");
|
||||
} else {
|
||||
errx(1, "not loaded");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "recovery")) {
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
|
|
@ -1845,5 +1902,5 @@ px4io_main(int argc, char *argv[])
|
|||
monitor();
|
||||
|
||||
out:
|
||||
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'");
|
||||
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -85,6 +85,9 @@ static int mixer_callback(uintptr_t handle,
|
|||
|
||||
static MixerGroup mixer_group(mixer_callback, 0);
|
||||
|
||||
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
|
||||
static void mixer_set_failsafe();
|
||||
|
||||
void
|
||||
mixer_tick(void)
|
||||
{
|
||||
|
|
@ -102,13 +105,16 @@ mixer_tick(void)
|
|||
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
|
||||
}
|
||||
|
||||
/* default to failsafe mixing */
|
||||
source = MIX_FAILSAFE;
|
||||
|
||||
/*
|
||||
* Decide which set of controls we're using.
|
||||
*/
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
|
||||
/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
|
||||
/* don't actually mix anything - we already have raw PWM values or
|
||||
not a valid mixer. */
|
||||
|
|
@ -117,6 +123,7 @@ mixer_tick(void)
|
|||
} else {
|
||||
|
||||
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
|
||||
/* mix from FMU controls */
|
||||
|
|
@ -132,15 +139,29 @@ mixer_tick(void)
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set failsafe status flag depending on mixing source
|
||||
*/
|
||||
if (source == MIX_FAILSAFE) {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
|
||||
} else {
|
||||
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
|
||||
}
|
||||
|
||||
/*
|
||||
* Run the mixers.
|
||||
*/
|
||||
if (source == MIX_FAILSAFE) {
|
||||
|
||||
/* copy failsafe values to the servo outputs */
|
||||
for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
|
||||
for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
|
||||
r_page_servos[i] = r_page_servo_failsafe[i];
|
||||
|
||||
/* safe actuators for FMU feedback */
|
||||
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
|
||||
}
|
||||
|
||||
|
||||
} else if (source != MIX_NONE) {
|
||||
|
||||
float outputs[IO_SERVO_COUNT];
|
||||
|
|
@ -156,7 +177,7 @@ mixer_tick(void)
|
|||
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
|
||||
|
||||
/* scale to servo output */
|
||||
r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
|
||||
r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
|
||||
|
||||
}
|
||||
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
|
||||
|
|
@ -175,7 +196,7 @@ mixer_tick(void)
|
|||
bool should_arm = (
|
||||
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
|
||||
/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
|
||||
/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
|
||||
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
|
||||
/* FMU is available or FMU is not available but override is an option */
|
||||
((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
|
||||
|
|
@ -225,7 +246,7 @@ mixer_callback(uintptr_t handle,
|
|||
|
||||
case MIX_FAILSAFE:
|
||||
case MIX_NONE:
|
||||
/* XXX we could allow for configuration of per-output failsafe values */
|
||||
control = 0.0f;
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
|
@ -303,8 +324,41 @@ mixer_handle_text(const void *buffer, size_t length)
|
|||
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
|
||||
|
||||
mixer_text_length = resid;
|
||||
|
||||
/* update failsafe values */
|
||||
mixer_set_failsafe();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
mixer_set_failsafe()
|
||||
{
|
||||
/*
|
||||
* Check if a custom failsafe value has been written,
|
||||
* else use the opportunity to set decent defaults.
|
||||
*/
|
||||
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
|
||||
return;
|
||||
|
||||
float outputs[IO_SERVO_COUNT];
|
||||
unsigned mixed;
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
|
||||
|
||||
/* scale to PWM and update the servo outputs as required */
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
|
||||
/* scale to servo output */
|
||||
r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
|
||||
|
||||
}
|
||||
|
||||
/* disable the rest of the outputs */
|
||||
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
|
||||
r_page_servo_failsafe[i] = 0;
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@
|
|||
#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */
|
||||
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
|
||||
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
|
||||
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */
|
||||
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */
|
||||
|
||||
/* dynamic status page */
|
||||
#define PX4IO_PAGE_STATUS 1
|
||||
|
|
@ -104,6 +104,7 @@
|
|||
#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */
|
||||
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
|
||||
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
|
||||
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
|
||||
|
||||
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
|
||||
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
|
||||
|
|
@ -148,6 +149,7 @@
|
|||
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
|
||||
#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */
|
||||
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
|
||||
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */
|
||||
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
|
||||
|
||||
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
|
||||
|
|
|
|||
|
|
@ -41,6 +41,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
|
@ -178,8 +179,10 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
|
|||
* PAGE 105
|
||||
*
|
||||
* Failsafe servo PWM values
|
||||
*
|
||||
* Disable pulses as default.
|
||||
*/
|
||||
uint16_t r_page_servo_failsafe[IO_SERVO_COUNT];
|
||||
uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
|
||||
|
||||
void
|
||||
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
|
||||
|
|
@ -230,11 +233,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
|||
case PX4IO_PAGE_FAILSAFE_PWM:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
|
||||
while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
/* XXX range-check value? */
|
||||
r_page_servo_failsafe[offset] = *values;
|
||||
|
||||
/* flag the failsafe values as custom */
|
||||
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
|
|
|
|||
Loading…
Reference in New Issue