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

loiter: set loiter position to current position setpoint, if it is a loiter item

This commit is contained in:
Konrad 2023-11-03 11:53:44 +01:00 committed by Thomas Stastny
parent 6800a448b0
commit 32127ca789
2 changed files with 17 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/navigator/loiter.cpp
View File

@ -94,7 +94,21 @@ Loiter::set_loiter_position()
_mission_item.nav_cmd = NAV_CMD_IDLE;
} else {
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
// Check if we already loiter on a circle and are on the loiter pattern.
bool on_loiter{false};
if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER
&& pos_sp_triplet->current.loiter_pattern == position_setpoint_s::LOITER_TYPE_ORBIT) {
const float d_current = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
on_loiter = d_current <= (_navigator->get_acceptance_radius() + pos_sp_triplet->current.loiter_radius);
}
if (on_loiter) {
setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
} else {

3
src/modules/navigator/mission_block.cpp
View File

@ -747,7 +747,8 @@ MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *it
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;
item->loiter_radius = pos_sp_triplet->current.loiter_direction_counter_clockwise ?
-pos_sp_triplet->current.loiter_radius : pos_sp_triplet->current.loiter_radius;
}
void