From 20c02ae093909e83af2967d9dd83561396914ac2 Mon Sep 17 00:00:00 2001
From: Philipp Oettershagen <philipp.oettershagen@mavt.ethz.ch>
Date: Sun, 17 Jun 2018 01:27:02 +0200
Subject: [PATCH] Fixed-wing autoland: Adapt according to @dagar's and
@antiheavy's comments.
---
.../FixedwingPositionControl.cpp | 2 +-
src/modules/navigator/mission_block.cpp | 19 ++++-------------
src/modules/navigator/navigator.h | 7 +++++++
src/modules/navigator/navigator_main.cpp | 21 ++++++++++++++++++-
4 files changed, 32 insertions(+), 17 deletions(-)
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 2eac47741f..cd5cafcbf0 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -847,7 +847,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
float alt_sp = pos_sp_curr.alt;
- if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid) {
+ if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid && _l1_control.circle_mode()) {
// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
// landing airspeed and potentially tighter throttle control) already such that we don't
// have to do this switch (which can cause significant altitude errors) close to the ground.
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index b0860b9575..5a0d2fe16c 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -243,7 +243,7 @@ MissionBlock::is_mission_item_reached()
&dist_xy, &dist_z);
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
- && dist_z <= _navigator->get_altitude_acceptance_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
curr_sp->alt = altitude_amsl;
@@ -251,26 +251,15 @@ MissionBlock::is_mission_item_reached()
}
} else {
- // Determine altitude acceptance radius. By default, this is taken from the respective parameter.
- // However, before a landing approach the acceptance radius needs to be tighter: Assume a 30% error
- // w.r.t. the remaining descent altitude is OK, but enforce min/max values (e.g. min=3m to make
- // sure that the waypoint can still be reached in case of wrong user input).
- float alt_accept_rad = _navigator->get_altitude_acceptance_radius();
- struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next;
-
- if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
- alt_accept_rad = math::constrain(0.3f * (curr_sp->alt - next_sp.alt), 3.0f,
- _navigator->get_altitude_acceptance_radius());
- PX4_INFO("Accept:%5.3f", double(alt_accept_rad));
- }
-
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
- && dist_z <= alt_accept_rad) {
+ && dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
// set required yaw from bearing to the next mission item
if (_mission_item.force_heading) {
+ const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
+
if (next_sp.valid) {
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index ceaeca4534..22dda8bede 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -181,6 +181,13 @@ 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
*
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 57971bc320..3bfec7f0d9 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -854,7 +854,7 @@ Navigator::get_acceptance_radius()
}
float
-Navigator::get_altitude_acceptance_radius()
+Navigator::get_default_altitude_acceptance_radius()
{
if (!get_vstatus()->is_rotary_wing) {
return _param_fw_alt_acceptance_radius.get();
@@ -864,6 +864,25 @@ Navigator::get_altitude_acceptance_radius()
}
}
+float
+Navigator::get_altitude_acceptance_radius()
+{
+ if (!get_vstatus()->is_rotary_wing) {
+ // The fixed-wing altitude acceptance radius default is the respective parameter. However, before a landing
+ // approach it needs to be tighter: Assume a 30% error w.r.t. the remaining descent altitude is OK, but enforce
+ // min/max values (e.g. min=3m to make sure that the waypoint can still be reached in case of wrong user input).
+
+ const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next;
+ const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current;
+
+ if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
+ return math::constrain(0.3f * (curr_sp.alt - next_sp.alt), 3.0f, _param_fw_alt_acceptance_radius.get());
+ }
+ }
+
+ return get_default_altitude_acceptance_radius();
+}
+
float
Navigator::get_cruising_speed()
{