diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 724ae6e74b68..b3723fd76334 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 9f79c9176091..72854c83f966 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -233,7 +233,7 @@ class FixedwingPositionControl final : public ModuleBase