From 45582fcf550473467c20a4b97bb32bb74f416497 Mon Sep 17 00:00:00 2001 From: Balduin Date: Fri, 31 Jan 2025 17:02:14 +0100 Subject: [PATCH] attempt at fixing xvert SensorAirspeedSim has hardcoded pitot tube direction in body front velocity. for tailsitter, this points DOWN in forward flight, thus the simulated sensor would read 0 in forward flight for tailsitters. fixed by using norm(velocity) instead like in sih. --- .../init.d-posix/airframes/10042_sihsim_xvert | 1 + .../sensor_airspeed_sim/SensorAirspeedSim.cpp | 15 +++++++++++++-- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert index eac28c8cdb..a644767973 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert @@ -17,6 +17,7 @@ param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 1 param set-default VT_B_TRANS_DUR 5 param set-default VT_ELEV_MC_LOCK 0 diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index 6d3ee9d0c8..512fb3bce3 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -140,8 +140,19 @@ void SensorAirspeedSim::Run() // calculate differential pressure + noise in hPa float _noise_scale = _sih_noise_scale.get(); const float diff_pressure_noise = _noise_scale * (float)generate_wgn() * 0.01f; - float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density * body_velocity(0) * body_velocity( - 0) + diff_pressure_noise; + + // as before, always body "forward" direction, but that is wrong for TS + // float body_speed = body_velocity(0); + + // hacky fix: just take norm of vel. + // inaccurate because flying sideways would not give significant pitot tube readings. + // but then again fixed wings can't really fly sideways at significant speed. + // also, wind is not considered at all here...? + float body_speed = body_velocity.norm(); + // even nicer would be to define the pitot tube direction and do: + // body_speed = pitot_direction.T @ body_velocity + + float diff_pressure = sign(body_speed) * 0.005f * air_density * body_speed * body_speed + diff_pressure_noise;