From 45d7165eb33301a68d106ffac50a1bc30b41f5b7 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 3 Jan 2019 13:59:54 -0500
Subject: [PATCH] fw_pos_control_l1 skip wind > max airspeed logic if airspeed
invalid
- cleanup airspeed validity check logic
---
.../FixedwingPositionControl.cpp | 57 ++++++++++---------
.../FixedwingPositionControl.hpp | 2 +-
2 files changed, 30 insertions(+), 29 deletions(-)
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 724ae6e74b..b3723fd763 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -374,35 +374,36 @@ FixedwingPositionControl::manual_control_setpoint_poll()
void
FixedwingPositionControl::airspeed_poll()
{
- if (!_parameters.airspeed_disabled && _sub_airspeed.update()) {
+ bool airspeed_valid = _airspeed_valid;
- const airspeed_s &airspeed = _sub_airspeed.get();
+ if (!_parameters.airspeed_disabled && _sub_airspeed.update()) {
- _airspeed_valid = PX4_ISFINITE(airspeed.indicated_airspeed_m_s)
- && PX4_ISFINITE(airspeed.true_airspeed_m_s)
- && (airspeed.indicated_airspeed_m_s > 0.0f);
+ const airspeed_s &as = _sub_airspeed.get();
- _airspeed_last_received = hrt_absolute_time();
- _airspeed = airspeed.indicated_airspeed_m_s;
+ if (PX4_ISFINITE(as.indicated_airspeed_m_s)
+ && PX4_ISFINITE(as.true_airspeed_m_s)
+ && (as.indicated_airspeed_m_s > 0.0f)) {
- if (_airspeed_valid
- && airspeed.true_airspeed_m_s > airspeed.indicated_airspeed_m_s) {
+ airspeed_valid = true;
- _eas2tas = max(airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s, 1.0f);
+ _airspeed_last_valid = as.timestamp;
+ _airspeed = as.indicated_airspeed_m_s;
- } else {
- _eas2tas = 1.0f;
+ _eas2tas = constrain(as.true_airspeed_m_s / as.indicated_airspeed_m_s, 0.9f, 2.0f);
}
} else {
- /* no airspeed updates for one second */
- if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_received) > 1e6) {
- _airspeed_valid = false;
+ // no airspeed updates for one second
+ if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) {
+ airspeed_valid = false;
}
}
- /* update TECS state */
- _tecs.enable_airspeed(_airspeed_valid);
+ // update TECS if validity changed
+ if (airspeed_valid != _airspeed_valid) {
+ _tecs.enable_airspeed(airspeed_valid);
+ _airspeed_valid = airspeed_valid;
+ }
}
void
@@ -820,20 +821,20 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
- // l1 navigation logic breaks down when wind speed exceeds max airspeed
- // compute 2D groundspeed from airspeed-heading projection
- Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
- Vector2f nav_speed_2d{0.0f, 0.0f};
+ Vector2f nav_speed_2d{ground_speed};
- // angle between air_speed_2d and ground_speed
- float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
+ if (_airspeed_valid) {
+ // l1 navigation logic breaks down when wind speed exceeds max airspeed
+ // compute 2D groundspeed from airspeed-heading projection
+ const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
- // if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
- if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
- nav_speed_2d = air_speed_2d;
+ // angle between air_speed_2d and ground_speed
+ const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
- } else {
- nav_speed_2d = ground_speed;
+ // if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
+ if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
+ nav_speed_2d = air_speed_2d;
+ }
}
/* no throttle limit as default */
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
index 9f79c91760..72854c83f9 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -233,7 +233,7 @@ private:
/* throttle and airspeed states */
bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists
- hrt_abstime _airspeed_last_received{0}; ///< last time airspeed was received. Used to detect timeouts.
+ hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts.
float _airspeed{0.0f};
float _eas2tas{1.0f};
--
GitLab