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

Navigator: for loiter position acceptance, use L1 distance and loiter radius

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-01-08 16:55:02 +01:00
parent acbada73dd
commit 017567792b
3 changed files with 16 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+8 -10
View File
@@ -197,12 +197,13 @@ MissionBlock::is_mission_item_reached()
* coordinates with a radius equal to the loiter_radius field. It is not flying
* through the waypoint center.
* Therefore the item is marked as reached once the system reaches the loiter
* radius (+ some margin). Time inside and turn count is handled elsewhere.
* radius + L1 distance. Time inside and turn count is handled elsewhere.
*/
float radius = (fabsf(_mission_item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(_mission_item.loiter_radius) :
_navigator->get_loiter_radius();
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(radius) * 1.2f)
// check if within loiter radius around wp, if yes then set altitude sp to mission item
if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
@@ -228,7 +229,8 @@ MissionBlock::is_mission_item_reached()
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
// check if within loiter radius around wp, if yes then set altitude sp to mission item
if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
&& dist_z <= _navigator->get_default_altitude_acceptance_radius()) {
// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
@@ -236,7 +238,7 @@ MissionBlock::is_mission_item_reached()
_navigator->set_position_setpoint_triplet_updated();
}
} else if (dist >= 0.f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
} else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
@@ -298,13 +300,9 @@ MissionBlock::is_mission_item_reached()
_time_wp_reached = now;
} else {
/* for normal mission items used their acceptance radius */
float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
/*normal mission items */
/* if set to zero use the default instead */
if (mission_acceptance_radius < NAV_EPSILON_POSITION) {
mission_acceptance_radius = _navigator->get_acceptance_radius();
}
float mission_acceptance_radius = _navigator->get_acceptance_radius();
/* for vtol back transition calculate acceptance radius based on time and ground speed */
if (_mission_item.vtol_back_transition
src/modules/navigator/navigator.h
-9
View File
@@ -246,15 +246,6 @@ public:
*/
void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }
/**
* Get the acceptance radius given the mission item preset radius
*
* @param mission_item_radius the radius to use in case the controller-derived radius is smaller
*
* @return the distance at which the next waypoint should be used
*/
float get_acceptance_radius(float mission_item_radius);
/**
* Get the yaw acceptance given the current mission item
*
src/modules/navigator/navigator_main.cpp
+8 -21
View File
@@ -873,12 +873,6 @@ Navigator::get_default_acceptance_radius()
return _param_nav_acc_rad.get();
}
float
Navigator::get_acceptance_radius()
{
return get_acceptance_radius(_param_nav_acc_rad.get());
}
float
Navigator::get_default_altitude_acceptance_radius()
{
@@ -996,24 +990,17 @@ Navigator::get_cruising_throttle()
}
float
Navigator::get_acceptance_radius(float mission_item_radius)
Navigator::get_acceptance_radius()
{
float radius = mission_item_radius;
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
// return the value specified in the parameter NAV_ACC_RAD
return get_default_acceptance_radius();
// XXX only use navigation capabilities for now
// when in fixed wing mode
// this might need locking against a commanded transition
// so that a stale _vstatus doesn't trigger an accepted mission item.
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
if (_vstatus.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& (pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp)
&& pos_ctrl_status.acceptance_radius > radius) {
radius = pos_ctrl_status.acceptance_radius;
} else {
// return the max of NAV_ACC_RAD and the controller acceptance radius (e.g. L1 distance)
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
return math::max(pos_ctrl_status.acceptance_radius, get_default_acceptance_radius());
}
return radius;
}
float