dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: fix abs bug / trigger POSCTL both ways 

The check if stick were touched was only working in one direction (per
axis) because fabsf was used incorrectly.

However, this check is still only a differential check triggered by
fast movement and does not trigger if someone slowly moves a stick to
the side. Also, the sensitivity depends on the rate of the commander
loop and/or the RC update loop. The correct solution would be a proper
filtering and trigger for movement.
This commit is contained in:
Julian Oes 2017-03-11 10:52:38 +01:00 committed by Lorenz Meier
parent fb64403607
commit 404719953c
1 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/commander/commander.cpp
View File

@ -2534,10 +2534,10 @@ int commander_thread_main(int argc, char *argv[])
// transition to previous state if sticks are touched
if ((_last_sp_man.timestamp != sp_man.timestamp) &&
((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) ||
(fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) ||
(fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) ||
(fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) ||
(fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) {
// revert to position control in any case
main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
@ -2550,11 +2550,12 @@ int commander_thread_main(int argc, char *argv[])
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER) {
// transition to previous state if sticks are touched
if ((_last_sp_man.timestamp != sp_man.timestamp) &&
((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) ||
(fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) ||
(fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) ||
(fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) ||
(fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) {
// revert to position control in any case
main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);