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

MissionBlock: base loiter course exit condition on a constant value instead of MIS_YAW_ERR

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-08-19 18:40:13 +02:00
parent 5a7f098b8d
commit 27780308c0
2 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+13 -16
View File
@@ -458,8 +458,8 @@ MissionBlock::is_mission_item_reached_or_completed()
time_inside_reached = true;
}
// check if heading for exit is reached (only applies for fixed-wing flight)
bool exit_heading_reached = false;
// check if course for exit is reached (only applies for fixed-wing flight)
bool exit_course_reached = false;
if (time_inside_reached) {
@@ -469,37 +469,34 @@ MissionBlock::is_mission_item_reached_or_completed()
const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
next_sp.lon);
/* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set,
/* enforce exit course if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set,
or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */
const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
const bool enforce_exit_course = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&&
next_sp.valid &&
curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER &&
(_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT);
// can only enforce exit heading if next waypoint is not within loiter radius of current waypoint
const bool exit_heading_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius;
// can only enforce exit course if next waypoint is not within loiter radius of current waypoint
const bool exit_course_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius;
if (enforce_exit_heading && exit_heading_is_reachable) {
float yaw_err = 0.0f;
if (enforce_exit_course && exit_course_is_reachable) {
// set required yaw from bearing to the next mission item
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
const float desired_exit_bearing = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx);
yaw_err = wrap_pi(_mission_item.yaw - cog);
exit_heading_reached = fabsf(yaw_err) < _navigator->get_yaw_threshold();
exit_course_reached = fabsf(wrap_pi(desired_exit_bearing - cog)) < kLoiterExitCourseReachedThreshold;
} else {
exit_heading_reached = true;
exit_course_reached = true;
}
}
// set exit flight course to next waypoint
if (exit_heading_reached) {
if (exit_course_reached) {
position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current;
const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
src/modules/navigator/mission_block.h
+3
View File
@@ -54,6 +54,9 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vtol_vehicle_status.h>
// Maximal course error to exit loiter if exit course is enforced (fixed-wing only)
static constexpr float kLoiterExitCourseReachedThreshold = 0.087f; // 5 degrees
class Navigator;
class MissionBlock : public NavigatorMode