fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 16:13:34 +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-10-27 11:01:13 +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_block.cpp
+27 -29
View File
@@ -644,42 +644,40 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
}
void
MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item)
{
if (_navigator->get_land_detected()->landed) {
/* landed, don't takeoff, but switch to IDLE mode */
item->nav_cmd = NAV_CMD_IDLE;
setLoiterItemCommonFields(item);
} else {
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
const position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
item->lat = pos_sp_triplet->current.lat;
item->lon = pos_sp_triplet->current.lon;
item->altitude = pos_sp_triplet->current.alt;
item->loiter_radius = pos_sp_triplet->current.loiter_radius;
}
if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
/* use current position setpoint */
item->lat = pos_sp_triplet->current.lat;
item->lon = pos_sp_triplet->current.lon;
item->altitude = pos_sp_triplet->current.alt;
void
MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
{
setLoiterItemCommonFields(item);
} else {
/* use current position and use return altitude as clearance */
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
item->loiter_radius = _navigator->get_loiter_radius();
}
if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
item->altitude = _navigator->get_home_position()->alt + min_clearance;
}
}
void
MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item)
{
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
item->altitude_is_relative = false;
item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius();
item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}
item->altitude_is_relative = false;
item->acceptance_radius = _navigator->get_acceptance_radius();
item->yaw = NAN;
item->time_inside = 0.0f;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}
void