fix：新增例程
This commit is contained in:
BIN
基础编程/e7.Offboard模式介绍及控制实验/Readme.docx
Normal file
BIN
基础编程/e7.Offboard模式介绍及控制实验/Readme.docx
Normal file
Binary file not shown.
BIN
基础编程/e7.Offboard模式介绍及控制实验/Readme.pdf
Normal file
BIN
基础编程/e7.Offboard模式介绍及控制实验/Readme.pdf
Normal file
Binary file not shown.
89
基础编程/e7.Offboard模式介绍及控制实验/code/OffboardCircle.py
Normal file
89
基础编程/e7.Offboard模式介绍及控制实验/code/OffboardCircle.py
Normal file
@@ -0,0 +1,89 @@
|
||||
|
||||
# import required libraries
|
||||
# pip3 install pymavlink pyserial
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import time
|
||||
import VisionCaptureApi
|
||||
import PX4MavCtrlV4 as PX4MavCtrl
|
||||
import math
|
||||
import ReqCopterSim
|
||||
|
||||
req = ReqCopterSim.ReqCopterSim() # 获取局域网内所有CopterSim程序的电脑IP列表
|
||||
StartCopterID = 1 # 初始飞机的ID号
|
||||
TargetIP = req.getSimIpID(StartCopterID) # 获取CopterSim的1号程序所在电脑的IP，作为目标IP
|
||||
# 注意：如果是本电脑运行的话，那TargetIP是127.0.0.1的本机地址；如果是远程访问，则是192打头的局域网地址。
|
||||
# 因此本程序能同时在本机运行，也能在其他电脑运行。
|
||||
|
||||
VehilceNum = 1
|
||||
MavList=[]
|
||||
# Create MAV instance
|
||||
for i in range(VehilceNum):
|
||||
CopterID=StartCopterID+i # 当前配置的飞机序号
|
||||
TargetIP = req.getSimIpID(CopterID) # 获取对应的电脑IP
|
||||
req.sendReSimIP(CopterID) # 请求回传数据到本电脑
|
||||
time.sleep(1)
|
||||
MavList = MavList+[PX4MavCtrl.PX4MavCtrler(CopterID,TargetIP)] # 初始化并建立i号飞机的MAVLink通信连接
|
||||
|
||||
time.sleep(2)
|
||||
# Start MAV loop with UDP mode: MAVLINK_FULL
|
||||
for i in range(VehilceNum):
|
||||
MavList[i].InitMavLoop()
|
||||
|
||||
# Enter Offboard mode to start vehicle control
|
||||
time.sleep(2)
|
||||
for i in range(VehilceNum):
|
||||
MavList[i].initOffboard()
|
||||
|
||||
# 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(2)
|
||||
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]
|
||||
|
||||
# target position in UE4 map global frame
|
||||
if t<10:
|
||||
targetPosE=np.array([-0,0,-15]) # fly to 15 meters high in 0 to 10s
|
||||
else: #fly circle after 10s
|
||||
targetPosE=np.array([10*math.sin(t/2+math.pi/2)-10,10*math.sin(t/2.0),-15])
|
||||
|
||||
# target position in vehilce takeoff frame
|
||||
targetPosE=targetPosE+Error2UE4Map[j]
|
||||
mav.SendPosNED(targetPosE[0],targetPosE[1],targetPosE[2],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()
|
||||
|
||||
5
基础编程/e7.Offboard模式介绍及控制实验/code/Python38Run.bat
Normal file
5
基础编程/e7.Offboard模式介绍及控制实验/code/Python38Run.bat
Normal file
@@ -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%"
|
||||
171
基础编程/e7.Offboard模式介绍及控制实验/code/SITLRun.bat
Normal file
171
基础编程/e7.Offboard模式介绍及控制实验/code/SITLRun.bat
Normal file
@@ -0,0 +1,171 @@
|
||||
@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=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=1
|
||||
|
||||
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., 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; 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
|
||||
|
||||
|
||||
:Top
|
||||
ECHO.
|
||||
ECHO ---------------------------------------
|
||||
REM Max vehicle number 50
|
||||
SET /a MAX_VEHICLE=50
|
||||
SET /P VehicleNum=Please input UAV swarm number:
|
||||
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
|
||||
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"
|
||||
|
||||
ECHO Start End.
|
||||
Reference in New Issue
Block a user