Add HITL support for fw, vtol and rover

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

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

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

src/modules/mavlink/mavlink_receiver.cpp

@@ -40,7 +40,6 @@
  * @author Thomas Gubler <[email protected]>
  */
 
-#include <lib/airspeed/airspeed.h>
 #include <lib/conversion/rotation.h>
 #include <lib/systemlib/px4_macros.h>
 
@@ -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
 	 */

src/modules/mavlink/mavlink_receiver.h

@@ -62,7 +62,6 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/airspeed.h>
 #include <uORB/topics/autotune_attitude_control_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/camera_status.h>
@@ -288,7 +287,6 @@ class MavlinkReceiver : public ModuleParams
 	uint16_t _mavlink_status_last_packet_rx_drop_count{0};
 
 	// ORB publications
-	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<camera_status_s>			_camera_status_pub{ORB_ID(camera_status)};
 	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};

src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp

@@ -34,11 +34,15 @@
 #ifndef HIL_ACTUATOR_CONTROLS_HPP
 #define HIL_ACTUATOR_CONTROLS_HPP
 
+#include <lib/mixer_module/mixer_module.hpp>
 #include <px4_platform_common/module_params.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_test.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/esc_status.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
 
 class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams
 {
@@ -63,61 +67,123 @@ 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_s> _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{};
 		esc_status.timestamp = hrt_absolute_time();
 		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
 			esc_status.esc[i].esc_current = armed ? 1.0f + math::abs_t(hil_act_control.controls[i]) * 15.0f :
 							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;
 		}

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.
 

src/modules/simulation/pwm_out_sim/PWMSim.hpp

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

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

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();
 

src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp

@@ -82,7 +82,7 @@ class SensorAirspeedSim : public ModuleBase<SensorAirspeedSim>, 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)};
 

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 ]
 
 #####################################