From bd2d6ae08d13020b958f56d749565aaadd7eecbe Mon Sep 17 00:00:00 2001 From: haitomatic Date: Tue, 20 Aug 2024 08:35:02 +0000 Subject: [PATCH] Add HITL support for fw, vtol and rover --- .../airframes/4404_gz_ssrc_standard_vtol | 1 - .../airframes/4430_gz_ssrc_strivermini | 9 +- .../init.d/airframes/4400_ssrc_fog_x | 6 - src/modules/mavlink/mavlink_receiver.cpp | 10 - src/modules/mavlink/mavlink_receiver.h | 2 - .../mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp | 107 +++++++++-- src/modules/simulation/pwm_out_sim/PWMSim.cpp | 23 ++- src/modules/simulation/pwm_out_sim/PWMSim.hpp | 7 +- .../simulation/pwm_out_sim/module_hil.yaml | 5 + .../sensor_airspeed_sim/SensorAirspeedSim.cpp | 2 +- .../sensor_airspeed_sim/SensorAirspeedSim.hpp | 2 +- ssrc_config/config_hitl_eth_gzsim.txt | 1 + ssrc_config/config_hitl_eth_gzsim_fw.txt | 107 +++++++++++ ssrc_config/config_hitl_eth_gzsim_mc.txt | 72 +++++++ ssrc_config/config_hitl_eth_gzsim_rover.txt | 83 +++++++++ ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt | 176 ++++++++++++++++++ ssrc_config/config_hitl_eth_gzsim_vtol_sv.txt | 120 ++++++++++++ 17 files changed, 686 insertions(+), 47 deletions(-) create mode 100644 ssrc_config/config_hitl_eth_gzsim_fw.txt create mode 100644 ssrc_config/config_hitl_eth_gzsim_mc.txt create mode 100644 ssrc_config/config_hitl_eth_gzsim_rover.txt create mode 100644 ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt create mode 100644 ssrc_config/config_hitl_eth_gzsim_vtol_sv.txt diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4404_gz_ssrc_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4404_gz_ssrc_standard_vtol index 2b24b898c91e..59f3c9db02e4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4404_gz_ssrc_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4404_gz_ssrc_standard_vtol @@ -82,7 +82,6 @@ param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS2_TYPE 3 param set-default CA_SV_CS2_TRQ_P 1.0 -param set-default FW_L1_PERIOD 12 param set-default FW_PR_FF 0.2 param set-default FW_PR_P 0.9 param set-default FW_PSP_OFF 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_strivermini b/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_strivermini index adc67bcd320d..1fb3f8dd9d79 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_strivermini +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_strivermini @@ -24,7 +24,12 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} PX4_SIM_MODEL=${PX4_SIM_MODEL:=strivermini} param set-default SIM_GZ_EN 1 -param set-default SIM_GZ_RUN_GZSIM 0 +param set-default SIM_GZ_SHOME 1 +if [ "$PX4_RUN_GZSIM" = "0" ]; then + param set-default SIM_GZ_RUN_GZSIM 0 +else + param set-default SIM_GZ_RUN_GZSIM 1 +fi param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 1 @@ -142,7 +147,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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x b/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x index 3708278758d4..035d2d527e15 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x +++ b/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x @@ -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 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 36958cbe69b2..3ab7055067d6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -40,7 +40,6 @@ * @author Thomas Gubler */ -#include #include #include @@ -2661,15 +2660,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) const double dt = math::constrain((timestamp_sample - _hil_timestamp_prev) * 1e-6, 0.001, 0.1); _hil_timestamp_prev = timestamp_sample; - /* airspeed */ - airspeed_s airspeed{}; - airspeed.timestamp_sample = timestamp_sample; - airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; - airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - airspeed.air_temperature_celsius = 15.f; - airspeed.timestamp = hrt_absolute_time(); - _airspeed_pub.publish(airspeed); - /* Receive attitude quaternion from gz-sim and publish vehicle attitude * groundtruth and angular velocity ground truth */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b47ca22c564b..8c6946fdb346 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -62,7 +62,6 @@ #include #include #include -#include #include #include #include @@ -288,7 +287,6 @@ class MavlinkReceiver : public ModuleParams uint16_t _mavlink_status_last_packet_rx_drop_count{0}; // ORB publications - uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; uORB::Publication _battery_pub{ORB_ID(battery_status)}; uORB::Publication _camera_status_pub{ORB_ID(camera_status)}; uORB::Publication _cellular_status_pub{ORB_ID(cellular_status)}; diff --git a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp index 182c1d40fab4..01d317b66975 100644 --- a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp +++ b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp @@ -34,11 +34,15 @@ #ifndef HIL_ACTUATOR_CONTROLS_HPP #define HIL_ACTUATOR_CONTROLS_HPP +#include #include #include +#include #include #include #include +#include +#include class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams { @@ -63,20 +67,38 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams { _act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; + int32_t rover_type = -1; + param_get(param_find("RI_ROVER_TYPE"), &rover_type); + + if (rover_type >= 0) { + param_get(param_find("GND_SPEED_MAX"), &_rover_max_speed); + _rover_thrust_sub = uORB::Subscription{ORB_ID(vehicle_thrust_setpoint)}; + _rover_torque_sub = uORB::Subscription{ORB_ID(vehicle_torque_setpoint)}; + _is_rover = true; + } + 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", "PWM_MAIN", "FUNC", i + 1); + snprintf(param_name, sizeof(param_name), "%s_%s%d", "HIL_ACT", "FUNC", i + 1); param_get(param_find(param_name), &_output_functions[i]); } } uORB::Subscription _act_sub{ORB_ID(actuator_outputs)}; + uORB::Subscription _act_test_sub{ORB_ID(actuator_test)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _rover_thrust_sub; + uORB::Subscription _rover_torque_sub; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {}; + bool _is_rover{false}; + float _rover_max_speed{0.0f}; + float _rover_thrust_control{0.0f}; + float _rover_torque_control{0.0f}; + void send_esc_telemetry(mavlink_hil_actuator_controls_t &hil_act_control, vehicle_status_s &vehicle_status) { esc_status_s esc_status{}; @@ -84,14 +106,14 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams const int max_esc_count = math::min(actuator_outputs_s::NUM_ACTUATOR_OUTPUTS, esc_status_s::CONNECTED_ESC_MAX); const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - int max_esc_index = 0; for (int i = 0; i < max_esc_count; i++) { - if (_output_functions[i] != 0) { - max_esc_index = i; + if (_output_functions[i] == 0 || _output_functions[i] < (int)OutputFunction::Motor1 + || _output_functions[i] > (int)OutputFunction::MotorMax) { + return; } - esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_sim... + esc_status.esc[i].actuator_function = _output_functions[i]; esc_status.esc[i].timestamp = esc_status.timestamp; esc_status.esc[i].esc_errorcount = 0; // TODO esc_status.esc[i].esc_voltage = 11.1f + math::abs_t(hil_act_control.controls[i]) * 3.0f; // TODO: magic number @@ -99,25 +121,69 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams 0.0f; // TODO: magic number esc_status.esc[i].esc_rpm = hil_act_control.controls[i] * 6000; // TODO: magic number esc_status.esc[i].esc_temperature = 20.0f + math::abs_t(hil_act_control.controls[i]) * 40.0f; + esc_status.esc_count += 1; } - esc_status.esc_count = max_esc_index + 1; esc_status.esc_armed_flags = (1u << esc_status.esc_count) - 1; esc_status.esc_online_flags = (1u << esc_status.esc_count) - 1; _esc_status_pub.publish(esc_status); } + bool updateRoverCmdVel() + { + if (_is_rover) { + bool do_update = false; + + // Check torque setppoint update + if (_rover_torque_sub.updated()) { + vehicle_torque_setpoint_s vehicle_torque_setpoint_msg; + + if (_rover_torque_sub.copy(&vehicle_torque_setpoint_msg)) { + _rover_torque_control = vehicle_torque_setpoint_msg.xyz[2]; + do_update = true; + } + } + + // Check thrust setpoint update + if (_rover_thrust_sub.updated()) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint_msg; + + if (_rover_thrust_sub.copy(&vehicle_thrust_setpoint_msg)) { + /** + * On physical rover the max speed scale back is only for mission mode + * But here, we simplify the logic and apply the scale back for both + * manual and mission mode + */ + _rover_thrust_control = vehicle_thrust_setpoint_msg.xyz[0] * _rover_max_speed; + do_update = true; + } + } + + return do_update ? true : false; + + } else { + return false; + } + } + bool send() override { actuator_outputs_s act; - if (_act_sub.update(&act)) { + if (_act_sub.update(&act) || updateRoverCmdVel()) { mavlink_hil_actuator_controls_t msg{}; msg.time_usec = act.timestamp; - for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { - msg.controls[i] = act.output[i]; + if (_is_rover) { + // Use last two control inputs for rover vel cmd + msg.controls[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS - 1] = _rover_thrust_control; + msg.controls[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS - 2] = _rover_torque_control; + + } else { + for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { + msg.controls[i] = act.output[i]; + } } // mode (MAV_MODE_FLAG) @@ -155,11 +221,30 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams } } - msg.flags = 0; + actuator_test_s actuator_test; + + if (_act_test_sub.copy(&actuator_test)) { + if (actuator_test.action != 0) { + msg.mode |= MAV_MODE_FLAG_TEST_ENABLED; + } + } + + // [custom use of flag] depending on OutputFunction, if output function is motor type, set bit in bitfield + uint64_t flags = 0; + + for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { + if (_output_functions[i] >= (int)OutputFunction::Motor1 && _output_functions[i] <= (int)OutputFunction::MotorMax) { + flags |= (1ULL << i); + } + } + + msg.flags = flags; mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg); - send_esc_telemetry(msg, status); + if (!_is_rover) { + send_esc_telemetry(msg, status); + } return true; } diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index c11a5eb365fe..6e8836bde638 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -44,10 +44,15 @@ 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]); + } _mixing_output.setIgnoreLockdown(hil_mode_enabled); } @@ -69,7 +74,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); @@ -78,12 +83,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; } } @@ -177,7 +182,7 @@ int PWMSim::print_usage(const char *reason) ### Description Driver for simulated PWM outputs. -Its only function is to take `actuator_control` uORB messages, +Its only function is to take `actuator_motors` and `actuator_servos` uORB messages, mix them with any loaded mixer and output the result to the `actuator_output` uORB topic. diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index b16a9fac5e75..a24ae9b526e3 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -76,10 +76,9 @@ class PWMSim : public ModuleBase, 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}; diff --git a/src/modules/simulation/pwm_out_sim/module_hil.yaml b/src/modules/simulation/pwm_out_sim/module_hil.yaml index 25b9db39e02b..9ed19eecca4e 100644 --- a/src/modules/simulation/pwm_out_sim/module_hil.yaml +++ b/src/modules/simulation/pwm_out_sim/module_hil.yaml @@ -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 diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index 744ab6639722..1facd3267994 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -195,7 +195,7 @@ int SensorAirspeedSim::print_usage(const char *reason) )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("sensor_arispeed_sim", "system"); + PRINT_MODULE_USAGE_NAME("sensor_airspeed_sim", "system"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp index 78121e9594cf..af67395097d3 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp @@ -82,7 +82,7 @@ class SensorAirspeedSim : public ModuleBase, public ModulePar matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz) { return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); } uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)}; diff --git a/ssrc_config/config_hitl_eth_gzsim.txt b/ssrc_config/config_hitl_eth_gzsim.txt index 9544446a4adb..4b06840f372d 100644 --- a/ssrc_config/config_hitl_eth_gzsim.txt +++ b/ssrc_config/config_hitl_eth_gzsim.txt @@ -1,4 +1,5 @@ # Default parameter set for HITL with ethernet Gazebo Sim connection (partly derived from indoor flying parameters) +# Currently this is used for FC only HITL with ssrc holybro x500 airframe # [ type: hitl_eth_gzsim ] ##################################### diff --git a/ssrc_config/config_hitl_eth_gzsim_fw.txt b/ssrc_config/config_hitl_eth_gzsim_fw.txt new file mode 100644 index 000000000000..0ae8d5b584a6 --- /dev/null +++ b/ssrc_config/config_hitl_eth_gzsim_fw.txt @@ -0,0 +1,107 @@ +# Default parameter set for HITL with ethernet Gazebo Sim connection on Skywalker X8 FW +# [ type: hitl_eth_gzsim ] + +param set SYS_AUTOSTART 4421 +param set SYS_AUTOCONFIG 0 + +#################################### +# 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 1 + +#################################### +# specific fw HITL configuration +#################################### + +# Override airframe defaults +param set ASPD_DO_CHECKS 7 +param set FW_LAUN_DETCN_ON 0 + +# HITL PWM functions +param set HIL_ACT_FUNC1 201 +param set HIL_ACT_DIS1 500 +param set HIL_ACT_FUNC2 202 +param set HIL_ACT_DIS2 500 +param set HIL_ACT_FUNC4 101 +param set HIL_ACT_MIN4 150 +param set HIL_ACT_MAX4 3000 + +# Control allocator parameters +param set CA_AIRFRAME 1 +param set CA_ROTOR_COUNT 1 +param set CA_SV_CS_COUNT 2 +param set CA_SV_CS0_TYPE 5 +param set CA_SV_CS0_TRQ_P 0.5 +param set CA_SV_CS0_TRQ_R -0.5 +param set CA_SV_CS1_TYPE 6 +param set CA_SV_CS1_TRQ_P 0.5 +param set CA_SV_CS1_TRQ_R 0.5 + +# Airspeed parameters +param set ASPD_PRIMARY 1 + +# Maximum landing slope angle in deg +param set FW_LND_ANG 8.0 + +# RC loss failsafe to HOLD mode +param set COM_RC_IN_MODE 1 + +# Maximum manual roll angle +param set FW_MAN_R_MAX 60.0 + +# Fixed wing control +# Pitch rate +param set FW_PR_P 0.9 +param set FW_PR_FF 0.5 +param set FW_PR_I 0.5 +param set TRIM_PITCH -0.15 +# Pitch angle in deg +param set FW_PSP_OFF 0.0 +param set FW_P_LIM_MIN -15.0 +# Roll rate +param set FW_RR_FF 0.5 +param set FW_RR_P 0.3 +param set FW_RR_I 0.5 +# Yaw rate +param set FW_YR_FF 0.5 +param set FW_YR_P 0.6 +param set FW_YR_I 0.5 +#Throttle limit +param set FW_THR_MAX 0.6 +param set FW_THR_MIN 0.05 +param set FW_THR_TRIM 0.25 +# Climb and sink rate +param set FW_T_CLMB_MAX 8.0 +param set FW_T_SINK_MAX 2.7 +param set FW_T_SINK_MIN 2.2 + +# Navigation +param set NAV_ACC_RAD 15.0 +param set NAV_DLL_ACT 2 + +# Misc +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 30.0 +param set FW_LND_USETER 0 +param set RWTO_TKOFF 1 +param set FD_ESCS_EN 0 diff --git a/ssrc_config/config_hitl_eth_gzsim_mc.txt b/ssrc_config/config_hitl_eth_gzsim_mc.txt new file mode 100644 index 000000000000..c10868f7a034 --- /dev/null +++ b/ssrc_config/config_hitl_eth_gzsim_mc.txt @@ -0,0 +1,72 @@ +# Default parameter set for HITL with ethernet Gazebo Sim connection on Holybro X500 MC +# [ type: hitl_eth_gzsim ] + +param set SYS_AUTOSTART 4400 +param set SYS_AUTOCONFIG 0 + +#################################### +# 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 1 + +#################################### +# 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 \ No newline at end of file diff --git a/ssrc_config/config_hitl_eth_gzsim_rover.txt b/ssrc_config/config_hitl_eth_gzsim_rover.txt new file mode 100644 index 000000000000..c4eacaade6f9 --- /dev/null +++ b/ssrc_config/config_hitl_eth_gzsim_rover.txt @@ -0,0 +1,83 @@ +# Default parameter set for HITL with ethernet Gazebo Sim connection on scout mini rover +# [ type: hitl_eth_gzsim ] + +param set SYS_AUTOSTART 50005 +param set SYS_AUTOCONFIG 0 + +#################################### +# 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 1 + +#################################### +# specific rover HITL configuration +#################################### + +param set IMU_GYRO_CUTOFF 60.0 +param set IMU_DGYRO_CUTOFF 30.0 + +param set SYS_HAS_BARO 0 + +param set BAT1_N_CELLS 4 + +param set MIS_TAKEOFF_ALT 0.01 + +param set NAV_ACC_RAD 0.5 # reached when within 0.5m of waypoint + +# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor +param set CBRK_AIRSPD_CHK 162128 + +# EKF2 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_MAG_TYPE 1 +param set EKF2_REQ_SACC 1.0 +param set EKF2_REQ_VDRIFT 0.4 +param set EKF2_REQ_HDRIFT 0.2 + + +# Rover Position Control Module +param set GND_SP_CTRL_MODE 1 +param set GND_L1_DIST 5.0 +param set GND_L1_PERIOD 3.0 +param set GND_THR_CRUISE 1.0 +param set GND_THR_MAX 1.0 + +# Because this is differential drive, it can make a turn with radius 0. +# This corresponds to a turn angle of pi radians. +# If a special case is made for differential-drive, this will need to change. +param set GND_MAX_ANG 3.142 +param set GND_WHEEL_BASE 0.45 + +# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase +# to support negative throttle. +param set GND_THR_MIN 0.0 +param set GND_SPEED_P 0.4 +param set GND_SPEED_I 1.0 +param set GND_SPEED_D 0.001 +param set GND_SPEED_MAX 3.0 +param set GND_SPEED_TRIM 3.0 +param set GND_SPEED_THR_SC 1.0 +param set GND_VEL_CTRL 1 +param set GND_ANG_VEL_CTRL 1 +param set GND_ACC_LIMIT 10.0 +param set GND_DEC_LIMIT 50.0 diff --git a/ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt b/ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt new file mode 100644 index 000000000000..414ece3fcd2e --- /dev/null +++ b/ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt @@ -0,0 +1,176 @@ +# Default parameter set for HITL with ethernet Gazebo Sim connection on Striver Mini VTOL +# [ type: hitl_eth_gzsim ] + +param set SYS_AUTOSTART 4430 +param set SYS_AUTOCONFIG 0 + +#################################### +# 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 1 + +#################################### +# specific vtol HITL configuration +#################################### + +# HITL PWM functions +param set HIL_ACT_FUNC1 101 +param set HIL_ACT_MIN1 10 +param set HIL_ACT_MAX1 1500 +param set HIL_ACT_FUNC2 102 +param set HIL_ACT_MIN2 10 +param set HIL_ACT_MAX2 1500 +param set HIL_ACT_FUNC3 103 +param set HIL_ACT_MIN3 10 +param set HIL_ACT_MAX3 1500 +param set HIL_ACT_FUNC4 104 +param set HIL_ACT_MIN4 10 +param set HIL_ACT_MAX4 1500 +param set HIL_ACT_FUNC5 201 +param set HIL_ACT_DIS5 500 +param set HIL_ACT_FUNC6 202 +param set HIL_ACT_DIS6 500 +param set HIL_ACT_FUNC7 203 +param set HIL_ACT_DIS7 500 +param set HIL_ACT_FUNC8 204 +param set HIL_ACT_DIS8 500 +param set HIL_ACT_FUNC9 205 +param set HIL_ACT_DIS9 500 +param set HIL_ACT_FUNC10 105 +param set HIL_ACT_MIN10 0 +param set HIL_ACT_MAX10 3500 + +# Control allocator parameters +param set CA_AIRFRAME 2 +param set CA_ROTOR_COUNT 5 +param set CA_ROTOR0_PX 0.37 +param set CA_ROTOR0_PY 0.42 +param set CA_ROTOR1_PX -0.41 +param set CA_ROTOR1_PY -0.42 +param set CA_ROTOR2_PX 0.37 +param set CA_ROTOR2_PY -0.42 +param set CA_ROTOR2_KM -0.05 +param set CA_ROTOR3_PX -0.41 +param set CA_ROTOR3_PY 0.42 +param set CA_ROTOR3_KM -0.05 +param set CA_ROTOR4_AX 1.0 +param set CA_ROTOR4_AZ 0.0 +param set CA_ROTOR4_PX 0.0 +param set CA_ROTOR4_KM 0.05 + +param set CA_SV_CS_COUNT 5 +param set CA_SV_CS0_TYPE 1 +param set CA_SV_CS0_TRQ_R -0.5 +param set CA_SV_CS1_TYPE 2 +param set CA_SV_CS1_TRQ_R 0.5 +param set CA_SV_CS2_TYPE 3 +param set CA_SV_CS2_TRQ_P 1.0 +param set CA_SV_CS2_TRIM 0.1 +param set CA_SV_CS3_TYPE 3 +param set CA_SV_CS3_TRQ_P 1.0 +param set CA_SV_CS3_TRIM 0.1 +param set CA_SV_CS4_TYPE 4 +param set CA_SV_CS4_TRQ_Y 1.0 + +# Fixed wing specific +param set FW_RR_IMAX 0.4000 +param set FW_YR_IMAX 0.4000 + +param set FW_YR_P 0.2 +param set FW_YR_I 0.01 + +param set FW_RR_FF 0.5 +param set FW_RR_P 0.05 +param set FW_RR_I 0.1 +param set FW_RR_IMAX 0.4 + +param set FW_R_LIM 50.0 +param set FW_R_RMAX 70.0 +param set FW_R_TC 0.4 + +param set FW_PR_FF 0.5 +param set FW_PR_I 0.1 +param set FW_PR_IMAX 0.4 + +param set FW_P_LIM_MAX 30.0 +param set FW_P_LIM_MIN -30.0 + +# Airspeed parameters +param set ASPD_PRIMARY 1 +param set FW_AIRSPD_MAX 25.0 +param set FW_AIRSPD_MIN 15.0 +param set FW_AIRSPD_STALL 12.0 +param set FW_AIRSPD_TRIM 18.0 + +# VTOL specific +# VTOL type +param set VT_TYPE 2 + +# Airspeed at which we can start blending both fw and mc controls. +param set VT_ARSP_BLEND 8.0 + +# Airspeed at which we can switch to fw mode +param set VT_ARSP_TRANS 14.0 + +# Back-transition duration +param set VT_B_TRANS_DUR 10.0 +param set VT_B_TRANS_RAMP 3.0 + +# Transition duration +param set VT_F_TRANS_DUR 6.0 +param set VT_F_TRANS_THR 1.0 + +# VTOL takeoff +param set MIS_TAKEOFF_ALT 10.0 +param set VTO_LOITER_ALT 10.0 + +param set MC_AIRMODE 0 +param set MC_ROLLRATE_P 0.14 +param set MC_ROLLRATE_I 0.2 +param set MC_PITCHRATE_P 0.14 +param set MC_PITCHRATE_I 0.2 +param set MC_YAW_P 2.0 +param set MC_YAW_WEIGHT 0.5 +param set MC_YAWRATE_P 0.2 +param set MC_YAWRATE_I 0.1 +param set MC_YAWRATE_MAX 120.0 + +param set MPC_XY_P 0.95 +param set MPC_XY_VEL_P_ACC 1.8 +param set MPC_XY_VEL_I_ACC 0.4 +param set MPC_XY_VEL_D_ACC 0.2 +param set MPC_YAW_MODE 4 + +param set NAV_ACC_RAD 10.0 + +# QuadChute altitude (transition to quad mode as a failsafe) +param set VT_FW_MIN_ALT 5.0 + +# QuadChute angle limits +param set VT_FW_QC_P 35 +param set VT_FW_QC_R 60 + +# Others +param set FD_ACT_EN 0 +param set FD_ACT_MOT_TOUT 500 +param set FD_ESCS_EN 0 diff --git a/ssrc_config/config_hitl_eth_gzsim_vtol_sv.txt b/ssrc_config/config_hitl_eth_gzsim_vtol_sv.txt new file mode 100644 index 000000000000..ec3f924a90f1 --- /dev/null +++ b/ssrc_config/config_hitl_eth_gzsim_vtol_sv.txt @@ -0,0 +1,120 @@ +# Default parameter set for HITL with ethernet Gazebo Sim connection on standard VTOL +# [ type: hitl_eth_gzsim ] + +param set SYS_AUTOSTART 13000 +param set SYS_AUTOCONFIG 0 + +#################################### +# 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 1 + +#################################### +# specific vtol HITL configuration +#################################### + +# HITL PWM functions +param set HIL_ACT_FUNC1 101 +param set HIL_ACT_MIN1 10 +param set HIL_ACT_MAX1 1500 +param set HIL_ACT_FUNC2 102 +param set HIL_ACT_MIN2 10 +param set HIL_ACT_MAX2 1500 +param set HIL_ACT_FUNC3 103 +param set HIL_ACT_MIN3 10 +param set HIL_ACT_MAX3 1500 +param set HIL_ACT_FUNC4 104 +param set HIL_ACT_MIN4 10 +param set HIL_ACT_MAX4 1500 +param set HIL_ACT_FUNC5 105 +param set HIL_ACT_MIN5 0 +param set HIL_ACT_MAX5 3500 +param set HIL_ACT_FUNC6 201 +param set HIL_ACT_DIS6 500 +param set HIL_ACT_FUNC7 202 +param set HIL_ACT_DIS7 500 +param set HIL_ACT_FUNC8 203 +param set HIL_ACT_DIS8 500 + + +# Control allocator parameters +param set CA_ROTOR_COUNT 5 +param set CA_ROTOR0_PX 0.1515 +param set CA_ROTOR0_PY 0.245 +param set CA_ROTOR0_KM 0.05 +param set CA_ROTOR1_PX -0.1515 +param set CA_ROTOR1_PY -0.1875 +param set CA_ROTOR1_KM 0.05 +param set CA_ROTOR2_PX 0.1515 +param set CA_ROTOR2_PY -0.245 +param set CA_ROTOR2_KM -0.05 +param set CA_ROTOR3_PX -0.1515 +param set CA_ROTOR3_PY 0.1875 +param set CA_ROTOR3_KM -0.05 +param set CA_ROTOR4_AX 1.0 +param set CA_ROTOR4_AZ 0.0 +param set CA_ROTOR4_PX 0.2 + +param set CA_SV_CS_COUNT 3 +param set CA_SV_CS0_TYPE 1 +param set CA_SV_CS0_TRQ_R -0.5 +param set CA_SV_CS1_TYPE 2 +param set CA_SV_CS1_TRQ_R 0.5 +param set CA_SV_CS2_TYPE 3 +param set CA_SV_CS2_TRQ_P 1.0 +param set CA_SV_CS3_TRQ_Y 0.0 +param set CA_SV_CS3_TYPE 0 + +# Airspeed parameters +param set ASPD_PRIMARY 1 + +param set FW_PR_FF 0.2 +param set FW_PR_P 0.9 +param set FW_PSP_OFF 2.0 +param set FW_P_LIM_MIN -15.0 +param set FW_RR_FF 0.1 +param set FW_RR_P 0.3 +param set FW_THR_TRIM 0.25 +param set FW_THR_MAX 0.6 +param set FW_THR_MIN 0.05 +param set FW_T_CLMB_MAX 8.0 +param set FW_T_SINK_MAX 2.7 +param set FW_T_SINK_MIN 2.2 + +param set MC_AIRMODE 1 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 1.6 + +param set MIS_TAKEOFF_ALT 10.0 + +param set MPC_XY_P 0.8 +param set MPC_XY_VEL_P_ACC 3.0 +param set MPC_XY_VEL_I_ACC 4.0 +param set MPC_XY_VEL_D_ACC 0.1 + +param set NAV_ACC_RAD 5.0 + +param set VT_FWD_THRUST_EN 4 +param set VT_F_TRANS_THR 0.75 +param set VT_TYPE 2 +param set FD_ESCS_EN 0 \ No newline at end of file