fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

all changes

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2024-09-27 11:29:41 +03:00
parent d4defb7a81
commit 580c196600
2 changed files with 19 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil
View File

@ -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

15
src/modules/simulation/simulator_sih/sih.cpp
View File

@ -76,7 +76,7 @@ void Sih::run()
_airspeed_time = task_start;
_dist_snsr_time = task_start;
_vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast<typeof _sih_vtype.get()>(0),
static_cast<typeof _sih_vtype.get()>(2));
static_cast<typeof _sih_vtype.get()>(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)