fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 16:48:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

MissionBlock: loiter exit condition based on vector operation instead of atan

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-09-14 10:47:21 +02:00
parent 27780308c0
commit b4f27c9abb
2 changed files with 16 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+14 -10
View File
@@ -472,23 +472,27 @@ MissionBlock::is_mission_item_reached_or_completed()
/* 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_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);
&& 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 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_course && exit_course_is_reachable) {
// set required yaw from bearing to the next mission item
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);
float vehicle_position_to_next_waypoint_north;
float vehicle_position_to_next_waypoint_east;
get_vector_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, next_sp.lat,
next_sp.lon, &vehicle_position_to_next_waypoint_north, &vehicle_position_to_next_waypoint_east);
exit_course_reached = fabsf(wrap_pi(desired_exit_bearing - cog)) < kLoiterExitCourseReachedThreshold;
// this vector defines the exit bearing
const matrix::Vector2f vector_vehicle_position_to_next_waypoint = {vehicle_position_to_next_waypoint_north, vehicle_position_to_next_waypoint_east};
const matrix::Vector2f vehicle_ground_velocity = {_navigator->get_local_position()->vx, _navigator->get_local_position()->vy};
exit_course_reached = vector_vehicle_position_to_next_waypoint.dot(vehicle_ground_velocity) >
vector_vehicle_position_to_next_waypoint.norm() * vehicle_ground_velocity.norm() * kCosineExitCourseThreshold;
} else {
exit_course_reached = true;
src/modules/navigator/mission_block.h
+2 -2
View File
@@ -54,8 +54,8 @@
#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
// cosine of maximal course error to exit loiter if exit course is enforced (fixed-wing only)
static constexpr float kCosineExitCourseThreshold = 0.99619f; // cos(5°)
class Navigator;