diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-22 15:32:28 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-22 15:32:28 +0100 |
commit | d5bca175e8b2100ba5e9033f0204746a891faae7 (patch) | |
tree | 4950dd160ee90ffb33a1d31effaa4312cf0ca55a | |
parent | 8942f7a39257477c6b6434b370d29c242c4139a1 (diff) | |
download | px4-firmware-d5bca175e8b2100ba5e9033f0204746a891faae7.tar.gz px4-firmware-d5bca175e8b2100ba5e9033f0204746a891faae7.tar.bz2 px4-firmware-d5bca175e8b2100ba5e9033f0204746a891faae7.zip |
Put safe link output active in mixer if enabled via register.
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 39 |
1 files changed, 37 insertions, 2 deletions
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 9ef11aa8e..5a834e07f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -79,7 +79,8 @@ enum mixer_source { MIX_FMU, MIX_OVERRIDE, MIX_FAILSAFE, - MIX_OVERRIDE_FMU_OK + MIX_OVERRIDE_FMU_OK, + MIX_SAFELINK }; static mixer_source source; @@ -159,6 +160,18 @@ mixer_tick(void) */ if (source == MIX_FAILSAFE) { r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; + + /* + * Check if we can fall back in safelink failsave + */ + + if ((r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SAFELINK_OUT) && (r_safelink_count > 0)) { + source = MIX_SAFELINK; + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFELINK_FS; + } else { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_SAFELINK_FS); + } + } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } @@ -192,7 +205,19 @@ mixer_tick(void) /* * Run the mixers. */ - if (source == MIX_FAILSAFE) { + if (source == MIX_SAFELINK) { + + actuator_count = r_safelink_count; + + /* copy failsafe values to the servo outputs */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_servos[i] = r_safelink_values[i]; + + /* safe actuators for FMU feedback */ + r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); + } + + } else if (source == MIX_FAILSAFE) { actuator_count = 0; @@ -273,6 +298,11 @@ mixer_tick(void) sbus2_output(r_page_servos, actuator_count); } + /* update safelink outputs */ + if (source != MIX_FAILSAFE && r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SAFELINK_OUT) { + safelink_output(r_page_servos, actuator_count); + } + } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) @@ -287,6 +317,11 @@ mixer_tick(void) if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(r_page_servos, actuator_count); } + + /* update safelink outputs */ + if (source != MIX_FAILSAFE && r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SAFELINK_OUT) { + safelink_output(r_page_servos, actuator_count); + } } } |