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

Navigator: use NAV_FW_ALTL_RAD only for FW landings, not VTOL landings

Browse Source 
Also containst a mini refactor by removing the unnecessary function
get_default_altitude_acceptance_radius().

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-09-22 16:52:51 +02:00
parent c6c7733587
commit 0df878bc18
3 changed files with 11 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+1 -1
View File
@@ -283,7 +283,7 @@ MissionBlock::is_mission_item_reached_or_completed()
// 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()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
curr_sp->alt = mission_item_altitude_amsl;
curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
src/modules/navigator/navigator.h
-7
View File
@@ -200,13 +200,6 @@ public:
*/
float get_acceptance_radius();
/**
* Get the default altitude acceptance radius (i.e. from parameters)
*
* @return the distance from the target altitude before considering the waypoint reached
*/
float get_default_altitude_acceptance_radius();
/**
* Get the altitude acceptance radius
*
src/modules/navigator/navigator_main.cpp
+10 -16
View File
@@ -988,10 +988,18 @@ float Navigator::get_default_acceptance_radius()
return _param_nav_acc_rad.get();
}
float Navigator::get_default_altitude_acceptance_radius()
float Navigator::get_altitude_acceptance_radius()
{
if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return _param_nav_fw_alt_rad.get();
const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next;
if (!force_vtol() && next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
// Use separate (tighter) altitude acceptance for clean altitude starting point before FW landing
return _param_nav_fw_altl_rad.get();
} else {
return _param_nav_fw_alt_rad.get();
}
} else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
return INFINITY;
@@ -1010,20 +1018,6 @@ float Navigator::get_default_altitude_acceptance_radius()
}
}
float Navigator::get_altitude_acceptance_radius()
{
if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next;
if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
// Use separate (tighter) altitude acceptance for clean altitude starting point before landing
return _param_nav_fw_altl_rad.get();
}
}
return get_default_altitude_acceptance_radius();
}
float Navigator::get_cruising_speed()
{
/* there are three options: The mission-requested cruise speed, or the current hover / plane speed */