Skip to content

Commit

Permalink
Update
Browse files Browse the repository at this point in the history
  • Loading branch information
haitomatic committed Aug 26, 2024
1 parent b925819 commit a4c5b2b
Show file tree
Hide file tree
Showing 14 changed files with 626 additions and 39 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -142,7 +142,7 @@ param set-default VT_F_TRANS_DUR 6.0
param set-default VT_F_TRANS_THR 1.0

# VTOL takeoff
param set-default VTO_LOITER_ALT 20
param set-default VTO_LOITER_ALT 20.0

param set-default MC_AIRMODE 0
param set-default MC_ROLLRATE_P 0.14
Expand Down
6 changes: 0 additions & 6 deletions ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x
Original file line number Diff line number Diff line change
Expand Up @@ -49,12 +49,6 @@ param set-default PWM_MAIN_MIN2 1050
param set-default PWM_MAIN_MIN3 1050
param set-default PWM_MAIN_MIN4 1050

# HITL PWM functions
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

# UAVCAN_EC functions
param set-default UAVCAN_EC_FUNC1 101
param set-default UAVCAN_EC_FUNC2 102
Expand Down
5 changes: 0 additions & 5 deletions ROMFS/px4fmu_common/init.d/airframes/4421_ssrc_skywalker
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,6 @@ param set-default PWM_MAIN_FUNC1 201
param set-default PWM_MAIN_FUNC2 202
param set-default PWM_MAIN_FUNC4 101

# HITL PWM functions
param set-default HIL_ACT_FUNC1 201
param set-default HIL_ACT_FUNC2 202
param set-default HIL_ACT_FUNC4 101

# Airspeed parameters
param set-default SENS_EN_MS4525DO 1
param set-default ASPD_DO_CHECKS 15
Expand Down
12 changes: 0 additions & 12 deletions ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini
Original file line number Diff line number Diff line change
Expand Up @@ -70,18 +70,6 @@ param set-default PWM_AUX_FUNC4 204
param set-default PWM_AUX_FUNC5 205
param set-default PWM_AUX_FUNC6 105

# HITL PWM functions
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 203
param set-default HIL_ACT_FUNC8 204
param set-default HIL_ACT_FUNC9 205
param set-default HIL_ACT_FUNC10 105

# Start airspeed sensor driver
param set-default SENS_EN_MS4525DO 1
param set-default SENS_EN_SDP3X 1
Expand Down
2 changes: 1 addition & 1 deletion packaging/build_px4fw.sh
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ else
fi

# Remove old build output
rm -Rf build/${arg}
#rm -Rf build/${arg}
# Build
make ${arg}

Expand Down
6 changes: 4 additions & 2 deletions packaging/entrypoint_sitl_gzsim.sh
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,10 @@ case $PX4_VEHICLE_TYPE in
export PX4_GZ_MODEL=scout_mini
;;
vtol)
export PX4_SYS_AUTOSTART=4430
export PX4_GZ_MODEL=striver_mini
#export PX4_SYS_AUTOSTART=4430
#export PX4_GZ_MODEL=striver_mini
export PX4_SYS_AUTOSTART=4004
export PX4_GZ_MODEL=standard_vtol
;;
fw)
export PX4_SYS_AUTOSTART=4440
Expand Down
22 changes: 14 additions & 8 deletions src/modules/simulation/pwm_out_sim/PWMSim.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,10 +44,16 @@
PWMSim::PWMSim(bool hil_mode_enabled) :
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
{
_mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC);
_mixing_output.setAllFailsafeValues(PWM_SIM_FAILSAFE_MAGIC);
_mixing_output.setAllMinValues(PWM_SIM_PWM_MIN_MAGIC);
_mixing_output.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC);
for (int i = 0; i < MAX_ACTUATORS; ++i) {
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "MIN", i + 1);
param_get(param_find(param_name), &_pwm_min[i]);
snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "MAX", i + 1);
param_get(param_find(param_name), &_pwm_max[i]);
snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "DIS", i + 1);
param_get(param_find(param_name), &_pwm_disarmed[i]);
PX4_INFO("PWM %d: min: %d, max: %d, disarmed: %d", i, _pwm_min[i], _pwm_max[i], _pwm_disarmed[i]);
}

_mixing_output.setIgnoreLockdown(hil_mode_enabled);
}
Expand All @@ -69,7 +75,7 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un
const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();

for (int i = 0; i < (int)num_outputs; i++) {
if (outputs[i] != PWM_SIM_DISARMED_MAGIC) {
if (outputs[i] != _pwm_disarmed[i]) {

OutputFunction function = _mixing_output.outputFunction(i);
bool is_reversible = reversible_outputs & (1u << i);
Expand All @@ -78,12 +84,12 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un
if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)
&& !is_reversible) {
// Scale non-reversible motors to [0, 1]
actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC);
actuator_outputs.output[i] = (output - _pwm_min[i]) / (_pwm_max[i] - _pwm_min[i]);

} else {
// Scale everything else to [-1, 1]
const float pwm_center = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2.f;
const float pwm_delta = (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC) / 2.f;
const float pwm_center = (_pwm_max[i] + _pwm_min[i]) / 2.f;
const float pwm_delta = (_pwm_max[i] - _pwm_min[i]) / 2.f;
actuator_outputs.output[i] = (output - pwm_center) / pwm_delta;
}
}
Expand Down
7 changes: 3 additions & 4 deletions src/modules/simulation/pwm_out_sim/PWMSim.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -76,10 +76,9 @@ class PWMSim : public ModuleBase<PWMSim>, public OutputModuleInterface
private:
void Run() override;

static constexpr uint16_t PWM_SIM_DISARMED_MAGIC = 900;
static constexpr uint16_t PWM_SIM_FAILSAFE_MAGIC = 600;
static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC = 1000;
static constexpr uint16_t PWM_SIM_PWM_MAX_MAGIC = 2000;
int32_t _pwm_min[MAX_ACTUATORS] {};
int32_t _pwm_max[MAX_ACTUATORS] {};
int32_t _pwm_disarmed[MAX_ACTUATORS] {};

MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
Expand Down
5 changes: 5 additions & 0 deletions src/modules/simulation/pwm_out_sim/module_hil.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,9 @@ actuator_output:
output_groups:
- param_prefix: HIL_ACT
channel_label: Channel
standard_params:
disarmed: { min: 0, max: 1000, default: 0 }
min: { min: 0, max: 1000, default: 0 }
max: { min: 0, max: 1000, default: 1000 }
failsafe: { min: 0, max: 1000 }
num_channels: 16
119 changes: 119 additions & 0 deletions ssrc_config/config_hitl_eth_gzsim_fw.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,119 @@
# Default parameter set for HITL with ethernet Gazebo Sim connection on FW
# [ type: hitl_eth_gzsim ]

####################################
# general HITL configuration
####################################

# Set HITL related flag
param set SYS_HITL 1
param set MAV_HITL_SHOME 1
param set SENS_EN_GPSSIM 1
param set SENS_EN_BAROSIM 1
param set SENS_EN_MAGSIM 1
param set SENS_EN_ARSPDSIM 1

# Disable safety switch
param set COM_PREARM_MODE 0
param set CBRK_IO_SAFETY 22027

# Disable RC loss failsafe check
param set NAV_RCL_ACT 0

# Disable RC input requirement
param set COM_RC_IN_MODE 1

# EKF2
param set EKF2_MULTI_IMU 3

# Sensor
param set CAL_ACC0_ID 1310988
param set CAL_GYRO0_ID 1310988
param set CAL_ACC1_ID 1310996
param set CAL_GYRO1_ID 1310996
param set CAL_ACC2_ID 1311004
param set CAL_GYRO2_ID 1311004
param set CAL_MAG1_ID 197388
param set CAL_MAG1_PRIO 50


####################################
# specific fw HITL configuration
####################################

# Override airframe defaults
param set-default ASPD_DO_CHECKS 7
param set-default FW_LAUN_DETCN_ON 0

# HITL PWM functions
param set-default HIL_ACT_FUNC1 201
param set-default HIL_ACT_DIS1 500
param set-default HIL_ACT_FUNC2 202
param set-default HIL_ACT_DIS2 500
param set-default HIL_ACT_FUNC4 101
param set-default HIL_ACT_MIN4 150
param set-default HIL_ACT_MAX4 1000

# Control allocator parameters
param set-default CA_AIRFRAME 1
param set-default CA_ROTOR_COUNT 1
param set-default CA_SV_CS_COUNT 2
param set-default CA_SV_CS0_TYPE 5
param set-default CA_SV_CS0_TRQ_P 0.5
param set-default CA_SV_CS0_TRQ_R -0.5
param set-default CA_SV_CS1_TYPE 6
param set-default CA_SV_CS1_TRQ_P 0.5
param set-default CA_SV_CS1_TRQ_R 0.5

# Airspeed parameters
param set-default ASPD_PRIMARY 1
param set-default FW_AIRSPD_MAX 22.0
param set-default FW_AIRSPD_MIN 14.0
param set-default FW_AIRSPD_STALL 12.0
param set-default FW_AIRSPD_TRIM 18.0

# Maximum landing slope angle in deg
param set-default FW_LND_ANG 8.0

# RC loss failsafe to HOLD mode
param set-default COM_RC_IN_MODE 1

# Maximum manual roll angle
param set-default FW_MAN_R_MAX 60.0

# Fixed wing control
# Pitch rate
param set-default FW_PR_P 0.9
param set-default FW_PR_FF 0.5
param set-default FW_PR_I 0.5
param set-default TRIM_PITCH -0.15
# Pitch angle in deg
param set-default FW_PSP_OFF 0.0
param set-default FW_P_LIM_MIN -15.0
# Roll rate
param set-default FW_RR_FF 0.5
param set-default FW_RR_P 0.3
param set-default FW_RR_I 0.5
# Yaw rate
param set-default FW_YR_FF 0.5
param set-default FW_YR_P 0.6
param set-default FW_YR_I 0.5
#Throttle limit
param set-default FW_THR_MAX 0.6
param set-default FW_THR_MIN 0.05
param set-default FW_THR_TRIM 0.25
# Climb and sink rate
param set-default FW_T_CLMB_MAX 8.0
param set-default FW_T_SINK_MAX 2.7
param set-default FW_T_SINK_MIN 2.2

# Navigation
param set-default NAV_ACC_RAD 15.0
param set-default NAV_DLL_ACT 2

# Misc
param set-default RTL_RETURN_ALT 30.0
param set-default RTL_DESCEND_ALT 30.0
param set-default FW_LND_USETER 0
param set-default RWTO_TKOFF 1
param set-default FD_ESCS_EN 0
80 changes: 80 additions & 0 deletions ssrc_config/config_hitl_eth_gzsim_mc.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,80 @@
# Default parameter set for HITL with ethernet Gazebo Sim connection on MC
# [ type: hitl_eth_gzsim ]

####################################
# general HITL configuration
####################################

# Set HITL related flag
param set SYS_HITL 1
param set MAV_HITL_SHOME 1
param set SENS_EN_GPSSIM 1
param set SENS_EN_BAROSIM 1
param set SENS_EN_MAGSIM 1

# Disable safety switch
param set COM_PREARM_MODE 0
param set CBRK_IO_SAFETY 22027

# Disable RC loss failsafe check
param set NAV_RCL_ACT 0

# Disable RC input requirement
param set COM_RC_IN_MODE 1

# EKF2
param set EKF2_MULTI_IMU 3

# Sensor
param set CAL_ACC0_ID 1310988
param set CAL_GYRO0_ID 1310988
param set CAL_ACC1_ID 1310996
param set CAL_GYRO1_ID 1310996
param set CAL_ACC2_ID 1311004
param set CAL_GYRO2_ID 1311004
param set CAL_MAG1_ID 197388
param set CAL_MAG1_PRIO 50


####################################
# specific mc HITL configuration
####################################

# HITL PWM functions
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 CA_ROTOR0_PX 0.175
param set-default CA_ROTOR0_PY 0.175
param set-default CA_ROTOR0_KM 0.05

param set-default CA_ROTOR1_PX -0.175
param set-default CA_ROTOR1_PY -0.175
param set-default CA_ROTOR1_KM 0.05

param set-default CA_ROTOR2_PX 0.175
param set-default CA_ROTOR2_PY -0.175
param set-default CA_ROTOR2_KM -0.05

param set-default CA_ROTOR3_PX -0.175
param set-default CA_ROTOR3_PY 0.175
param set-default CA_ROTOR3_KM -0.05

# extra
param set-default MPC_THR_HOVER 0.60
param set COM_RCL_EXCEPT 4
param set NAV_DLL_ACT 0
param set NAV_RCL_ACT 0
param set MAV_0_BROADCAST 1
param set IMU_GYRO_CUTOFF 60
param set IMU_DGYRO_CUTOFF 30
param set MC_ROLLRATE_P 0.14
param set MC_PITCHRATE_P 0.14
param set MC_ROLLRATE_I 0.3
param set MC_PITCHRATE_I 0.3
param set MC_ROLLRATE_D 0.004
param set MC_PITCHRATE_D 0.004
param set BAT_N_CELLS 4
param set SDLOG_MODE 0
Loading

0 comments on commit a4c5b2b

Please sign in to comment.