diff options
author | Julian Oes <julian@oes.ch> | 2013-06-18 15:35:26 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-06-18 15:35:26 +0200 |
commit | 202792294ac8a4d0db2c0e64d944be8e95608930 (patch) | |
tree | e4c39e482d20ac53d8cf09b6df4454a617c522e5 /src/modules/px4iofirmware | |
parent | 34c197c7cc77de0317112530eb60aa5f3ba5687d (diff) | |
parent | c3a8f177b6316a9cefd814e312742f47d3049739 (diff) | |
download | px4-firmware-202792294ac8a4d0db2c0e64d944be8e95608930.tar.gz px4-firmware-202792294ac8a4d0db2c0e64d944be8e95608930.tar.bz2 px4-firmware-202792294ac8a4d0db2c0e64d944be8e95608930.zip |
Merge remote-tracking branch 'upstream/io_fixes' into new_state_machine
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
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 77 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 10 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 20 | ||||
-rw-r--r-- | src/modules/px4iofirmware/safety.c | 10 |
4 files changed, 71 insertions, 46 deletions
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 62a6ebf13..e3ec2fa76 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -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; - /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + /* 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"); + } - if (!init_complete && mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { - init_complete = true; + /* 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); + /* 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; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 1c9715451..b43054197 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -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 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index bc1c83901..379cf09e3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -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; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 4dbecc274..95335f038 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -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; |