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

navigator: break smoothly when entering hold mode

Browse Source 
This uses the existing breaking functionality when hold/loiter mode is
activated.
This commit is contained in:
Julian Oes 2021-12-13 14:31:47 +01:00 committed by Daniel Agar
parent f88dd28e85
commit fa6c051ae5
4 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -110,7 +110,7 @@ Loiter::set_loiter_position()
setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
} else {
setLoiterItemFromCurrentPosition(&_mission_item);
setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
}
}

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

@ -704,6 +704,17 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
item->loiter_radius = _navigator->get_loiter_radius();
}
void
MissionBlock::setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s *item)
{
setLoiterItemCommonFields(item);
_navigator->calculate_breaking_stop(item->lat, item->lon, item->yaw);
item->altitude = _navigator->get_global_position()->alt;
item->loiter_radius = _navigator->get_loiter_radius();
}
void
MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item)
{

1
src/modules/navigator/mission_block.h
View File

@ -111,6 +111,7 @@ protected:
void setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item);
void setLoiterItemFromCurrentPosition(struct mission_item_s *item);
void setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s *item);
void setLoiterItemCommonFields(struct mission_item_s *item);

2
src/modules/navigator/navigator_main.cpp
View File

@ -1577,8 +1577,6 @@ void Navigator::calculate_breaking_stop(double &lat, double &lon, float &yaw)
waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, course_over_ground,
multirotor_braking_distance, &lat, &lon);
lat = lat;
lon = lon;
yaw = get_local_position()->heading;
}