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

Fix RTL abort detection on stick change

This commit is contained in:
Lorenz Meier 2016-05-05 16:55:04 +02:00
parent bbd2b763a3
commit 639a589233
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2267,7 +2267,7 @@ int commander_thread_main(int argc, char *argv[])
main_state_before_rtl == commander_state_s::MAIN_STATE_STAB)) { main_state_before_rtl == commander_state_s::MAIN_STATE_STAB)) {
// transition to previous state if sticks are increased // transition to previous state if sticks are increased
const float min_stick_change = 0.2; const float min_stick_change = 0.2f;
if ((_last_sp_man.timestamp != sp_man.timestamp) && if ((_last_sp_man.timestamp != sp_man.timestamp) &&
((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) || ((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.y) - fabsf(_last_sp_man.y) > min_stick_change) ||
@ -2982,11 +2982,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
(_last_sp_man.mode_slot == sp_man->mode_slot))) { (_last_sp_man.mode_slot == sp_man->mode_slot))) {
// update these fields for the geofence system // update these fields for the geofence system
_last_sp_man.timestamp = sp_man->timestamp;
_last_sp_man.x = sp_man->x; if (!rtl_on) {
_last_sp_man.y = sp_man->y; _last_sp_man.timestamp = sp_man->timestamp;
_last_sp_man.z = sp_man->z; _last_sp_man.x = sp_man->x;
_last_sp_man.r = sp_man->r; _last_sp_man.y = sp_man->y;
_last_sp_man.z = sp_man->z;
_last_sp_man.r = sp_man->r;
}
/* no timestamp change or no switch change -> nothing changed */ /* no timestamp change or no switch change -> nothing changed */
return TRANSITION_NOT_CHANGED; return TRANSITION_NOT_CHANGED;