fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 22:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/io_fixes' into new_state_machine

Browse Source 
Conflicts:
	src/drivers/px4io/px4io.cpp
	src/modules/commander/commander.c
	src/modules/commander/state_machine_helper.c
	src/modules/commander/state_machine_helper.h
	src/modules/px4iofirmware/mixer.cpp
	src/modules/uORB/topics/actuator_controls.h
	src/modules/uORB/topics/vehicle_status.h
This commit is contained in:
Julian Oes
2013-06-18 15:35:26 +02:00
parent 34c197c7cc c3a8f177b6
commit 202792294a
13 changed files with 343 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/mixer.cpp
+48 -29
View File
@@ -62,7 +62,7 @@ extern "C" {
/*
* Time that the ESCs need to initialize
*/
#define ESC_INIT_TIME_US 2000000
#define ESC_INIT_TIME_US 1000000
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
@@ -74,8 +74,9 @@ extern "C" {
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
static uint64_t time_armed;
static bool init_complete = false;
static uint64_t esc_init_time;
static bool esc_init_active = false;
static bool esc_init_done = false;
/* selected control values and count for mixing */
enum mixer_source {
@@ -106,7 +107,7 @@ mixer_tick(void)
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
} else {
@@ -120,12 +121,11 @@ mixer_tick(void)
* Decide which set of controls we're using.
*/
/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
/* do not mix 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)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* don't actually mix anything - we already have raw PWM values or
not a valid mixer. */
/* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
} else {
@@ -175,28 +175,42 @@ mixer_tick(void)
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* after arming, some ESCs need an initalization period, count the time from here */
if (mixer_servos_armed && !esc_init_done && !esc_init_active) {
esc_init_time = hrt_absolute_time();
esc_init_active = true;
isr_debug(1, "start counting now");
}
/* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */
if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) {
esc_init_active = false;
esc_init_done = true;
isr_debug(1, "time is up");
}
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
if (!init_complete && mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) {
init_complete = true;
}
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* scale to control range after init time */
if (init_complete) {
/* XXX maybe this check for an armed FMU could be achieved a little less messy */
if (source == MIX_FMU && !(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) {
r_page_servos[i] = r_page_servo_failsafe[i];
}
/* during ESC initialization, use low PWM */
else if (esc_init_active) {
r_page_servos[i] = (outputs[i] * 600 + 1500);
/* afterwards use min and max values */
} else {
r_page_servos[i] = (outputs[i]
* (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+ (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
/* but use init range from 900 to 2100 right after arming */
} else {
r_page_servos[i] = (outputs[i] * 600 + 1500);
}
}
@@ -214,26 +228,31 @@ mixer_tick(void)
* here.
*/
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 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) ))
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
/* and either FMU is armed */ ( ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* and there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))) ||
/* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) )
);
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
time_armed = hrt_absolute_time();
init_complete = false;
r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
isr_debug(5, "> armed");
} else if (!should_arm && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
init_complete = false;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
isr_debug(5, "> disarmed");
esc_init_active = false;
isr_debug(1, "disarming, and init aborted");
}
if (mixer_servos_armed) {
@@ -288,9 +307,8 @@ static unsigned mixer_text_length = 0;
void
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while fully armed */
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
/* do not allow a mixer change while outputs armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return;
}
@@ -367,6 +385,7 @@ mixer_set_failsafe()
* Check if a custom failsafe value has been written,
* or if the mixer is not ok and bail out.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
return;
src/modules/px4iofirmware/protocol.h
+7 -3
View File
@@ -75,10 +75,13 @@
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
#define PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC 2
#define PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC 2
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers */
#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers */
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
@@ -93,7 +96,7 @@
#define PX4IO_P_STATUS_CPULOAD 1
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
@@ -105,6 +108,7 @@
#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_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
#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 */
src/modules/px4iofirmware/registers.c
+11 -9
View File
@@ -57,8 +57,8 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt
* Static configuration parameters.
*/
static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC,
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC,
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
@@ -146,7 +146,8 @@ volatile uint16_t r_page_setup[] =
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
PX4IO_P_SETUP_ARMING_IO_ARM_OK)
PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -377,9 +378,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* so that an in-air reset of FMU can not lead to a
* lockup of the IO arming state.
*/
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;
}
// 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;
// }
r_setup_arming = value;
@@ -427,9 +430,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
/* do not allow a RC config change while fully armed */
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
/* do not allow a RC config change while outputs armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
break;
}
src/modules/px4iofirmware/safety.c
+5 -5
View File
@@ -110,7 +110,7 @@ safety_check_button(void *arg)
* state machine, keep ARM_COUNTER_THRESHOLD the same
* length in all cases of the if/else struct below.
*/
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +118,18 @@ safety_check_button(void *arg)
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* switch to armed state */
r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
counter++;
}
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
counter++;
}
@@ -140,7 +140,7 @@ safety_check_button(void *arg)
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_IO_FMU_ARMED;