fix：新增例程

基础智能/e4.四旋翼与无人车异构集群协同控制实验/CarNoCtrlSITLRun.bat

@@ -0,0 +1,168 @@
+@ECHO OFF
+
+REM Run script as administrator
+%1 mshta vbscript:CreateObject("Shell.Application").ShellExecute("cmd.exe","/c ""%~s0"" ::","","runas",1)(window.close)&&exit
+
+REM Set the path of the RflySim tools
+if not defined PSP_PATH (
+    SET PSP_PATH=E:\PX4PSP
+    SET PSP_PATH_LINUX=/mnt/e/PX4PSP
+)
+
+REM Start index of vehicle number (should larger than 0)
+REM This option is useful for simulation with multi-computers
+SET /a START_INDEX=4
+
+
+REM Set the start UDP port for SIMULINK/OFFBOARD API
+REM This option should not be modified for swarm simulation
+SET /a UDP_START_PORT=20100
+
+
+REM Set use DLL model name or not, use number index or name string
+REM This option is useful for simulation with other types of vehicles instead of multicopters
+set DLLModel=CarNoCtrl
+
+REM Check if DLLModel is a name string, if yes, copy the DLL file to CopterSim folder
+SET /A DLLModelVal=DLLModel
+if %DLLModelVal% NEQ %DLLModel% (
+    REM Copy the latest dll file to CopterSim folder
+    copy /Y "%~dp0"\%DLLModel%.dll %PSP_PATH%\CopterSim\external\model\%DLLModel%.dll
+)
+
+
+REM Set the simulation mode on CopterSim, use number index or name string
+REM e.g., SimMode=2 equals to  SimMode=PX4_SITL_RFLY
+set SimMode=2
+
+REM Set the vehicle-model (airframe) of PX4 SITL simulation, the default airframe is a quadcopter: iris
+REM Check folder Firmware\ROMFS\px4fmu_common\init.d-posix for supported airframes (Note: You can also create your airframe file here)
+REM E.g., fixed-wing aircraft: PX4SitlFrame=plane; small cars: PX4SitlFrame=rover
+set PX4SitlFrame=r1_rover
+
+
+REM Set the map, use index or name of the map on CopterSim
+REM e.g., UE4_MAP=1 equals to UE4_MAP=Grasslands
+SET UE4_MAP=OldFactory
+
+REM Set the origin x,y position (m) and yaw angle (degree) at the map
+SET /a ORIGIN_POS_X=-250
+SET /a ORIGIN_POS_Y=-119
+SET /a ORIGIN_YAW=0
+
+REM Set the interval between two vehicle, unit:m 
+SET /a VEHICLE_INTERVAL=3
+
+
+REM Set broadcast to other computer; 0: only this computer, 1: broadcast; or use IP address to increase speed
+REM e.g., IS_BROADCAST=0 equals to IS_BROADCAST=127.0.0.1, IS_BROADCAST=1 equals to IS_BROADCAST=255.255.255.255
+SET IS_BROADCAST=0
+
+REM Set UDP data mode; 0: UDP_FULL, 1:UDP_Simple, 2: Mavlink_Full, 3: Mavlink_simple. input number or string
+REM e.g., UDPSIMMODE=1 equals to UDPSIMMODE=UDP_Simple
+SET UDPSIMMODE=1
+
+
+:Top
+ECHO.
+ECHO ---------------------------------------
+REM Max vehicle number 50
+SET /a MAX_VEHICLE=50
+REM SET /P VehicleNum=Please input UAV swarm number:
+SET /A VehicleNum=3
+SET /A Evaluated=VehicleNum
+if %Evaluated% EQU %VehicleNum% (
+    IF %VehicleNum% GTR 0 (
+        IF %VehicleNum% GTR %MAX_VEHICLE% (
+            ECHO The vehicle number is too large, which may cause a crash
+            pause
+        )
+        GOTO StartSim
+    )
+    ECHO Not a positive integer
+    GOTO Top
+) ELSE (
+    ECHO Not a integer
+    GOTO Top
+)
+:StartSim
+
+SET /A VehicleTotalNum=%VehicleNum% + %START_INDEX% - 1
+if not defined TOTOAL_COPTER (
+    SET /A TOTOAL_COPTER=%VehicleTotalNum%
+)
+
+set /a sqrtNum=1
+set /a squareNum=1
+:loopSqrt
+set /a squareNum=%sqrtNum% * %sqrtNum%
+if %squareNum% EQU %TOTOAL_COPTER% (
+    goto loopSqrtEnd
+)
+if %squareNum% GTR %TOTOAL_COPTER% (
+    goto loopSqrtEnd
+)
+set /a sqrtNum=%sqrtNum%+1
+goto loopSqrt
+:loopSqrtEnd
+
+
+REM QGCPath
+REM tasklist|find /i "QGroundControl.exe" || start %PSP_PATH%\QGroundControl\QGroundControl.exe
+ECHO Start QGroundControl
+
+REM UE4Path
+cd %PSP_PATH%\RflySim3D
+REM tasklist|find /i "RflySim3D.exe" || start %PSP_PATH%\RflySim3D\RflySim3D.exe
+REM choice /t 5 /d y /n >nul
+
+
+REM tasklist|find /i "CopterSim.exe" && taskkill /im "CopterSim.exe"
+ECHO Kill all CopterSims
+
+
+REM CptSmPath
+cd /d %PSP_PATH%\CopterSim
+
+
+set /a cntr=%START_INDEX%
+set /a endNum=%VehicleTotalNum%+1
+set /a portNum=%UDP_START_PORT%+((%START_INDEX%-1)*2)
+:loopBegin
+set /a PosXX=((%cntr%-1) / %sqrtNum%)*%VEHICLE_INTERVAL% + %ORIGIN_POS_X%
+set /a PosYY=((%cntr%-1) %% %sqrtNum%)*%VEHICLE_INTERVAL% + %ORIGIN_POS_Y%
+start /realtime CopterSim.exe 1 %cntr% %portNum% %DLLModel% %SimMode% %UE4_MAP% %IS_BROADCAST% %PosXX% %PosYY% %ORIGIN_YAW% 1 %UDPSIMMODE%
+choice /t 1 /d y /n >nul
+set /a cntr=%cntr%+1
+set /a portNum=%portNum%+2
+if %cntr% EQU %endNum% goto loopEnd
+goto loopBegin
+:loopEnd
+
+REM Set ToolChainType 1:Win10WSL 3:Cygwin
+SET /a ToolChainType=1
+
+if "%IS_BROADCAST%" == "0" (
+    SET IS_BROADCAST=0
+) else (
+    SET IS_BROADCAST=1
+)
+
+SET WINDOWSPATH=%PATH%
+if %ToolChainType% EQU 1 (
+    wsl echo Starting PX4 Build; cd %PSP_PATH_LINUX%/Firmware; ./BkFile/EnvOri.sh; export PATH=$HOME/ninja:$HOME/gcc-arm-none-eabi-7-2017-q4-major/bin:$PATH;make px4_sitl_default; ./Tools/sitl_multiple_run_rfly.sh %VehicleNum% %START_INDEX% %PX4SitlFrame%;echo Press any key to exit; read -n 1
+) else (
+    REM CYGPath
+    cd %PSP_PATH%\CygwinToolchain
+    CALL setPX4Env.bat
+    bash -l -i -c 'echo Starting SITL SIMULATION; cd %PSP_PATH_LINUX%/Firmware; ./BkFile/EnvOri.sh; pwd; make px4_sitl_default; ./Tools/sitl_multiple_run_rfly.sh %VehicleNum% %START_INDEX% %PX4SitlFrame%;echo Press any key to exit; read -n 1; pkill -x px4 || true;'
+)
+SET PATH=%WINDOWSPATH%
+
+
+REM kill all applications when press a key
+tasklist|find /i "CopterSim.exe" && taskkill /im "CopterSim.exe"
+tasklist|find /i "QGroundControl.exe" && taskkill /f /im "QGroundControl.exe"
+tasklist|find /i "RflySim3D.exe" && taskkill /f /im "RflySim3D.exe"
+
+ECHO Start End.

基础智能/e4.四旋翼与无人车异构集群协同控制实验/MultiVehicleRunALL.bat

@@ -0,0 +1,13 @@
+if not defined PSP_PATH (
+    SET PSP_PATH=C:\PX4PSP
+    SET PSP_PATH_LINUX=/mnt/c/PX4PSP
+)
+@echo off
+rem <20><><EFBFBD><EFBFBD> MulticopterNoCtrlSITLRun.bat
+start "" MulticopterNoCtrlSITLRun.bat
+
+rem <20><>ʱ 5 <20><>
+timeout /t 25 /nobreak >nul
+
+rem <20><><EFBFBD><EFBFBD> CarNoCtrlSITLRun.bat
+start "" CarNoCtrlSITLRun.bat

基础智能/e4.四旋翼与无人车异构集群协同控制实验/MulticopterNoCtrlSITLRun.bat

@@ -0,0 +1,164 @@
+@ECHO OFF
+
+REM Run script as administrator
+%1 mshta vbscript:CreateObject("Shell.Application").ShellExecute("cmd.exe","/c ""%~s0"" ::","","runas",1)(window.close)&&exit
+
+REM Set the path of the RflySim tools
+if not defined PSP_PATH (
+    SET PSP_PATH=E:\PX4PSP
+    SET PSP_PATH_LINUX=/mnt/e/PX4PSP
+)
+
+REM The text start with 'REM' is annotation, the following options are corresponding to Options on CopterSim
+
+REM Start index of vehicle number (should larger than 0)
+REM This option is useful for simulation with multi-computers
+SET /a START_INDEX=1
+SET /a TOTOAL_COPTER=6
+
+REM Set the vehicleType/ClassID of vehicle 3D display in RflySim3D
+
+SET /a CLASS_3D_ID=-1
+
+
+REM Set use DLL model name or not, use number index or name string
+REM This option is useful for simulation with other types of vehicles instead of multicopters
+set DLLModel=0
+
+REM Set the simulation mode on CopterSim, use number index or name string
+REM e.g., SimMode=2 equals to  SimMode=PX4_SITL_RFLY
+set SimMode=2
+
+REM Set the vehicle-model (airframe) of PX4 SITL simulation, the default airframe is a quadcopter: iris
+REM Check folder Firmware\ROMFS\px4fmu_common\init.d-posix for supported airframes (Note: You can also create your airframe file here)
+REM E.g., fixed-wing aircraft: PX4SitlFrame=plane; small cars: PX4SitlFrame=rover
+set PX4SitlFrame=iris
+
+
+REM Set the map, use index or name of the map on CopterSim
+REM e.g., UE4_MAP=1 equals to UE4_MAP=Grasslands
+SET UE4_MAP=OldFactory
+
+REM Set the origin x,y position (m) and yaw angle (degree) at the map
+SET /a ORIGIN_POS_X=-250
+SET /a ORIGIN_POS_Y=-119
+SET /a ORIGIN_YAW=0
+
+REM Set the interval between two vehicle, unit:m 
+SET /a VEHICLE_INTERVAL=3
+
+
+REM Set broadcast to other computer; 0: only this computer, 1: broadcast; or use IP address to increase speed
+REM e.g., IS_BROADCAST=0 equals to IS_BROADCAST=127.0.0.1, IS_BROADCAST=1 equals to IS_BROADCAST=255.255.255.255
+SET IS_BROADCAST=0
+
+REM Set UDP data mode; 0: UDP_FULL, 1:UDP_Simple, 2: Mavlink_Full, 3: Mavlink_simple. input number or string
+REM 4:Mavlink_NoSend, 5:Mavlink_NoGPS, 6:Mavlink_Vision (NoGPS and set PX4 EKF)
+SET UDPSIMMODE=1
+
+:Top
+ECHO.
+ECHO ---------------------------------------
+REM Max vehicle number 50
+SET /a MAX_VEHICLE=50
+SET /A VehicleNum=3
+SET /A Evaluated=VehicleNum
+if %Evaluated% EQU %VehicleNum% (
+    IF %VehicleNum% GTR 0 (
+        IF %VehicleNum% GTR %MAX_VEHICLE% (
+            ECHO The vehicle number is too large, which may cause a crash
+            pause
+        )
+        GOTO StartSim
+    )
+    ECHO Not a positive integer
+    GOTO Top
+) ELSE (
+    ECHO Not a integer
+    GOTO Top
+)
+:StartSim
+
+
+SET /A VehicleTotalNum=%VehicleNum% + %START_INDEX% - 1
+if not defined TOTOAL_COPTER (
+    SET /A TOTOAL_COPTER=%VehicleTotalNum%
+)
+
+set /a sqrtNum=1
+set /a squareNum=1
+:loopSqrt
+set /a squareNum=%sqrtNum% * %sqrtNum%
+if %squareNum% EQU %TOTOAL_COPTER% (
+    goto loopSqrtEnd
+)
+if %squareNum% GTR %TOTOAL_COPTER% (
+    goto loopSqrtEnd
+)
+set /a sqrtNum=%sqrtNum%+1
+goto loopSqrt
+:loopSqrtEnd
+
+
+REM QGCPath
+tasklist|find /i "QGroundControl.exe" || start %PSP_PATH%\QGroundControl\QGroundControl.exe -noComPix
+ECHO Start QGroundControl
+
+REM UE4Path
+cd /d %PSP_PATH%\RflySim3D
+tasklist|find /i "RflySim3D.exe" || start %PSP_PATH%\RflySim3D\RflySim3D.exe -key=I1
+choice /t 5 /d y /n >nul
+
+
+tasklist|find /i "CopterSim.exe" && taskkill /im "CopterSim.exe"
+ECHO Kill all CopterSims
+
+
+REM CptSmPath
+cd /d %PSP_PATH%\CopterSim
+
+
+set /a cntr=%START_INDEX%
+set /a endNum=%VehicleTotalNum%+1
+
+:loopBegin
+set /a PosXX=((%cntr%-1) / %sqrtNum%)*%VEHICLE_INTERVAL% + %ORIGIN_POS_X%
+set /a PosYY=((%cntr%-1) %% %sqrtNum%)*%VEHICLE_INTERVAL% + %ORIGIN_POS_Y%
+start /realtime CopterSim.exe 1 %cntr% %CLASS_3D_ID% %DLLModel% %SimMode% %UE4_MAP% %IS_BROADCAST% %PosXX% %PosYY% %ORIGIN_YAW% 1 %UDPSIMMODE%
+choice /t 1 /d y /n >nul
+set /a cntr=%cntr%+1
+
+if %cntr% EQU %endNum% goto loopEnd
+goto loopBegin
+:loopEnd
+
+REM Set ToolChainType 1:Win10WSL 3:Cygwin
+SET /a ToolChainType=1
+
+
+if "%IS_BROADCAST%" == "0" (
+    SET IS_BROADCAST=0
+) else (
+    SET IS_BROADCAST=1
+)
+
+SET WINDOWSPATH=%PATH%
+if %ToolChainType% EQU 1 (
+    wsl -d RflySim-20.04 echo Starting PX4 Build; cd %PSP_PATH_LINUX%/Firmware; ./BkFile/EnvOri.sh; export PATH=$HOME/ninja:$HOME/gcc-arm-none-eabi-7-2017-q4-major/bin:$PATH;make px4_sitl_default; ./Tools/sitl_multiple_run_rfly.sh %VehicleNum% %START_INDEX% %PX4SitlFrame%;echo Press any key to exit; read -n 1
+) else (
+    REM CYGPath
+    cd /d %PSP_PATH%\CygwinToolchain
+    CALL setPX4Env.bat
+    bash -l -i -c 'echo Starting SITL SIMULATION; cd %PSP_PATH_LINUX%/Firmware; ./BkFile/EnvOri.sh; pwd; make px4_sitl_default; ./Tools/sitl_multiple_run_rfly.sh %VehicleNum% %START_INDEX% %PX4SitlFrame%;echo Press any key to exit; read -n 1; pkill -x px4 || true;'
+)
+SET PATH=%WINDOWSPATH%
+
+
+REM kill all applications when press a key
+tasklist|find /i "CopterSim.exe" && taskkill /im "CopterSim.exe"
+tasklist|find /i "QGroundControl.exe" && taskkill /f /im "QGroundControl.exe"
+tasklist|find /i "RflySim3D.exe" && taskkill /f /im "RflySim3D.exe"
+SET BatName=%~n0
+tasklist|find /i "%BatName%.exe" && taskkill /f /im "%BatName%.exe"
+taskkill /F /IM cmd.exe /T
+ECHO Start End.

基础智能/e4.四旋翼与无人车异构集群协同控制实验/Python38Run.bat

@@ -0,0 +1,5 @@
+if not defined PSP_PATH (
+    SET PSP_PATH=C:\PX4PSP
+    SET PSP_PATH_LINUX=/mnt/c/PX4PSP
+)
+start cmd.exe /k "echo Python3.8 environment has been set with openCV+pymavlink+numpy+pyulog etc. && echo You can use pip or pip3 command to install other libraries && echo Put Python38Run.bat into your code folder && echo Use the command: 'python XXX.py' to run the script with Python && SET PATH=%PSP_PATH%\Python38;%PSP_PATH%\Python38\Scripts;%CD%;%PATH%"

基础智能/e4.四旋翼与无人车异构集群协同控制实验/UDPSimpleMultiVehicleSwarm.py

@@ -0,0 +1,83 @@
+import time
+import math
+import sys
+import numpy as np
+import PX4MavCtrlV4 as PX4MavCtrl
+VehilceNum = 3
+CarNum = 3
+
+MavList=[]
+flagI = np.zeros(VehilceNum, dtype=bool)
+# Create MAV instance
+for i in range(VehilceNum + CarNum):
+    MavList=MavList+[PX4MavCtrl.PX4MavCtrler(1+i)]
+
+time.sleep(2)
+# Start MAV loop with UDP mode: Udp_Simple
+for i in range(VehilceNum+CarNum):
+    MavList[i].InitMavLoop(1)
+
+# Enter Offboard mode to start vehicle control
+time.sleep(2)
+for i in range(VehilceNum+CarNum):
+    MavList[i].initOffboard()
+    time.sleep(0.5)
+
+time.sleep(2)
+for i in range(VehilceNum+CarNum):
+    MavList[i].SendMavArm(True)
+# Get the takeoff position of each vehicle to the UE4 Map
+# this can be adopted to obtain the global position of a vehicle in swarm simulation
+time.sleep(5)
+Error2UE4Map=[]
+for i in range(VehilceNum):
+    mav=MavList[i]
+    Error2UE4Map = Error2UE4Map+[-np.array([mav.uavGlobalPos[0]-mav.uavPosNED[0],mav.uavGlobalPos[1]-mav.uavPosNED[1],mav.uavGlobalPos[2]-mav.uavPosNED[2]])]
+
+# fly to 10m high above its takeoff position
+for i in range(VehilceNum):
+    MavList[i].SendPosNED(0, 0, -10, 0) 
+time.sleep(10)
+
+
+lastTime = time.time()
+startTime = time.time()
+timeInterval = 1/30.0 #here is 0.0333s (30Hz)
+
+while True:
+    lastTime = lastTime + timeInterval
+    sleepTime = lastTime - time.time()
+    if sleepTime > 0:
+        time.sleep(sleepTime) # sleep until the desired clock
+    else:
+        lastTime = time.time()
+    # The following code will be executed 30Hz (0.0333s)
+    
+    t=time.time()-startTime
+    for j in range(VehilceNum):
+        
+        mav=MavList[j]
+
+        if abs(mav.uavPosNED[2] - (-10)) <2 and (flagI[j] == False):
+            flagI[j] = True
+            print(str(j)+"号飞机已到达指定高度")
+        
+        if flagI[j] == True:
+            mav.SendPosNED(MavList[j+3].uavPosNED[0],MavList[j+3].uavPosNED[1],mav.uavPosNED[2],0)
+    
+    for i in range(VehilceNum,VehilceNum+CarNum):
+        if flagI[i-3]  == True:
+            # print(f"编号 {i-3}：北 {MavList[i-3].uavPosNED[0]:.2f} m，东 {MavList[i-3].uavPosNED[1]:.2f} m，地 {MavList[i-3].uavPosNED[2]:.2f} m")
+            MavList[i].SendPosNEDNoYaw(MavList[i].uavPosNED[0]+2.5,MavList[i].uavPosNED[1],0)
+        
+
+    if t>200: # end simulation when 200s
+        break
+    
+
+for i in range(VehilceNum):
+    MavList[i].endOffboard() 
+
+time.sleep(1)
+for i in range(VehilceNum):
+    MavList[i].stopRun()