diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-11 16:38:11 +0100 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-13 20:33:59 +0100 |
| commit | 2fb493e639bb78d862529062dedb79b97c96a769 (patch) | |
| tree | 53f5d671f874916749eab78ce6371ea59cd2227a | |
| parent | 2761d47be2792a3e28e92bfd60407a0aaa243106 (diff) | |
| download | px4-firmware-2fb493e639bb78d862529062dedb79b97c96a769.tar.gz px4-firmware-2fb493e639bb78d862529062dedb79b97c96a769.tar.bz2 px4-firmware-2fb493e639bb78d862529062dedb79b97c96a769.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
| -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; |