From 580c1966009ef2f164d55813a12928b515bc2cfd Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 27 Sep 2024 11:29:41 +0300 Subject: [PATCH] all changes Signed-off-by: RomanBapst --- .../airframes/1103_standard_vtol_sih.hil | 22 +++++++------------ src/modules/simulation/simulator_sih/sih.cpp | 15 +++++++++---- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil index a82f6ee716..a4ab53347d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil @@ -17,35 +17,30 @@ . ${R}etc/init.d/rc.vtol_defaults -param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters - param set UAVCAN_ENABLE 0 param set-default VT_B_TRANS_DUR 5 param set-default VT_ELEV_MC_LOCK 0 param set-default VT_TYPE 2 -param set-default VT_FW_DIFTHR_EN 1 -param set-default VT_FW_DIFTHR_S_Y 0.3 param set-default MPC_MAN_Y_MAX 60 param set-default MC_PITCH_P 5 -param set-default CA_AIRFRAME 3 +param set-default CA_AIRFRAME 2 param set-default CA_ROTOR_COUNT 5 -param set-default CA_ROTOR0_KM -0.05 +param set-default CA_ROTOR0_KM 0.05 param set-default CA_ROTOR0_PX 0.2 param set-default CA_ROTOR0_PY 0.2 -param set-default CA_ROTOR1_KM -0.05 +param set-default CA_ROTOR1_KM 0.05 param set-default CA_ROTOR1_PX -0.2 param set-default CA_ROTOR1_PY -0.2 param set-default CA_ROTOR2_PX 0.2 param set-default CA_ROTOR2_PY -0.2 -param set-default CA_ROTOR2_KM 0.05 +param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -0.2 param set-default CA_ROTOR3_PY 0.2 -param set-default CA_ROTOR3_KM 0.05 +param set-default CA_ROTOR3_KM -0.05 param set-default CA_ROTOR4_PX -0.3 -param set-default CA_ROTOR4_PY 0.2 param set-default CA_ROTOR4_KM 0.05 param set-default CA_ROTOR4_AX 1 param set-default CA_ROTOR4_AZ 0 @@ -67,11 +62,10 @@ param set-default HIL_ACT_FUNC3 103 param set-default HIL_ACT_FUNC4 104 param set-default HIL_ACT_FUNC5 201 param set-default HIL_ACT_FUNC6 202 -param set-default HIL_ACT_FUNC7 202 -param set-default HIL_ACT_FUNC7 105 -param set-default HIL_ACT_REV 32 +param set-default HIL_ACT_FUNC7 203 +param set-default HIL_ACT_FUNC8 105 -param set-default MAV_TYPE 19 +param set-default MAV_TYPE 22 diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 7a02c5ec73..03d02507ca 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -76,7 +76,7 @@ void Sih::run() _airspeed_time = task_start; _dist_snsr_time = task_start; _vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast(0), - static_cast(2)); + static_cast(3)); _actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; @@ -323,7 +323,7 @@ void Sih::generate_force_and_torques() // _Ma_B = -_KDW * _w_B; // first order angular damper } else if (_vehicle == VehicleType::SVTOL) { - _T_B = Vector3f(_T_MAX * _u[7], 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); + _T_B = Vector3f(_T_MAX * 2 * _u[7], 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]), _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]), _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3])); @@ -458,14 +458,21 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) // TODO: send differential pressure instead? airspeed_s airspeed{}; airspeed.timestamp_sample = time_now_us; + // airspeed sensor is mounted along the negative Z axis since the vehicle is a tailsitter - airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f); + if (_vehicle == VehicleType::TS) { + + airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f); + + } else { + airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B(0) + generate_wgn() * 0.2f); + } + airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); airspeed.air_temperature_celsius = NAN; airspeed.confidence = 0.7f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); - PX4_INFO("test"); } void Sih::send_dist_snsr(const hrt_abstime &time_now_us)