fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 01:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

px4io frimware: improve handling of manual mode when fmu is still healthy, use data from fmu for channels which are not controlled by rc

Browse Source
This commit is contained in:
Thomas Gubler
2013-12-11 16:58:03 +01:00
parent 5e273bf225
commit b69097df38
1 changed files with 24 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/mixer.cpp
+24 -3
View File
@@ -77,7 +77,8 @@ enum mixer_source {
MIX_NONE, MIX_NONE,
MIX_FMU, MIX_FMU,
MIX_OVERRIDE, MIX_OVERRIDE,
MIX_FAILSAFE MIX_FAILSAFE,
MIX_OVERRIDE_FMU_OK
}; };
static mixer_source source; static mixer_source source;
@@ -135,10 +136,19 @@ mixer_tick(void)
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && 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_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_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 */ /* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE; 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) { switch (source) {
case MIX_FMU: 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)]); control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
break; break;
} }
@@ -254,6 +264,17 @@ mixer_callback(uintptr_t handle,
} }
return -1; 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_FAILSAFE:
case MIX_NONE: case MIX_NONE:
control = 0.0f; control = 0.0f;