From d4defb7a81fb573260c4c33f937d9b4ce3c50fa5 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 27 Sep 2024 09:40:06 +0300 Subject: [PATCH 1/6] commit all changes Signed-off-by: RomanBapst --- .../airframes/1103_standard_vtol_sih.hil | 103 ++++++++++++++++++ .../init.d/airframes/CMakeLists.txt | 1 + src/modules/simulation/simulator_sih/sih.cpp | 28 +++-- src/modules/simulation/simulator_sih/sih.hpp | 6 +- .../simulation/simulator_sih/sih_params.c | 1 + 5 files changed, 128 insertions(+), 11 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil 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 new file mode 100644 index 0000000000..a82f6ee716 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil @@ -0,0 +1,103 @@ +#!/bin/sh +# +# @name SIH Tailsitter Duo +# +# @type Simulation +# @class VTOL +# +# @output Motor1 motor right +# @output Motor2 motor left +# @output Servo1 elevon right +# @output Servo2 elevon left +# +# @maintainer Romain Chiappinelli +# +# @board px4_fmu-v2 exclude +# + +. ${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_ROTOR_COUNT 5 +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_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_ROTOR3_PX -0.2 +param set-default CA_ROTOR3_PY 0.2 +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 + +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_CS0_TRQ_R 0.5 +param set-default CA_SV_CS0_TYPE 2 +param set-default CA_SV_CS1_TRQ_P 1 +param set-default CA_SV_CS1_TYPE 3 +param set-default CA_SV_CS2_TRQ_Y 1 + +param set-default FW_AIRSPD_MAX 12 +param set-default FW_AIRSPD_MIN 7 +param set-default FW_AIRSPD_TRIM 10 + +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +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 MAV_TYPE 19 + + + +# set SYS_HITL to 2 to start the SIH and avoid sensors startup +param set-default SYS_HITL 2 + +# disable some checks to allow to fly: +# - without real battery +param set-default CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set-default CBRK_IO_SAFETY 22027 + +param set-default SENS_DPRES_OFF 0.001 + +param set SIH_T_MAX 2.0 +param set SIH_Q_MAX 0.0165 +param set SIH_MASS 0.2 +# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg +param set SIH_IXX 0.00354 +param set SIH_IYY 0.000625 +param set SIH_IZZ 0.00300 +param set SIH_IXZ 0 +param set SIH_KDV 0.2 +param set SIH_L_ROLL 0.145 + +# sih as tailsitter +param set SIH_VEHICLE_TYPE 3 + +param set-default VT_ARSP_TRANS 6 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 634eb3e1b8..4a767f6595 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -48,6 +48,7 @@ if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM) 1100_rc_quad_x_sih.hil 1101_rc_plane_sih.hil 1102_tailsitter_duo_sih.hil + 1103_standard_vtol_sih.hil ) endif() diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 2ca4987338..7a02c5ec73 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -216,7 +216,8 @@ void Sih::sensor_step() reconstruct_sensors_signals(now); - if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && now - _airspeed_time >= 50_ms) { + if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) + && now - _airspeed_time >= 50_ms) { _airspeed_time = now; send_airspeed(now); } @@ -311,7 +312,7 @@ void Sih::generate_force_and_torques() _T_B = Vector3f(_T_MAX * _u[3], 0.0f, 0.0f); // forward thruster // _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); // thruster torque _Mt_B = Vector3f(); - generate_fw_aerodynamics(); + generate_fw_aerodynamics(_u[0], _u[1], _u[2], _u[3]); } else if (_vehicle == VehicleType::TS) { _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (_u[0] + _u[1])); @@ -320,17 +321,24 @@ void Sih::generate_force_and_torques() // _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft // _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])); + _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])); + generate_fw_aerodynamics(_u[4], _u[5], _u[6], 0); } } -void Sih::generate_fw_aerodynamics() +void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust) { _v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s] float altitude = _H0 - _p_I(2); - _wing_l.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX); - _wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX); - _tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]); - _fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]); + _wing_l.update_aero(_v_B, _w_B, altitude, roll_cmd * FLAP_MAX); + _wing_r.update_aero(_v_B, _w_B, altitude, -roll_cmd * FLAP_MAX); + _tailplane.update_aero(_v_B, _w_B, altitude, pitch_cmd * FLAP_MAX, _T_MAX * thrust); + _fin.update_aero(_v_B, _w_B, altitude, yaw_cmd * FLAP_MAX, _T_MAX * thrust); _fuselage.update_aero(_v_B, _w_B, altitude); // sum of aerodynamic forces @@ -383,7 +391,7 @@ void Sih::equations_of_motion(const float dt) // fake ground, avoid free fall if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) { - if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) { + if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) { if (!_grounded) { // if we just hit the floor // for the accelerometer, compute the acceleration that will stop the vehicle in one time step _v_I_dot = -_v_I / dt; @@ -457,6 +465,7 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) 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) @@ -619,6 +628,9 @@ int Sih::print_status() PX4_INFO("aoa [deg]: %d", (int)(degrees(_ts[4].get_aoa()))); PX4_INFO("v segment (m/s)"); _ts[4].get_vS().print(); + + } else if (_vehicle == VehicleType::SVTOL) { + PX4_INFO("Running Standard VTOL"); } PX4_INFO("vehicle landed: %d", _grounded); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 8988474e01..d6ac768081 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -132,7 +132,7 @@ private: uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)}; // hard constants - static constexpr uint16_t NB_MOTORS = 6; + static constexpr uint16_t NB_MOTORS = 8; static constexpr float T1_C = 15.0f; // ground temperature in Celsius static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius; // ground temperature in Kelvin static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre @@ -159,7 +159,7 @@ private: void send_airspeed(const hrt_abstime &time_now_us); void send_dist_snsr(const hrt_abstime &time_now_us); void publish_ground_truth(const hrt_abstime &time_now_us); - void generate_fw_aerodynamics(); + void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust); void generate_ts_aerodynamics(); void sensor_step(); @@ -200,7 +200,7 @@ private: matrix::Vector3f _w_B_dot{}; // body rates differential float _u[NB_MOTORS] {}; // thruster signals - enum class VehicleType {MC, FW, TS}; + enum class VehicleType {MC, FW, TS, SVTOL}; VehicleType _vehicle = VehicleType::MC; // aerodynamic segments for the fixedwing diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index 16c58430f6..f9e053a5eb 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/src/modules/simulation/simulator_sih/sih_params.c @@ -332,6 +332,7 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f); * @value 0 Multicopter * @value 1 Fixed-Wing * @value 2 Tailsitter + * @value 3 Standard VTOL * @reboot_required true * @group Simulation In Hardware */ From 580c1966009ef2f164d55813a12928b515bc2cfd Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 27 Sep 2024 11:29:41 +0300 Subject: [PATCH 2/6] 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) From 0b7cddeca0d588a9badfb8ce5f35a687b0222f31 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 27 Sep 2024 11:36:38 +0300 Subject: [PATCH 3/6] reverse pitch Signed-off-by: RomanBapst --- ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil | 2 ++ 1 file changed, 2 insertions(+) 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 a4ab53347d..adf846807e 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 @@ -52,6 +52,8 @@ param set-default CA_SV_CS1_TRQ_P 1 param set-default CA_SV_CS1_TYPE 3 param set-default CA_SV_CS2_TRQ_Y 1 +param set HIL_ACT_REV 32 + param set-default FW_AIRSPD_MAX 12 param set-default FW_AIRSPD_MIN 7 param set-default FW_AIRSPD_TRIM 10 From 84150189d4b7fce5f6a5dfcca6aea674c815cc32 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 27 Sep 2024 14:46:45 +0300 Subject: [PATCH 4/6] added some more changes Signed-off-by: RomanBapst --- ROMFS/px4fmu_common/init.d/rcS | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 28804361d1..2c14c39007 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -105,6 +105,13 @@ then fi fi + if [ -e /fs/microsd/new ] + then + echo "Updating external autostart files" + rm -r $SDCARD_EXT_PATH + mv /fs/microsd/new $SDCARD_EXT_PATH + fi + # Check for an update of the ext_autostart folder, and replace the old one with it if [ -e /fs/microsd/ext_autostart_new ] then From d847f2051535d3399c13f3676479cd1794bd0e4a Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 2 Oct 2024 17:18:30 +0300 Subject: [PATCH 5/6] enable sih for v6x Signed-off-by: RomanBapst --- boards/px4/fmu-v6x/default.px4board | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index eedfd4c90e..b83b438144 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -23,26 +23,27 @@ CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y -CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=n +CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=n CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_OSD_MSP_OSD=y -CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=n CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y -CONFIG_DRIVERS_UAVCAN=y +CONFIG_DRIVERS_UAVCAN=n CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_BATTERY_STATUS=y From f97ca30c3f1f314f08644d9fc862e8de62e209ac Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 4 Oct 2024 10:03:26 +0300 Subject: [PATCH 6/6] small changes to be able to configure two ailerons Signed-off-by: RomanBapst --- src/modules/simulation/simulator_sih/sih.cpp | 4 ++-- src/modules/simulation/simulator_sih/sih.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 03d02507ca..4e044f52fe 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -323,11 +323,11 @@ 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 * 2 * _u[7], 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); + _T_B = Vector3f(_T_MAX * 2 * _u[8], 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])); - generate_fw_aerodynamics(_u[4], _u[5], _u[6], 0); + generate_fw_aerodynamics(_u[4], _u[6], _u[7], 0); } } diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index d6ac768081..fcbd1431b1 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -132,7 +132,7 @@ private: uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)}; // hard constants - static constexpr uint16_t NB_MOTORS = 8; + static constexpr uint16_t NB_MOTORS = 9; static constexpr float T1_C = 15.0f; // ground temperature in Celsius static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius; // ground temperature in Kelvin static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre