|
| | __init__ (self, CopterID=1, Vehicletype=3, mapName='Grasslands', updatefreq=100, isBroadCast=False) |
| | 构造函数,初始化飞行器的ID、类型、地图场景、更新频率以及其他控制参数。
|
| | yawSat (self, yaw) |
| | 对输入的偏航角进行角度约束,将偏航角限制在[-pi, pi]区间内。
|
| | yawSat1 (self, yaw) |
| | 对偏航角进行幅度限制,将偏航角限制在[-pi, pi]附近的范围内。
|
| | RollSat (self, Roll) |
| | 对滚转角进行幅度限制,将滚转角限制在[-pi/6, pi/6]范围内。
|
| | sat (self, inPwm=0, thres=1) |
| | 对输入的PWM值进行饱和限制,限制在[-thres, thres]范围内。
|
| | initSimpleModel (self, intState=[0, 0, 0], targetIP='127.0.0.1', GPSOrigin=[40.1540302, 116.2593683, 50]) |
| | 初始化简化模型(质点模型)的运行环境,包括起始状态、目标IP地址和GPS原点设定。 启动模型更新循环与Mavlink数据监听线程。
|
| | RecMavLoop (self) |
| | 接收并处理来自Mavlink的控制数据循环线程。
|
| | EndSimpleModel (self) |
| | 停止质点模型的运行。
|
| | enFixedWRWTO (self) |
| | 固定翼滑跑(Runway TakeOff)相关预留函数(当前未实现)。
|
| | TypeMask (self, EnList) |
| | 根据EnList生成POSITION_TARGET_LOCAL_NED消息的type_mask位图。
|
| | SimpleModelLoop (self) |
| | 点质量模型的主循环函数,以固定频率更新模型状态、发送UAV状态和输出数据到RflySim3D。
|
| | ModelStep (self, CurTime=-1) |
| | 模型单步更新函数。
|
| | SendUavState (self, CurTime) |
| | 发送UAV的状态数据给Simulink,用于模拟PX4内部估计状态。
|
| | ProssInput (self, dt) |
| | 根据当前模式和控制指令对输入进行处理的函数。
|
| | fixedPitchFromVel (self, vel) |
| | 根据当前速度vel来确定飞行器的俯仰角。
|
| | Step (self, dt) |
| | 使用龙格-库塔法以及内部定义的动力学方程,进行状态更新(位置、速度、高度、航向等)。
|
| | SendOutput (self, CurTime) |
| | 将当前飞行器的真实状态(位置、姿态、速度)发送给RflySim3D以更新显示。
|
| | SendPosNED (self, x=0, y=0, z=0, yaw=0) |
| | 发送目标位置与偏航指令给PX4(NED坐标系)。 设置CurFlag=2表示进入某种自定义模式(例如Offboard位置控制),offMode=0表示SET_POSITION_TARGET_LOCAL_NED消息格式。 启用位置和偏航控制相关的使能位,禁用速度、加速度等其他通道。
|
| | SendPosNEDNoYaw (self, x=0, y=0, z=0) |
| | 发送目标位置指令给PX4(NED坐标系,不控制偏航)。 设置CurFlag=0表示Offboard模式,offMode=0表示SET_POSITION_TARGET_LOCAL_NED消息格式。 仅启用位置控制,使能位中不启用yaw控制通道。
|
| | SendPosFRD (self, x=0, y=0, z=0, yaw=0) |
| | SendPosFRDNoYaw (self, x=0, y=0, z=0) |
| | SendVelNED (self, vx=0, vy=0, vz=0, yawrate=0) |
| | SendVelNEDNoYaw (self, vx, vy, vz) |
| | SendVelFRD (self, vx=0, vy=0, vz=0, yawrate=0) |
| | SendVelNoYaw (self, vx, vy, vz) |
|
| sendBuf (self, buf, windowID=-1) |
| | sendUE4PosNew (self, copterID=1, vehicleType=3, PosE=[0, 0, 0], AngEuler=[0, 0, 0], VelE=[0, 0, 0], PWMs=[0] *8, runnedTime=-1, windowID=-1) |
| | SendCruiseSpeed (self, Speed=10) |
| | SendCopterSpeed (self, Speed=5) |
| | SendGroundSpeed (self, Speed=15) |
| | SendCruiseRadius (self, rad=20) |
|
| SendMavArm (self, isArm=0) |
| | sendUE4Cmd (self, cmd, windowID=-1) |
| | SendVelYawAlt (self, vel=10, alt=-100, yaw=6.28) |
| | sendMavTakeOff (self, xM=0, yM=0, zM=0, YawRad=0, PitchRad=20/180.0 *math.pi) |
| | sendMavTakeOffGPS (self, lat, lon, alt, yawDeg=0, pitchDeg=15) |
|
|
| udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) |
|
| CopterID = CopterID |
|
| Vehicletype = Vehicletype |
|
| updatefreq = updatefreq |
|
| isBroadCast = isBroadCast |
|
| map = UEMapServe(mapName) |
|
int | intStateX = 0 |
|
int | intStateY = 0 |
|
int | intAlt = 0 |
|
int | intStateYaw = 0 |
|
bool | isInPointMode = False |
|
list | uavAngEular = [0, 0, 0] |
|
list | trueAngEular = [0, 0, 0] |
|
list | uavAngRate = [0, 0, 0] |
|
list | trueAngRate = [0, 0, 0] |
|
list | uavPosNED = [0, 0, 0] |
|
list | truePosNED = [0, 0, 0] |
|
list | uavVelNED = [0, 0, 0] |
|
list | trueVelNED = [0, 0, 0] |
|
list | uavPosGPS = [0, 0, 0,0, 0, 0,0,0,0] |
|
list | uavGlobalPos = [0, 0, 0] |
|
int | offMode = 0 |
|
list | EnList = [0,1,0,0,0,1] |
|
| type_mask = self.TypeMask(self.EnList) |
|
int | coordinate_frame = mavlink2.MAV_FRAME_BODY_NED |
|
list | pos = [0,0,0] |
|
list | vel = [0,0,0] |
|
list | acc = [0, 0, 0] |
|
int | yaw = 0 |
|
int | yawrate = 0 |
|
list | B = [5, 10, 5, 12, 1] |
|
list | velOff = list(self.vel) |
|
int | yawOff = self.yaw |
|
int | yawRateOff = self.yawrate |
|
list | MotorRPMS = [0,0,0,0,0,0,0,0] |
|
int | velair = 0 |
|
int | MaxSpeed = 20 |
|
int | FwSpeedTrim = 10 |
|
int | FwLoiterRad = 10 |
|
bool | isArmed = False |
|
int | CurFlag = 0 |
|
int | lastFlag = 0 |
|
int | TkOffYaw = 0 |
|
int | TkOffAccSpeed = 1 |
|
int | TkOffMaxSpeed = 10 |
|
int | TkOffNextFlag = 11 |
|
int | ClimbSlope = 10/180.0*math.pi |
|
int | ClimbStartSpeed = 10 |
|
int | ClimbAccSpeed = 1 |
|
int | ClimbMaxSpeed = 20 |
|
int | ClimbMaxAlt = -50 |
|
int | ClimbNextFlag = 12 |
|
int | ForwardSpeed = 20 |
|
list | ForwardPosXYZ = [180,0,-50] |
|
int | ForwardNextFlag = 13 |
|
int | CircleSpeed = 20 |
|
int | CircleRadius = 20 |
|
list | CirclePosXYZ = [200,0,-50] |
|
int | CircleDir = 1 |
|
int | VehiclePitch = 0 |
|
int | VehicleYaw = 0 |
|
list | GPSOrigin = [40.1540302,116.2593683,50] |
|
| geo = EarthModel() |
|
int | lastTimeStep = -1 |
|
list | lastCtrl = [0,0,0,0] |
|
| t3 = threading.Thread(target=self.SimpleModelLoop, args=()) |
|
| udpCtrlRec = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) |
|
tuple | RecIPPort = ('0.0.0.0', 20100+(self.CopterID-1)*2) |
|
tuple | SendIpPort = (targetIP,20100+(self.CopterID-1)*2+1) |
|
bool | t4Flag = True |
|
| t4 = threading.Thread(target=self.RecMavLoop, args=()) |
|
float | startTime3 = time.time() |
|
| startTime = time.time() |
|
| lastTime3 = self.startTime3 |
|
int | CircleStep = self.CircleStep+W*dt |
|
| uavVelAir = math.sqrt(self.uavVelNED[0]*self.uavVelNED[0]+self.uavVelNED[1]*self.uavVelNED[1]) |
|
int | yawf = self.yaw |
|
int | yawRatef = self.yawrate |
|
list | velEf = self.vel |
|
list | targetPosEf = self.pos |
|
int | VelAirf = self.velair |
|
| trueAngRat = list(self.uavAngRate) |
|
| uavTimeStmp = CurTime |
|
| trueTimeStmp = CurTime |
|
| GNDSpeed = Speed |
本类用于创建并控制飞行器对象(Vehicle)。包含对飞行器类型、位置、姿态、速度、飞行模式以及地图场景等的初始化和更新逻辑。
| sendUE4Cmd |
( |
| self, |
|
|
| cmd, |
|
|
| windowID = -1 ) |
send command to control the display style of RflySim3D
The available command are list as follows, the command string is as b'RflyShowTextTime txt time'
RflyShowTextTime(String txt, float time)\\ let UE4 show txt with time second
RflyShowText(String txt)\\ let UE4 show txt 5 second
RflyChangeMapbyID(int id)\\ Change the map to ID (int number)
RflyChangeMapbyName(String txt)\\ Change to map with name txt
RflyChangeViewKeyCmd(String key, int num) \\ the same as press key + num on UE4
RflyCameraPosAngAdd(float x, float y, float z,float roll,float pitch,float yaw) \\ move the camera with x-y-z(m) and roll-pitch-yaw(degree) related to current pos
RflyCameraPosAng(float x, float y, float z, float roll, float pitch, float yaw) \\ set the camera with x-y-z(m) and roll-pitch-yaw(degree) related to UE origin
RflyCameraFovDegrees(float degrees) \\ change the cameras fov (degree)
RflyChange3DModel(int CopterID, int veTypes=0) \\ change the vehicle 3D model to ID
RflyChangeVehicleSize(int CopterID, float size=0) \\change vhielce's size
RflyMoveVehiclePosAng(int CopterID, int isFitGround, float x, float y, float z, float roll, float pitch, float yaw) \\ move the vehicle's x-y-z(m) and roll-pitch-yaw(degree) related to current pos
RflySetVehiclePosAng(int CopterID, int isFitGround, float x, float y, float z, float roll, float pitch, float yaw) \\ set the vehilce's x-y-z(m) and roll-pitch-yaw(degree) related to UE origin
RflyScanTerrainH(float xLeftBottom(m), float yLeftBottom(m), float xRightTop(m), float yRightTop(m), float scanHeight(m), float scanInterval(m)) \\ send command to let UE4 scan the map to generate png and txt files
RflyCesiumOriPos(double lat, double lon, double Alt) \\ change the lat, lon, Alt (degrees) of the Cesium map origin
RflyClearCapture \\ clear the image capture unit
struct Ue4CMD0{
int checksum;
char data[52];
} i52s
struct Ue4CMD{
int checksum;
char data[252];
} i252s