gimbal: fix input selection

Once we have valid input we should use that and not overwrite our
update_result with the result from other inputs, otherwise the
automatic selection does not actually work.

This behavior means that only the first update will be used if there are
several sources updating in the same cycle.
This commit is contained in:
Julian Oes
2022-02-18 11:16:34 +01:00
committed by Beat Küng
parent 3c09448daf
commit 9b35b680f6
+8
View File
@@ -228,6 +228,8 @@ static int gimbal_thread_main(int argc, char *argv[])
update_result = thread_data.input_objs[i]->update(poll_timeout, control_data, already_active);
bool break_loop = false;
switch (update_result) {
case InputBase::UpdateResult::NoUpdate:
if (already_active) {
@@ -239,10 +241,12 @@ static int gimbal_thread_main(int argc, char *argv[])
case InputBase::UpdateResult::UpdatedActive:
thread_data.last_input_active = i;
break_loop = true;
break;
case InputBase::UpdateResult::UpdatedActiveOnce:
thread_data.last_input_active = -1;
break_loop = true;
break;
case InputBase::UpdateResult::UpdatedNotActive:
@@ -253,6 +257,10 @@ static int gimbal_thread_main(int argc, char *argv[])
break;
}
if (break_loop) {
break;
}
}
if (params.mnt_do_stab == 1) {