Added supporting hitl testing for the mavsdk_test_runner

On branch feature_hitl_testing_support_via_mavsdk_test_runner
Changes to be committed:
	modified:   ROMFS/px4fmu_common/init.d/CMakeLists.txt
	new file:   ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil
	modified:   ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
	new file:   ROMFS/px4fmu_common/init.d/rc.hitl_testing
	new file:   Tools/simulation/gz/models/ssrc_standard_vtol/model_hitl.sdf
	modified:   src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake
	modified:   test/mavsdk_tests/CMakeLists.txt
	new file:   test/mavsdk_tests/configs/hitl.json
	new file:   test/mavsdk_tests/configs/hitl_gz_harm.json
	new file:   test/mavsdk_tests/configs/sitl_gz_harm.json
	new file:   test/mavsdk_tests/mavsdk_preparing.cpp
	modified:   test/mavsdk_tests/mavsdk_test_runner.py
	modified:   test/mavsdk_tests/process_helper.py

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -56,4 +56,5 @@ px4_add_romfs_files(
 	rc.vehicle_setup
 	rc.vtol_apps
 	rc.vtol_defaults
+	rc.hitl_testing
 )

ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil

@@ -0,0 +1,41 @@
+#!/bin/sh
+#
+# @name Gazebo x500 for hitl testing
+#
+# @type Quadrotor x
+# @class Copter
+
+. ${R}etc/init.d/rc.mc_defaults
+. ${R}etc/init.d/rc.hitl_testing
+
+# Default rates
+param set-default IMU_GYRO_CUTOFF 30
+param set-default IMU_GYRO_NF0_FRQ 75
+param set-default IMU_DGYRO_CUTOFF 30
+param set-default MC_ROLLRATE_P 0.14
+param set-default MC_PITCHRATE_P 0.14
+param set-default MC_ROLLRATE_I 0.3
+param set-default MC_PITCHRATE_I 0.3
+param set-default MC_ROLLRATE_D 0.004
+param set-default MC_PITCHRATE_D 0.004
+
+# Control allocator parameters
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.175
+param set-default CA_ROTOR0_PY 0.175
+param set-default CA_ROTOR1_PX -0.175
+param set-default CA_ROTOR1_PY -0.175
+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
+
+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
+
+# Set takeoff ramp to disabled for a more decisive takeoff action
+param set-default MPC_TKO_RAMP_T 0

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -40,6 +40,7 @@ px4_add_romfs_files(
 	1100_rc_quad_x_sih.hil
 	1101_rc_plane_sih.hil
 	1102_tailsitter_duo_sih.hil
+	1401_ssrc_holybro_x500.hil
 
 	# [2000, 2999] Standard planes"
 	2100_standard_plane

ROMFS/px4fmu_common/init.d/rc.hitl_testing

@@ -0,0 +1,20 @@
+#!/bin/sh
+#
+# HITL testing parameters.
+#
+
+
+param set SYS_HITL 1
+param set UAVCAN_ENABLE 0
+param set-default SYS_FAILURE_EN 1
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+# disable some checks to allow to fly
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+# - without safety switch
+param set-default CBRK_IO_SAFETY 22027

Tools/simulation/gz/models/ssrc_standard_vtol/model_hitl.sdf

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='ssrc_standard_vtol_hitl'>
+    <include merge='true'>
+      <uri>model://ssrc_standard_vtol</uri>
+    </include>
+    <plugin filename="libmavlink_hitl_gazebosim.so" name="mavlink_interface::GazeboMavlinkInterface">
+      <robotNamespace/>
+      <imuSubTopic>/link/base_link/sensor/imu_sensor/imu</imuSubTopic>
+      <poseSubTopic>/pose/info</poseSubTopic>
+      <mavlink_addr>192.168.200.101</mavlink_addr>
+      <mavlink_udp_local_port>14542</mavlink_udp_local_port>
+      <mavlink_udp_remote_port>14543</mavlink_udp_remote_port>
+      <mavlink_tcp_port>4560</mavlink_tcp_port>
+      <use_tcp>0</use_tcp>
+    </plugin>
+  </model>
+</sdf>

src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake

@@ -48,5 +48,17 @@ if(gz-sim8_FOUND)
 		BUILD_ALWAYS 1
 		BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR> -- -j ${parallel_jobs}
 	)
+
+	ExternalProject_Add(mavsdk_tests
+		SOURCE_DIR ${PX4_SOURCE_DIR}/test/mavsdk_tests
+		CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+		BINARY_DIR ${PX4_BINARY_DIR}/mavsdk_tests
+		DEPENDS mavlink_c_generate
+		INSTALL_COMMAND ""
+		USES_TERMINAL_CONFIGURE true
+		USES_TERMINAL_BUILD true
+		EXCLUDE_FROM_ALL true
+		BUILD_ALWAYS 1
+	)
 endif()
 

test/mavsdk_tests/CMakeLists.txt

@@ -42,6 +42,15 @@ if(MAVSDK_FOUND)
             -Werror
             -Wno-error=deprecated-declarations
     )
+
+    add_executable(mavsdk_preparing
+    mavsdk_preparing.cpp
+    )
+
+    target_link_libraries(mavsdk_preparing
+        MAVSDK::mavsdk
+        ${CMAKE_THREAD_LIBS_INIT}
+    )
 else()
     message("MAVSDK C++ not found, skipping mavsdk_tests build..")
 endif()

test/mavsdk_tests/configs/hitl.json

@@ -0,0 +1,14 @@
+{
+	"mode": "hitl",
+	"simulator": "gazebo",
+	"mavlink_connection": "udp://:14540",
+	"tests":
+	[
+	    {
+		"model": "iris_hitl_set_your_model",
+		"vehicle": "iris_hitl_set_your_model",
+		"test_filter": "[multicopter],[offboard][offboard_attitude]",
+		"timeout_min": 10
+	    }
+	]
+    }

test/mavsdk_tests/configs/hitl_gz_harm.json

@@ -0,0 +1,24 @@
+{
+    "mode": "hitl",
+    "simulator": "gz_sim",
+    "mavlink_connection": "udp://:14540",
+    "tests":
+    [
+        {
+            "model": "ssrc_holybro_x500",
+            "vehicle": "ssrc_holybro_x500",
+            "model_file": "model_hitl",
+            "sys_autostart": 1401,
+            "test_filter": "[multicopter],[offboard][offboard_attitude]",
+            "timeout_min": 10
+        },
+        {
+            "model": "ssrc_standard_vtol",
+            "vehicle": "ssrc_standard_vtol",
+            "model_file": "model_hitl",
+            "sys_autostart": 1404,
+            "test_filter": "[vtol], [vtol_wind], [vtol_airspeed_fail]",
+            "timeout_min": 10
+        }
+    ]
+}

test/mavsdk_tests/configs/sitl_gz_harm.json

@@ -0,0 +1,20 @@
+{
+    "mode": "sitl",
+    "simulator": "gz_sim",
+    "mavlink_connection": "udp://:14540",
+    "tests":
+    [
+        {
+            "model": "x500",
+            "vehicle": "x500",
+            "test_filter": "[multicopter],[offboard][offboard_attitude]",
+            "timeout_min": 10
+        },
+        {
+            "model": "ssrc_standard_vtol",
+            "vehicle": "ssrc_standard_vtol",
+            "test_filter": "[vtol], [vtol_wind], [vtol_airspeed_fail]",
+            "timeout_min": 10
+        }
+    ]
+}

test/mavsdk_tests/mavsdk_preparing.cpp

@@ -0,0 +1,164 @@
+
+#include <iostream>
+#include <future>
+#include <memory>
+#include <thread>
+#include <string>
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/param/param.h>
+#include <atomic>
+#include <chrono>
+#include <ctime>
+#include <iostream>
+#include <memory>
+#include <optional>
+#include <thread>
+
+using namespace mavsdk;
+using std::chrono::seconds;
+using std::this_thread::sleep_for;
+
+template<typename Rep>
+bool poll_condition_with_timeout_(
+	std::function<bool()> fun, std::chrono::duration<Rep> duration)
+{
+	static constexpr unsigned check_resolution = 100;
+
+	const std::chrono::microseconds duration_us(duration);
+	const auto start_time = std::chrono::steady_clock::now();
+
+	while (!fun()) {
+		std::this_thread::sleep_for(duration_us / check_resolution);
+		const auto elapsed_time_us = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() -
+						start_time);
+
+		if (elapsed_time_us > duration_us) {
+			std::cout << "Timeout, waiting for the vehicle for "
+					<< elapsed_time_us.count() * std::chrono::steady_clock::period::num
+					/ static_cast<double>(std::chrono::steady_clock::period::den)
+					<< " seconds\n";
+			return false;
+		}
+	}
+	return true;
+}
+
+int main(int argc, char **argv)
+{
+	const std::string arg_url {"--url"};
+	const std::string arg_command {"--command"};
+	const std::string command_set_param {"set_sys_autostart"};
+	const std::string command_check {"check"};
+	const std::string command_reboot {"reboot"};
+
+	std::string connection_url {};
+	std::string command {};
+	int param_value {};
+	for (int i = 0; i < argc; ++i) {
+		const std::string argv_string(argv[i]);
+		if (argv_string == arg_url && (argc > (i + 1))) {
+			connection_url = argv[i + 1];
+			i++;
+		}
+		else if (argv_string == arg_command && (argc > (i + 1))) {
+			command = argv[i + 1];
+			i++;
+
+			if (command == command_set_param) {
+				if ((argc > (i + 1))){
+					param_value = std::atoi(argv[i + 1]);
+					i++;
+				} else {
+					command.erase();
+				}
+			}
+		}
+
+	}
+
+	if (connection_url.empty()) {
+		std::cerr << "No connection URL  was supplied" << std::endl;
+		return 1;
+	}
+
+   	Mavsdk mavsdk{};
+	std::cout << "connection url " << connection_url << std::endl;
+	ConnectionResult connection_result = mavsdk.add_any_connection(connection_url);
+	if (connection_result != ConnectionResult::Success)
+	{
+		std::cerr << "Connect was failed" << std::endl;
+		return 1;
+	}
+	std::cout << "Waiting for system connect" << std::endl;
+
+    	if(!poll_condition_with_timeout_(
+	[&]() { return mavsdk.systems().size() > 0; }, std::chrono::seconds(25)))
+	{
+		std::cerr << "Polling was failed" << std::endl;
+		return 1;
+	}
+
+	auto system = mavsdk.systems().at(0);
+
+	if(!system)
+	{
+		std::cerr << "The system wasn't be found" << std::endl;
+		return 1;
+	}
+
+	if (command_check == command)
+	{
+		std::cout << "Success! The connection was checked." << std::endl;
+		return 0;
+	}
+
+	if (command_set_param == command)
+	{
+		//TODO: For some reason when we set the new value it sometimes takes several attempts to change a parameter.
+		int attempt = 1;
+		const int max_attemp = 5;
+		const auto param_name = "SYS_AUTOSTART";
+		for(auto i = 0; i <= max_attemp; i++)
+		{
+			using namespace std::chrono_literals;
+			auto param = Param(system);
+
+
+			if (param.set_param_int(param_name, param_value) != Param::Result::Success)
+			{
+				std::cerr << "Fail setting param: " << param_name << ", value: "
+								<< param_value <<std::endl;
+				return 1;
+			}
+
+			auto res = param.get_param_int(param_name);
+			if (res.first == Param::Result::Success)
+				if (res.second == param_value)
+					break;
+
+			std::this_thread::sleep_for(1s);
+		}
+		if (i <= max_attemp) {
+			std::cout << "Param " << param_name << " was successly set after " << attempt
+				 << " attempt. Tne new value " << param_value << std::endl;
+		} else {
+			std::cerr << "Fail. Setting param: " << param_name << " wasn't set to value: "
+				  << param_value << "after " << max_attemp << " attempts. " <<std::endl;
+				return 1;
+		}
+	}
+
+	if (command_reboot == command) {
+		auto action = Action{system};
+		if ( Action::Result::Success != action.reboot())
+		{
+			std::cerr << "Reboot doesn't work" << std::endl;
+			return 1;
+		}
+		std::cout << "Rebooting...\n";
+	}
+
+	return 0;
+}