From 99d08d5dc1f89ff960d57f3053df4bd03e0b26bc Mon Sep 17 00:00:00 2001 From: Ilia-Loginov Date: Thu, 24 Oct 2024 16:04:50 +0400 Subject: [PATCH] Added support for hitl testing for vtol and fw On branch adding_hitl_testing_for_vtol_and_fw Changes to be committed: new file: ROMFS/px4fmu_common/init.d/airframes/1404_ssrc_standard_vtol.hil new file: ROMFS/px4fmu_common/init.d/airframes/1440_ssrc_skywalker_x8.hil modified: ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt modified: ROMFS/px4fmu_common/init.d/rc.hitl_testing modified: Tools/simulation/gz/hitl_run.sh new file: Tools/simulation/gz/models/ssrc_skywalker_x8/model_hitl.sdf modified: Tools/simulation/gz/plugins/px4-gzsim-plugins modified: boards/ssrc/saluki-v2 modified: src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake modified: test/mavsdk_tests/CMakeLists.txt modified: test/mavsdk_tests/configs/hitl_gz_harm.json new file: test/mavsdk_tests/fw_mission.plan modified: test/mavsdk_tests/mavsdk_test_runner.py new file: test/mavsdk_tests/test_fw_mission.cpp --- .../airframes/1404_ssrc_standard_vtol.hil | 97 +++++++++++++++ .../airframes/1440_ssrc_skywalker_x8.hil | 83 +++++++++++++ .../init.d/airframes/CMakeLists.txt | 2 + ROMFS/px4fmu_common/init.d/rc.hitl_testing | 15 ++- Tools/simulation/gz/hitl_run.sh | 4 +- .../models/ssrc_skywalker_x8/model_hitl.sdf | 18 +++ Tools/simulation/gz/plugins/px4-gzsim-plugins | 2 +- boards/ssrc/saluki-v2 | 2 +- .../sitl_targets_gz-sim.cmake | 2 +- test/mavsdk_tests/CMakeLists.txt | 1 + test/mavsdk_tests/configs/hitl_gz_harm.json | 10 +- test/mavsdk_tests/fw_mission.plan | 111 ++++++++++++++++++ test/mavsdk_tests/mavsdk_test_runner.py | 2 +- test/mavsdk_tests/test_fw_mission.cpp | 48 ++++++++ 14 files changed, 387 insertions(+), 10 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/1404_ssrc_standard_vtol.hil create mode 100644 ROMFS/px4fmu_common/init.d/airframes/1440_ssrc_skywalker_x8.hil create mode 100644 Tools/simulation/gz/models/ssrc_skywalker_x8/model_hitl.sdf create mode 100644 test/mavsdk_tests/fw_mission.plan create mode 100644 test/mavsdk_tests/test_fw_mission.cpp diff --git a/ROMFS/px4fmu_common/init.d/airframes/1404_ssrc_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1404_ssrc_standard_vtol.hil new file mode 100644 index 000000000000..06725298bcef --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1404_ssrc_standard_vtol.hil @@ -0,0 +1,97 @@ +#!/bin/sh +# +# @name Standard VTOL +# +# @type Standard VTOL +# +# @class VTOL + +. ${R}etc/init.d/rc.vtol_defaults +. ${R}etc/init.d/rc.hitl_testing + +# Default rates +param set-default CA_AIRFRAME 2 + +param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR0_PX 0.1515 +param set-default CA_ROTOR0_PY 0.245 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.1515 +param set-default CA_ROTOR1_PY -0.1875 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.1515 +param set-default CA_ROTOR2_PY -0.245 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.1515 +param set-default CA_ROTOR3_PY 0.1875 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_AX 1.0 +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-default ASPD_PRIMARY 1 + +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS1_TYPE 2 +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 CA_SV_CS3_TRQ_Y 0.0 +param set-default CA_SV_CS3_TYPE 0 + +param set-default FW_PR_FF 0.2 +param set-default FW_PR_P 0.9 +param set-default FW_PSP_OFF 2 +param set-default FW_P_LIM_MIN -15 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_P 0.3 +param set-default FW_THR_TRIM 0.25 +param set-default FW_THR_MAX 0.6 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_CLMB_MAX 8 +param set-default FW_T_SINK_MAX 2.7 +param set-default FW_T_SINK_MIN 2.2 + +param set-default MC_AIRMODE 1 +param set-default MC_ROLLRATE_P 0.3 +param set-default MC_YAW_P 1.6 + +param set-default MIS_TAKEOFF_ALT 10 + +param set-default MPC_XY_P 0.8 +param set-default MPC_XY_VEL_P_ACC 3 +param set-default MPC_XY_VEL_I_ACC 4 +param set-default MPC_XY_VEL_D_ACC 0.1 + +param set-default NAV_ACC_RAD 5 + +param set-default VT_FWD_THRUST_EN 4 +param set-default VT_F_TRANS_THR 0.75 +param set-default VT_TYPE 2 +param set-default FD_ESCS_EN 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1440_ssrc_skywalker_x8.hil b/ROMFS/px4fmu_common/init.d/airframes/1440_ssrc_skywalker_x8.hil new file mode 100644 index 000000000000..b749073ff01f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1440_ssrc_skywalker_x8.hil @@ -0,0 +1,83 @@ +#!/bin/sh +# +# @name SSRC Skywalker X8 +# +# @type Flying Wing +# @class Plane +# + +. ${R}etc/init.d/rc.fw_defaults +. ${R}etc/init.d/rc.hitl_testing + +# 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 + +# 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 3000 + +# 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 + +# 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 +param set-default FW_P_LIM_MIN -15 +# 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 +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 +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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index de0bdef6f954..1dd1db38cd32 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -41,6 +41,8 @@ px4_add_romfs_files( 1101_rc_plane_sih.hil 1102_tailsitter_duo_sih.hil 1401_ssrc_holybro_x500.hil + 1404_ssrc_standard_vtol.hil + 1440_ssrc_skywalker_x8.hil # [2000, 2999] Standard planes" 2100_standard_plane diff --git a/ROMFS/px4fmu_common/init.d/rc.hitl_testing b/ROMFS/px4fmu_common/init.d/rc.hitl_testing index ed89d23734e2..f6185bf72e7e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hitl_testing +++ b/ROMFS/px4fmu_common/init.d/rc.hitl_testing @@ -4,11 +4,11 @@ # -param set SYS_HITL 1 -param set UAVCAN_ENABLE 0 +param set-default SYS_HITL 1 +param set-default UAVCAN_ENABLE 0 param set-default SYS_FAILURE_EN 1 -param set MAV_HITL_SHOME 0 +param set-default MAV_HITL_SHOME 0 param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 1 @@ -19,4 +19,13 @@ param set-default SENS_EN_ARSPDSIM 1 # - without real battery param set-default CBRK_SUPPLY_CHK 894281 # - without safety switch +param set-default COM_PREARM_MODE 0 param set-default CBRK_IO_SAFETY 22027 + + +# Disable RC loss failsafe check +param set-default COM_RC_LOSS_T 180 +param set-default NAV_RCL_ACT 1 + +# Disable RC input requirement +param set-default COM_RC_IN_MODE 1 diff --git a/Tools/simulation/gz/hitl_run.sh b/Tools/simulation/gz/hitl_run.sh index 9a9c98e71be0..d97c1c727fac 100755 --- a/Tools/simulation/gz/hitl_run.sh +++ b/Tools/simulation/gz/hitl_run.sh @@ -20,8 +20,8 @@ fi source $(pwd)/build/px4_sitl_default/rootfs/gz_env.sh if [ ! -f $GZ_SIM_SYSTEM_PLUGIN_PATH/libmavlink_hitl_gazebosim.so ]; then - echo "You should build gz-sim" - echo "make px4_sitl gz-sim" + echo "You should build gzsim-plugins" + echo "make px4_sitl gzsim-plugins" exit fi diff --git a/Tools/simulation/gz/models/ssrc_skywalker_x8/model_hitl.sdf b/Tools/simulation/gz/models/ssrc_skywalker_x8/model_hitl.sdf new file mode 100644 index 000000000000..e22146137142 --- /dev/null +++ b/Tools/simulation/gz/models/ssrc_skywalker_x8/model_hitl.sdf @@ -0,0 +1,18 @@ + + + + + model://ssrc_skywalker_x8 + + + + /link/base_link/sensor/imu_sensor/imu + /pose/info + 192.168.200.101 + 14542 + 14543 + 4560 + 0 + + + diff --git a/Tools/simulation/gz/plugins/px4-gzsim-plugins b/Tools/simulation/gz/plugins/px4-gzsim-plugins index f2da227901b1..06b5c1606bd4 160000 --- a/Tools/simulation/gz/plugins/px4-gzsim-plugins +++ b/Tools/simulation/gz/plugins/px4-gzsim-plugins @@ -1 +1 @@ -Subproject commit f2da227901b115679b18767ecf73ae695cf4113a +Subproject commit 06b5c1606bd4b595eef1524d983dfcdb18c73db1 diff --git a/boards/ssrc/saluki-v2 b/boards/ssrc/saluki-v2 index e0093f3f46ae..0228932073aa 160000 --- a/boards/ssrc/saluki-v2 +++ b/boards/ssrc/saluki-v2 @@ -1 +1 @@ -Subproject commit e0093f3f46ae188415c8f3bdeee9f315b3851aec +Subproject commit 0228932073aaa1fdd4679000ba949369556c1b22 diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake index f434ba340c40..97fc038d6b6c 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake @@ -37,7 +37,7 @@ if(gz-sim8_FOUND) include(ExternalProject) - ExternalProject_Add(gz-sim + ExternalProject_Add(gzsim-plugins SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation/gz/plugins/px4-gzsim-plugins/ BINARY_DIR ${PX4_BINARY_DIR}/build_gz-sim_plugins INSTALL_COMMAND "" diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 65d587d893f4..beae49e0230f 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -27,6 +27,7 @@ if(MAVSDK_FOUND) test_multicopter_offboard.cpp test_multicopter_manual.cpp test_vtol_mission.cpp + test_fw_mission.cpp # test_multicopter_follow_me.cpp ) diff --git a/test/mavsdk_tests/configs/hitl_gz_harm.json b/test/mavsdk_tests/configs/hitl_gz_harm.json index d3591bdeb647..209cde879415 100644 --- a/test/mavsdk_tests/configs/hitl_gz_harm.json +++ b/test/mavsdk_tests/configs/hitl_gz_harm.json @@ -9,7 +9,7 @@ "vehicle": "ssrc_holybro_x500", "model_file": "model_hitl", "sys_autostart": 1401, - "test_filter": "[multicopter],[offboard][offboard_attitude]", + "test_filter": "[multicopter],[offboard],[offboard_attitude]", "timeout_min": 10 }, { @@ -19,6 +19,14 @@ "sys_autostart": 1404, "test_filter": "[vtol], [vtol_wind], [vtol_airspeed_fail]", "timeout_min": 10 + }, + { + "model": "ssrc_skywalker_x8", + "vehicle": "ssrc_skywalker_x8", + "model_file": "model_hitl", + "sys_autostart": 1440, + "test_filter": "[fw]", + "timeout_min": 10 } ] } diff --git a/test/mavsdk_tests/fw_mission.plan b/test/mavsdk_tests/fw_mission.plan new file mode 100644 index 000000000000..0643c0fcbe20 --- /dev/null +++ b/test/mavsdk_tests/fw_mission.plan @@ -0,0 +1,111 @@ +{ + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 12, + "firmwareType": 12, + "globalPlanAltitudeMode": 1, + "hoverSpeed": 5, + "items": [ + { + "AMSLAltAboveTerrain": null, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 22, + "doJumpId": 1, + "frame": 3, + "params": [ + 0, + 0, + 0, + 0, + 47.39850965774455, + 8.548779680687378, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 2, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.395991354538396, + 8.54774933263701, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.395990640081514, + 8.542164833009195, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 31, + "doJumpId": 4, + "frame": 3, + "params": [75, 1, 0, null, 47.39702025037069, 8.54155416817485, 30], // loiter_radius = 75, loiterClockwise = 1 + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 0, + "AltitudeMode": 1, + "autoContinue": true, + "command": 21, + "doJumpId": 5, + "frame": 3, + "params": [0, 0, 0, null, 47.39774072, 8.54532786, 0], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 47.3977525, + 8.5456078, + 0 + ], + "vehicleType": 1, + "version": 2 + }, + "rallyPoints": { + "points": [ + ], + "version": 2 + }, + "version": 1 +} diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 2ba89046db99..5b3ca4ac2d39 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -306,7 +306,6 @@ def send_command_to_px4(self, command, args=None, use_shell=True): return True def reboot_px4(self): - time.sleep(5) res = False if (self.connection == "serial"): res = self.reboot_using_serial() @@ -320,6 +319,7 @@ def reboot_px4(self): time.sleep(15) else: print("Reboot failed") + exit(-1) diff --git a/test/mavsdk_tests/test_fw_mission.cpp b/test/mavsdk_tests/test_fw_mission.cpp new file mode 100644 index 000000000000..9b34e4357f3e --- /dev/null +++ b/test/mavsdk_tests/test_fw_mission.cpp @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (c) 2024 Technology Innovation Institute. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "autopilot_tester.h" + +TEST_CASE("Fly fix wing mission", "[fw]") +{ + AutopilotTester::MissionOptions mission_options; + + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/fw_mission.plan"); + tester.arm(); + tester.takeoff(); + tester.execute_mission_raw(); + tester.wait_until_disarmed(); +}