fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mission: keep current heading if close to ROI while heading sp is invalid

Browse Source
This commit is contained in:
Dusan Zivkovic 2020-05-29 00:48:32 +02:00 committed by Julian Oes
parent a9dacae57e
commit 08fdc90175
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/navigator/mission.cpp
View File

@ -1321,6 +1321,7 @@ Mission::heading_sp_update()
case vehicle_roi_s::ROI_TARGET:
case vehicle_roi_s::ROI_ENUM_END:
default: {
return;
}
}
@ -1339,6 +1340,13 @@ Mission::heading_sp_update()
_mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;
pos_sp_triplet->current.yaw_valid = true;
} else {
if (!pos_sp_triplet->current.yaw_valid) {
_mission_item.yaw = _navigator->get_local_position()->yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;
pos_sp_triplet->current.yaw_valid = true;
}
}
// we set yaw directly so we can run this in parallel to the FOH update