Skip to content

Commit

Permalink
Simulation: deprecate pwm_out_sim module HITL mode and use only pwm_e…
Browse files Browse the repository at this point in the history
…sc module
  • Loading branch information
haitomatic committed Dec 16, 2024
1 parent 202c303 commit 9f65415
Show file tree
Hide file tree
Showing 18 changed files with 208 additions and 313 deletions.
8 changes: 4 additions & 4 deletions ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil
Original file line number Diff line number Diff line change
Expand Up @@ -28,10 +28,10 @@ param set-default CA_ROTOR3_PX -0.15
param set-default CA_ROTOR3_PY 0.15
param set-default CA_ROTOR3_KM -0.05

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 PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104

# disable some checks to allow to fly
# - without real battery
Expand Down
16 changes: 8 additions & 8 deletions ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
Original file line number Diff line number Diff line change
Expand Up @@ -77,14 +77,14 @@ param set-default CA_SV_CS1_TYPE 2
param set-default CA_SV_CS2_TRQ_P 1.0
param set-default CA_SV_CS2_TYPE 3

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 105
param set-default HIL_ACT_FUNC6 201
param set-default HIL_ACT_FUNC7 202
param set-default HIL_ACT_FUNC8 203
param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
param set-default PWM_MAIN_FUNC5 105
param set-default PWM_MAIN_FUNC6 201
param set-default PWM_MAIN_FUNC7 202
param set-default PWM_MAIN_FUNC8 203


param set SYS_HITL 1
Expand Down
8 changes: 4 additions & 4 deletions ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil
Original file line number Diff line number Diff line change
Expand Up @@ -27,10 +27,10 @@ param set-default CA_ROTOR3_PX -0.15
param set-default CA_ROTOR3_PY 0.15
param set-default CA_ROTOR3_KM -0.05

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 PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104

# set SYS_HITL to 2 to start the SIH and avoid sensors startup
param set SYS_HITL 2
Expand Down
14 changes: 7 additions & 7 deletions ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil
Original file line number Diff line number Diff line change
Expand Up @@ -27,13 +27,13 @@ param set-default CA_SV_CS2_TRQ_Y 1.0
param set-default CA_SV_CS2_TYPE 4
param set-default CA_SV_CS3_TYPE 10

param set-default HIL_ACT_REV 2
param set-default HIL_ACT_FUNC1 201
param set-default HIL_ACT_FUNC2 202
param set-default HIL_ACT_FUNC3 203
param set-default HIL_ACT_FUNC4 101
param set-default HIL_ACT_FUNC5 204
param set-default HIL_ACT_FUNC6 400
param set-default PWM_MAIN_REV 2
param set-default PWM_MAIN_FUNC1 201
param set-default PWM_MAIN_FUNC2 202
param set-default PWM_MAIN_FUNC3 203
param set-default PWM_MAIN_FUNC4 101
param set-default PWM_MAIN_FUNC5 204
param set-default PWM_MAIN_FUNC6 400

# set SYS_HITL to 2 to start the SIH and avoid sensors startup
param set-default SYS_HITL 2
Expand Down
10 changes: 5 additions & 5 deletions ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil
Original file line number Diff line number Diff line change
Expand Up @@ -43,11 +43,11 @@ param set-default CA_SV_CS1_TRQ_P 0.3
param set-default CA_SV_CS1_TRQ_Y -0.3
param set-default CA_SV_CS1_TYPE 6

param set-default HIL_ACT_FUNC1 101
param set-default HIL_ACT_FUNC2 102
param set-default HIL_ACT_FUNC5 202
param set-default HIL_ACT_FUNC6 201
param set-default HIL_ACT_REV 32
param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC5 202
param set-default PWM_MAIN_FUNC6 201
param set-default PWM_MAIN_REV 32

param set-default MAV_TYPE 19

Expand Down
23 changes: 19 additions & 4 deletions ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil
Original file line number Diff line number Diff line change
Expand Up @@ -32,10 +32,25 @@ param set-default CA_ROTOR3_PX -0.175
param set-default CA_ROTOR3_PY 0.175
param set-default CA_ROTOR3_KM -0.05

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 PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104

param set-default PWM_MAIN_MIN1 0
param set-default PWM_MAIN_MIN2 0
param set-default PWM_MAIN_MIN3 0
param set-default PWM_MAIN_MIN4 0

param set-default PWM_MAIN_MAX1 1000
param set-default PWM_MAIN_MAX2 1000
param set-default PWM_MAIN_MAX3 1000
param set-default PWM_MAIN_MAX4 1000

param set-default PWM_MAIN_DIS1 0
param set-default PWM_MAIN_DIS2 0
param set-default PWM_MAIN_DIS3 0
param set-default PWM_MAIN_DIS4 0

# Set takeoff ramp to disabled for a more decisive takeoff action
param set-default MPC_TKO_RAMP_T 0
55 changes: 34 additions & 21 deletions ROMFS/px4fmu_common/init.d/airframes/1404_ssrc_standard_vtol.hil
Original file line number Diff line number Diff line change
Expand Up @@ -30,28 +30,41 @@ param set-default CA_ROTOR4_AZ 0.0
param set-default CA_ROTOR4_PX 0.2

# HITL PWM functions
param set-default HIL_ACT_FUNC1 101
param set-default HIL_ACT_MIN1 10
param set-default HIL_ACT_MAX1 1500
param set-default HIL_ACT_FUNC2 102
param set-default HIL_ACT_MIN2 10
param set-default HIL_ACT_MAX2 1500
param set-default HIL_ACT_FUNC3 103
param set-default HIL_ACT_MIN3 10
param set-default HIL_ACT_MAX3 1500
param set-default HIL_ACT_FUNC4 104
param set-default HIL_ACT_MIN4 10
param set-default HIL_ACT_MAX4 1500
param set-default HIL_ACT_FUNC5 105
param set-default HIL_ACT_MIN5 0
param set-default HIL_ACT_MAX5 3500
param set-default HIL_ACT_FUNC6 201
param set-default HIL_ACT_DIS6 500
param set-default HIL_ACT_FUNC7 202
param set-default HIL_ACT_DIS7 500
param set-default HIL_ACT_FUNC8 203
param set-default HIL_ACT_DIS8 500
param set PWM_MAIN_FUNC1 101
param set PWM_MAIN_FUNC2 102
param set PWM_MAIN_FUNC3 103
param set PWM_MAIN_FUNC4 104
param set PWM_MAIN_FUNC5 105
param set PWM_MAIN_FUNC6 201
param set PWM_MAIN_FUNC7 202
param set PWM_MAIN_FUNC8 203

param set PWM_MAIN_MIN1 10
param set PWM_MAIN_MIN2 10
param set PWM_MAIN_MIN3 10
param set PWM_MAIN_MIN4 10
param set PWM_MAIN_MIN5 0
param set PWM_MAIN_MIN6 0
param set PWM_MAIN_MIN7 0
param set PWM_MAIN_MIN8 0

param set PWM_MAIN_MAX1 1500
param set PWM_MAIN_MAX2 1500
param set PWM_MAIN_MAX3 1500
param set PWM_MAIN_MAX4 1500
param set PWM_MAIN_MAX5 3500
param set PWM_MAIN_MAX6 1000
param set PWM_MAIN_MAX7 1000
param set PWM_MAIN_MAX8 1000

param set PWM_MAIN_DIS1 0
param set PWM_MAIN_DIS2 0
param set PWM_MAIN_DIS3 0
param set PWM_MAIN_DIS4 0
param set PWM_MAIN_DIS5 0
param set PWM_MAIN_DIS6 500
param set PWM_MAIN_DIS7 500
param set PWM_MAIN_DIS8 500

param set-default ASPD_PRIMARY 1

Expand Down
22 changes: 15 additions & 7 deletions ROMFS/px4fmu_common/init.d/airframes/1440_ssrc_skywalker_x8.hil
Original file line number Diff line number Diff line change
Expand Up @@ -21,13 +21,21 @@ param set-default CA_SV_CS1_TRQ_P 0.5
param set-default CA_SV_CS1_TRQ_R 0.5

# 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
param set PWM_MAIN_FUNC1 201
param set PWM_MAIN_FUNC2 202
param set PWM_MAIN_FUNC4 101

param set PWM_MAIN_MIN1 0
param set PWM_MAIN_MIN2 0
param set PWM_MAIN_MIN4 150

param set PWM_MAIN_MAX1 1000
param set PWM_MAIN_MAX2 1000
param set PWM_MAIN_MAX4 3000

param set PWM_MAIN_DIS1 500
param set PWM_MAIN_DIS2 500
param set PWM_MAIN_DIS4 0

# Airspeed parameters
param set-default ASPD_PRIMARY 1
Expand Down
6 changes: 0 additions & 6 deletions ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor
Original file line number Diff line number Diff line change
Expand Up @@ -51,12 +51,6 @@ param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104

# 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

# Increase velocity controller P gain
param set-default MPC_XY_VEL_P_ACC 2.4

Expand Down
5 changes: 0 additions & 5 deletions ROMFS/px4fmu_common/init.d/rcS
Original file line number Diff line number Diff line change
Expand Up @@ -355,11 +355,6 @@ else
#
if param greater SYS_HITL 0
then
if ! pwm_out_sim start -m hil
then
tune_control play error
fi

sensors start -h
commander start -h
# disable GPS
Expand Down
1 change: 0 additions & 1 deletion boards/px4/sitl/default.px4board
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_OSD_MSP_OSD=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_PWM_OUT_SIM=y
CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
Expand Down
16 changes: 11 additions & 5 deletions src/drivers/pwm_esc/pwm_esc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@

/**
* @file pwm_esc.cpp
* Driver for the NuttX PWM driver controleed escs
* Driver for the NuttX PWM driver controlled escs
*
*/

Expand Down Expand Up @@ -352,19 +352,25 @@ PWMESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne
} else {
// In hitl, publish actuator_outputs_sim
// Only publish once we receive actuator_controls (important for lock-step to work correctly)
actuator_outputs_s actuator_outputs_sim{};

if (num_control_groups_updated > 0) {
actuator_outputs_s actuator_outputs_sim{};
actuator_outputs_sim.noutputs = num_outputs;

const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();

for (int i = 0; i < (int)num_outputs; i++) {
uint16_t disarmed = _mixing_output.disarmedValue(i);
uint16_t min = _mixing_output.minValue(i);
uint16_t max = _mixing_output.maxValue(i);

OutputFunction function = _mixing_output.outputFunction(i);
bool is_reversible = reversible_outputs & (1u << i);
float output = outputs[i];

if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)) {
// Scale motors to [0, 1]
if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax
&& !is_reversible)) {
// Scale non-reversible motors to [0, 1]
actuator_outputs_sim.output[i] = (output - disarmed) / (max - disarmed);

} else {
Expand Down Expand Up @@ -633,7 +639,7 @@ int PWMESC::print_usage(const char *reason)
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Driver for PWM outputs.
Driver for PWM outputs. Used also in HITL mode.
)DESCR_STR");

Expand Down
4 changes: 0 additions & 4 deletions src/lib/mixer_module/mixer_module_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,7 @@

#include "mixer_module.hpp"

#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
#define PARAM_PREFIX "PWM_MAIN"
#else
#define PARAM_PREFIX "HIL_ACT"
#endif

static constexpr int max_num_outputs = 8;

Expand Down
12 changes: 10 additions & 2 deletions src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,16 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams

for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) {
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_%s%d", "HIL_ACT", "FUNC", i + 1);
param_get(param_find(param_name), &_output_functions[i]);
snprintf(param_name, sizeof(param_name), "%s_%s%d", "PWM_MAIN", "FUNC", i + 1);
param_t param_handle = param_find(param_name);

if (param_handle == PARAM_INVALID) {
_output_functions[i] = 0;
continue;

} else {
param_get(param_handle, &_output_functions[i]);
}
}
}

Expand Down
Loading

0 comments on commit 9f65415

Please sign in to comment.