fix：新增例程

基础智能/e1.集群Python接口演示实验/HITLMAVLinkFull4Swarm.bat

@@ -0,0 +1,188 @@
+@ECHO OFF
+
+REM Run script as administrator
+%1 mshta vbscript:CreateObject("Shell.Application").ShellExecute("cmd.exe","/c %~s0 ::","","runas",1)(window.close)&&exit
+
+
+REM The text start with 'REM' is annotation, the following options are corresponding to Options on CopterSim
+
+REM Set the path of the RflySim tools
+if not defined PSP_PATH (
+    SET PSP_PATH=C:\PX4PSP
+    SET PSP_PATH_LINUX=/mnt/c/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=1
+
+REM Auto determine CopterID according to SysID of Pixhawk
+SET /a IsSysID=0
+
+REM Total vehicle Number to auto arrange position
+REM SET /a TOTOAL_COPTER=8
+
+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 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., 0:PX4_HITL, 1:PX4_SITL, 2:PX4_SITL_RFLY, 3:"Simulink&DLL_SIL", 4:PX4_HITL_NET, 5:EXT_HITL_COM, 6:EXT_SIM_NET
+set SimMode=0
+
+REM SET the baudrate for Pixhawk COM
+SET /a BaudRate=921600
+
+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=Grasslands
+
+REM Set the origin x,y position (m) and yaw angle (degree) at the map
+SET /a ORIGIN_POS_X=0
+SET /a ORIGIN_POS_Y=0
+SET /a ORIGIN_YAW=0
+
+REM Set the interval between two vehicle, unit:m 
+SET /a VEHICLE_INTERVAL=2
+
+
+REM Set broadcast to other computer; IS_BROADCAST=0: only this computer, IS_BROADCAST=1: broadcast; 
+REM or use IP address to increase speed, e.g., IS_BROADCAST=192.168.3.1
+REM Note: in IP mode, IS_BROADCAST=0 equals to IS_BROADCAST=127.0.0.1, IS_BROADCAST=1 equals to IS_BROADCAST=255.255.255.255
+REM You can also use a IP list with seperator "," or ";" to specify IPs to send, e.g., 127.0.0.1,192.168.1.4,192.168.1.5
+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=Mavlink_Full
+
+ECHO.
+ECHO ---------------------------------------
+REM Get the Com port number
+for /f "delims=" %%t in ('%PSP_PATH%\CopterSim\GetComList.exe 2 %BaudRate%') do set ComNumExe=%%t
+
+REM Get the Com port list
+for /f "delims=" %%t in ('%PSP_PATH%\CopterSim\GetComList.exe 0 %BaudRate%') do set ComNameList=%%t
+
+REM Get the Com port info
+for /f "delims=" %%t in ('%PSP_PATH%\CopterSim\GetComList.exe 1 %BaudRate%') do set ComInfoList=%%t
+
+echo Please input the Pixhawk COM port list for HIL
+echo Use ',' as the separator if more than one Pixhawk
+echo E.g., input 3 for COM3 of Pixhawk on the computer
+echo Input 3,6,7 for COM3, COM6 and COM7 of Pixhawks 
+
+ECHO ---------------------------------------
+set remain=%ComInfoList%
+echo All COM ports on this computer are:
+echo.
+:loopInfo
+for /f "tokens=1* delims=;" %%a in ("%remain%") do (
+    echo %%a
+    set remain=%%b
+)
+if defined remain goto :loopInfo
+echo.
+ECHO ---------------------------------------
+
+if %ComNumExe% EQU 0 (
+    echo Warning: there is no available COM port
+) else (
+    echo Recommended COM list input is: %ComNameList%
+)
+
+ECHO.
+ECHO ---------------------------------------
+SET /P ComNum=My COM list for HITL simulation is:
+SET string=%ComNum%
+set subStr = ""
+set /a VehicleNum=0
+:split
+    for /f "tokens=1,* delims=," %%i in ("%string%") do (
+    set subStr=%%i
+    set string=%%j
+    )
+    set /a eValue=subStr
+    if not %eValue% EQU %subStr% (
+        echo Error: Input '%subStr%' is not a integer!
+        goto EOF
+    )
+    set /a VehicleNum = VehicleNum +1
+if not "%string%"=="" goto split
+REM cho total com number is %VehicleNum%
+
+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 UE4Path
+cd /d %PSP_PATH%\RflySim3D
+tasklist|find /i "RflySim3D.exe" || start %PSP_PATH%\RflySim3D\RflySim3D.exe
+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
+
+SET string=%ComNum%
+:split1
+    for /f "tokens=1,* delims=," %%i in ("%string%") do (
+    set subStr=%%i
+    set string=%%j
+    )
+    set /a PosXX=((%cntr%-1) / %sqrtNum%)*%VEHICLE_INTERVAL% + %ORIGIN_POS_X%
+    set /a PosYY=((%cntr%-1) %% %sqrtNum%)*%VEHICLE_INTERVAL% + %ORIGIN_POS_Y%
+    REM echo start CopterSim
+    start /realtime CopterSim.exe 1 %cntr% %CLASS_3D_ID% %DLLModel% %SimMode% %UE4_MAP% %IS_BROADCAST% %PosXX% %PosYY% %ORIGIN_YAW% %subStr%:%BaudRate% %UDPSIMMODE% %IsSysID%
+    choice /t 2 /d y /n >nul
+    set /a cntr=%cntr%+1
+    
+    REM TIMEOUT /T 1
+if not "%string%"=="" goto split1
+
+REM QGCPath
+tasklist|find /i "QGroundControl.exe" || start %PSP_PATH%\QGroundControl\QGroundControl.exe -noComPix
+ECHO Start QGroundControl
+
+pause
+
+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.

基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.bat

@@ -0,0 +1,135 @@
+@ECHO OFF
+
+REM Run script as administrator
+%1 mshta vbscript:CreateObject("Shell.Application").ShellExecute("cmd.exe","/c ""%~s0"" ::","","runas",1)(window.close)&&exit
+
+
+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
+
+
+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=Grasslands
+
+REM Set the origin x,y position (m) and yaw angle (degree) at the map
+SET /a ORIGIN_POS_X=0
+SET /a ORIGIN_POS_Y=0
+SET /a ORIGIN_YAW=0
+
+REM Set the interval between two vehicle, unit:m 
+SET /a VEHICLE_INTERVAL=2
+
+
+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=2
+
+REM Set the path of the RflySim tools
+if not defined PSP_PATH (
+    SET PSP_PATH=C:\PX4PSP
+    SET PSP_PATH_LINUX=/mnt/c/PX4PSP
+)
+
+
+SET /a VehicleNum=4
+
+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
+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
+
+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"
+
+ECHO Start End.

基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.py

@@ -0,0 +1,82 @@
+import time
+import math
+import sys
+
+import PX4MavCtrlV4 as PX4MavCtrl
+
+# Create four MAVLink API instances, the CopterSim UDP ports are 20100+2*i, where i=1,2,3,4
+mav = PX4MavCtrl.PX4MavCtrler(1)
+mav1 = PX4MavCtrl.PX4MavCtrler(2)
+mav2 = PX4MavCtrl.PX4MavCtrler(3)
+mav3 = PX4MavCtrl.PX4MavCtrler(4)
+
+# Start MAVLink listening loop from CopterSim/Pixhawk
+# MAVLink Full mode is adopted here
+mav.InitMavLoop()
+mav1.InitMavLoop()
+mav2.InitMavLoop()
+mav3.InitMavLoop()
+time.sleep(2)
+
+# Display the position, velocity, angle, angular rate from Pixhawk/CopterSIm
+print((mav.uavPosNED,mav.uavVelNED,mav.uavAngEular,mav.uavAngRate,mav.uavPosGPSHome,mav.uavPosGPS,mav.uavGlobalPos))
+print((mav1.uavPosNED,mav1.uavVelNED,mav1.uavAngEular,mav1.uavAngRate,mav1.uavPosGPSHome,mav1.uavPosGPS,mav1.uavGlobalPos))
+print((mav2.uavPosNED,mav2.uavVelNED,mav2.uavAngEular,mav2.uavAngRate,mav2.uavPosGPSHome,mav2.uavPosGPS,mav2.uavGlobalPos))
+print((mav3.uavPosNED,mav3.uavVelNED,mav3.uavAngEular,mav3.uavAngRate,mav3.uavPosGPSHome,mav3.uavPosGPS,mav3.uavGlobalPos))
+
+
+# Start Offboard mode of Pixhawk
+mav.initOffboard()
+mav1.initOffboard()
+mav2.initOffboard()
+mav3.initOffboard()
+
+
+# send desired poisition control command, fly to target postion 0,0,-1.7m with yaw 0 degree
+mav.SendPosNED(0, 0, -1.7, 0) 
+mav1.SendPosNED(2, 2, -1.7, 0) 
+mav2.SendPosNED(-2, -2, -1.7, 0) 
+mav3.SendPosNED(0, 0, -3, 0) 
+
+
+print("Send target Pos")
+time.sleep(0.5)
+# Send arm command
+mav.SendMavArm(True) 
+time.sleep(2)
+mav1.SendMavArm(True) 
+time.sleep(2)
+mav2.SendMavArm(True) 
+time.sleep(2)
+mav3.SendMavArm(True) 
+print("Send Arm Command")
+
+time.sleep(10)
+
+# send desired NED velocity signals, 0.2m/s download
+mav.SendVelNED(0, 0, 0.2, 0) 
+mav1.SendVelNED(0, 0, 0.2, 0) 
+mav2.SendVelNED(0, 0, 0.2, 0) 
+mav3.SendVelNED(0, 0, 0.2, 0) 
+print("Send Velocity Speed")
+
+time.sleep(10)
+
+# exit from Offboard control mode of Pixhawk
+print("Send offboard stop")
+mav.endOffboard()
+mav1.endOffboard()
+mav2.endOffboard()
+mav3.endOffboard()
+time.sleep(1)
+
+# exit from MAVLink listening loop
+print("Send Mavlink stop")
+mav.stopRun()
+mav1.stopRun()
+mav2.stopRun()
+mav3.stopRun()
+time.sleep(1)
+#while True:
+#    print(mav.uavPosNED)
+#    time.sleep(2)

基础智能/e1.集群Python接口演示实验/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%"