From 982e204c39452b4ac2a095a4044a3e0e885da5e0 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 26 Sep 2024 15:10:35 +0200 Subject: [PATCH] harden body vel x for airspeed Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 4a20c0cf69..ff7e110cf3 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -321,11 +321,16 @@ FixedwingPositionControl::vehicle_attitude_poll() _pitch = euler_angles(1); _yaw = euler_angles(2); - Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; + const Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; _body_acceleration_x = body_acceleration(0); - Vector3f body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz}; - _body_velocity_x = body_velocity(0); + if (_local_pos.v_xy_valid) { + const Vector3f body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz}; + _body_velocity_x = body_velocity(0); + + } else { + _body_velocity_x = NAN; + } // load factor due to banking _tecs.set_load_factor(getLoadFactor()); @@ -372,17 +377,16 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, } // Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained - if (!_wind_valid && !in_takeoff_situation) { + if (!_wind_valid && !in_takeoff_situation && PX4_ISFINITE(_body_velocity_x) && _body_velocity_x > FLT_EPSILON) { /* * This error value ensures that a plane (as long as its throttle capability is * not exceeded) travels towards a waypoint (and is not pushed more and more away * by wind). Not countering this would lead to a fly-away. Only non-zero in presence * of sufficient wind. "minimum ground speed undershoot". */ - const float ground_speed_body = _body_velocity_x; - if (ground_speed_body < _param_fw_gnd_spd_min.get()) { - calibrated_airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body; + if (_body_velocity_x < _param_fw_gnd_spd_min.get()) { + calibrated_airspeed_setpoint += _param_fw_gnd_spd_min.get() - _body_velocity_x; } }