From cbde8d27f8a18eeb14038521115fd5bd2f97e29a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Dec 2013 20:14:32 +0100 Subject: [PATCH 1/4] fix small copy paste error in px4io driver --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 702ec1c3ae..f313a98f2b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -950,9 +950,9 @@ PX4IO::io_set_control_groups() bool attitude_ok = io_set_control_state(0); /* send auxiliary control groups */ - (void)io_set_control_state(0); (void)io_set_control_state(1); (void)io_set_control_state(2); + (void)io_set_control_state(3); return attitude_ok; } From 5e273bf225ac5ed57c10093296a00ee190cfbf7d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Dec 2013 21:34:31 +0100 Subject: [PATCH 2/4] px4iofirmware: in manual mode: ignore control indices which are not controlled by the rmeote control --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 9bb93ce9f1..87844ca70a 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -248,7 +248,7 @@ mixer_callback(uintptr_t handle, return -1; case MIX_OVERRIDE: - if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) { + 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; } From b69097df387ed6cea65d9884f6b3bc2ecb3ce3d9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Dec 2013 16:38:11 +0100 Subject: [PATCH 3/4] px4io frimware: improve handling of manual mode when fmu is still healthy, use data from fmu for channels which are not controlled by rc --- src/modules/px4iofirmware/mixer.cpp | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 87844ca70a..fdf01e09cb 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; From c033443208666d6972d99fe5a7b8e0c3fa5050b5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Dec 2013 16:57:19 +0100 Subject: [PATCH 4/4] px4iofirmware: improve check for rc controlled channels in manual mode --- src/modules/px4iofirmware/mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index fdf01e09cb..9fc86db5e7 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -258,7 +258,7 @@ mixer_callback(uintptr_t handle, return -1; case MIX_OVERRIDE: - if ((r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) && CONTROL_PAGE_INDEX(control_group, control_index) < PX4IO_RC_INPUT_CHANNELS) { + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } @@ -266,7 +266,7 @@ mixer_callback(uintptr_t handle, 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) { + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { 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) {