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

navigator: cleanup of set_loiter_item

Browse Source 
Unwraps the set_loiter_item() to solve the issue where the altitdue setpoint
in a MC takeoff wasn't correctly used.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-03-09 22:10:05 +03:00
committed by Roman Bapst
parent cb78ba34d7
commit bf6a47ba6a
5 changed files with 76 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+14 -3
View File
@@ -683,11 +683,22 @@ Mission::set_mission_items()
_mission_type = MISSION_TYPE_NONE;
/* set loiter mission item and ensure that there is a minimum clearance from home */
set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
if (_navigator->get_land_detected()->landed) {
_mission_item.nav_cmd = NAV_CMD_IDLE;
} else {
if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
} else {
setLoiterItemFromCurrentPosition(&_mission_item);
}
}
/* update position setpoint triplet */
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);