diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-11 16:38:11 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-11 16:58:03 +0100 |
commit | b69097df387ed6cea65d9884f6b3bc2ecb3ce3d9 (patch) | |
tree | 57a78758dbf9ddae808bcbe25a9569821b2acd3e /src/modules | |
parent | 5e273bf225ac5ed57c10093296a00ee190cfbf7d (diff) | |
download | px4-firmware-b69097df387ed6cea65d9884f6b3bc2ecb3ce3d9.tar.gz px4-firmware-b69097df387ed6cea65d9884f6b3bc2ecb3ce3d9.tar.bz2 px4-firmware-b69097df387ed6cea65d9884f6b3bc2ecb3ce3d9.zip |
px4io frimware: improve handling of manual mode when fmu is still healthy, use data from fmu for channels which are not controlled by rc
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 27 |
1 files changed, 24 insertions, 3 deletions
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 87844ca70..fdf01e09c 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -77,7 +77,8 @@ enum mixer_source { MIX_NONE, MIX_FMU, MIX_OVERRIDE, - MIX_FAILSAFE + MIX_FAILSAFE, + MIX_OVERRIDE_FMU_OK }; static mixer_source source; @@ -135,10 +136,19 @@ mixer_tick(void) if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) { + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; + } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + /* if allowed, mix from RC inputs directly up to available rc channels */ + source = MIX_OVERRIDE_FMU_OK; } } @@ -241,7 +251,7 @@ mixer_callback(uintptr_t handle, switch (source) { case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS) { + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } @@ -254,6 +264,17 @@ mixer_callback(uintptr_t handle, } return -1; + case MIX_OVERRIDE_FMU_OK: + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ + if ((r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) && CONTROL_PAGE_INDEX(control_group, control_index) < PX4IO_RC_INPUT_CHANNELS) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + break; + } + return -1; + case MIX_FAILSAFE: case MIX_NONE: control = 0.0f; |