fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 05:11:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Konrad
2023-11-13 12:43:58 +01:00
committed by Thomas Stastny
parent 6800a448b0
commit 32127ca789
2 changed files with 17 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/loiter.cpp
+15 -1
View File
@@ -94,7 +94,21 @@ Loiter::set_loiter_position()
_mission_item.nav_cmd = NAV_CMD_IDLE; _mission_item.nav_cmd = NAV_CMD_IDLE;
} else { } 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); setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
} else { } else {
src/modules/navigator/mission_block.cpp
+2 -1
View File
@@ -747,7 +747,8 @@ MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *it
item->lat = pos_sp_triplet->current.lat; item->lat = pos_sp_triplet->current.lat;
item->lon = pos_sp_triplet->current.lon; item->lon = pos_sp_triplet->current.lon;
item->altitude = pos_sp_triplet->current.alt; 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 void