fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 23:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Slightly reworked IO internal failsafe, added command to activate it (px4io failsafe), does not parse commandline arguments yet

Browse Source
This commit is contained in:
Lorenz Meier
2013-05-28 17:46:24 +02:00
parent f1a8f6e75b
commit 2876bc72f9
4 changed files with 73 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/mixer.cpp
+23 -6
View File
@@ -102,13 +102,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 +120,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 +136,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 +174,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 +193,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 +243,6 @@ mixer_callback(uintptr_t handle,
case MIX_FAILSAFE:
case MIX_NONE:
/* XXX we could allow for configuration of per-output failsafe values */
return -1;
}
src/modules/px4iofirmware/protocol.h
+3 -1
View File
@@ -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 */
src/modules/px4iofirmware/registers.c
+2 -1
View File
@@ -41,6 +41,7 @@
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
@@ -179,7 +180,7 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
*
* Failsafe servo PWM values
*/
uint16_t r_page_servo_failsafe[IO_SERVO_COUNT];
uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
void
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)