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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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,21 @@ 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];
unsigned max = (len < _max_actuators) ? len : _max_actuators;
/* set failsafe values */
for (unsigned i = 0; i < max; i++)
regs[i] = FLOAT_TO_REG(vals[i]);
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, regs, max);
}
int
PX4IO::io_set_arming_state()
{
@ -1250,7 +1273,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 +1285,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 +1742,20 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "failsafe")) {
/* XXX parse arguments here */
if (g_dev != nullptr) {
/* XXX testing values */
uint16_t failsafe[4] = {1500, 1500, 1200, 1200};
g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "recovery")) {
if (g_dev != nullptr) {
@ -1845,5 +1883,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'");
}

29
src/modules/px4iofirmware/mixer.cpp
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;
}

4
src/modules/px4iofirmware/protocol.h
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 */

3
src/modules/px4iofirmware/registers.c
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)