Module MAPLEAF.IO.HIL
Hardware in the loop simulation functionality. Orientation/Position/Velocity info is passed to a hardware control system, which passes control inputs back to the simulator.
Specific implementation - can serve as inspiration, but would have to be modified to work with other avionics systems
Expand source code
'''
Hardware in the loop simulation functionality.
Orientation/Position/Velocity info is passed to a hardware control system, which passes control inputs back to the simulator.
Specific implementation - can serve as inspiration, but would have to be modified to work with other avionics systems
'''
import struct
import time
import serial
from MAPLEAF.Motion import Quaternion
from MAPLEAF.Motion import Vector
class packet:
def __init__(self, address):
self.PTByte = 0
self.address = address
self.data = []
self.checkSumUpperByte = 0
self.checkSumLowerByte = 0
def writeData(self, writtenData):
for i in range(len(writtenData)):
self.data.append(writtenData[i])
def computeCheckSum(self):
total = ord('s') + ord('n') + ord('p') + self.PTByte + self.address
for i in range(len(self.data)):
total += self.data[i]
decimalRepresentation = (int)(total/1.0)
binaryRepresentation = decimalRepresentation.to_bytes(2,'big',signed=False)
self.checkSumUpperByte = binaryRepresentation[0]
self.checkSumLowerByte = binaryRepresentation[1]
def createPTByte(self, hasData, isBatch, numRegisters):
PTByteValue = 0
if(hasData == True):
PTByteValue += 128
if(isBatch == True):
PTByteValue += 64
PTByteValue += numRegisters*4
self.PTByte = PTByteValue
class HILInterface:
def __init__(self, quatUpdateRate, posUpdateRate, velUpdateRate, teensyComPort = "Com20", imuComPort = "Com15", teensyBaudrate = 9600, imuBaudrate = 57600):
self.teensyComs = serial.Serial()
self.teensyComs.port = teensyComPort
self.teensyComs.baudrate = teensyBaudrate
self.teensyComs.timeout = 0.001
self.imuSpoof = serial.Serial()
self.imuSpoof.port = imuComPort
self.imuSpoof.baudrate = imuBaudrate
self.imuSpoof.timeout = 0.001
self.quatUpdatePeriod = 1 / quatUpdateRate
self.posUpdatePeriod = 1 / posUpdateRate
self.velUpdatePeriod = 1 / velUpdateRate
self.startTime = time.time() #Gives milliseconds
self.lastUpdateTime = self.startTime
self.lastQuaternionUpdateTime = self.startTime
self.lastPositionUpdateTime = self.startTime
self.lastVelocityUpdateTime = self.startTime
self.canardAngles = [0]*4
self.teensyComs.open()
self.imuSpoof.open()
def sendTeensyCommands(self,command):
self.teensyComs.write(command)
def getCanardAngles(self):
#for i in range(4):
#print(self.canardAngles[i])
return self.canardAngles
def requestCanardAngles(self):
self.teensyComs.write('q'.encode('UTF-8'))
time.sleep(0.001)
for i in range(4):
a = self.teensyComs.readline()
if a == b'': #If there was a timeout
self.canardAngles[i] = self.canardAngles[i] #If there was a timeout just use the alst iterations angle
print("Serial Timeout")
else:
self.canardAngles[i] = float(a)
return self.canardAngles
def setupHIL(self,currentRigidBodyState):
self.sendTeensyCommands('R'.encode('UTF-8'))
self.sendIMUData(currentRigidBodyState,10000)
time.sleep(2)
self.sendTeensyCommands('L'.encode('UTF-8'))
self.startTime = time.time()
self.lastUpdateTime = self.startTime #Seconds
self.lastQuaternionUpdateTime = self.startTime
self.lastPositionUpdateTime = self.startTime
self.lastVelocityUpdateTime = self.startTime
return
def performHIL(self, currentRigidBodyState, simTime):
#Send IMU data if the update times have passed
self.sendIMUData(currentRigidBodyState, simTime)
#This is the delay waiting for the real world to catch up with the simulation
while simTime > (time.time() - self.startTime):
#print("Stalling")
#print('{0:.8f}'.format(time.time()-self.lastUpdateTime))
pass
#Send IMU data if the update times have passed
self.requestCanardAngles()
self.lastUpdateTime = time.time()
def sendIMUData(self, currentRigidBodyState, simTime):
#print("Hello1")
#print(simTime)
#print(self.startTime)
#print(self.lastQuaternionUpdateTime)
#print(self.quatUpdatePeriod)
#If the simulation time is greater than the quat update period
if((simTime + self.startTime - self.lastQuaternionUpdateTime) > self.quatUpdatePeriod):
#print("Hello2")
#send quaternion data
quaternionByteArray = self.getQuaternionByteArray(currentRigidBodyState)
myPacket = packet(109)
myPacket.writeData(quaternionByteArray)
myPacket.createPTByte(True, True, 3)
self.writePacket(myPacket)
#print("Sent")
self.lastQuaternionUpdateTime = simTime+self.startTime
if((simTime + self.startTime - self.lastPositionUpdateTime) > self.posUpdatePeriod):
#send position data
positionByteArray = self.getPositionByteArray(currentRigidBodyState)
myPacket = packet(117)
myPacket.writeData(positionByteArray)
myPacket.createPTByte(True, True, 4)
self.writePacket(myPacket)
self.lastPositionUpdateTime = simTime+self.startTime
if((simTime+self.startTime - self.lastVelocityUpdateTime) > self.velUpdatePeriod):
#send velocity data
velocityByteArray = self.getVelocityByteArray(currentRigidBodyState)
myPacket = packet(121)
myPacket.writeData(velocityByteArray)
myPacket.createPTByte(True, True, 4)
self.writePacket(myPacket)
self.lastVelocityUpdateTime = simTime+self.startTime
return
def convertQuaternionComponent(self, quaternionComponent):
imuDecimalRepresentation = (int)(quaternionComponent*29789.09091)
imuBinaryRepresentation = imuDecimalRepresentation.to_bytes(2,'big',signed=True)
imuUpperByte = imuBinaryRepresentation[0]
imuLowerByte = imuBinaryRepresentation[1]
return imuUpperByte, imuLowerByte
def convertPositionComponent(self, positionComponent):
ba = bytearray(struct.pack("f", positionComponent))
imuMSB = ba[3]
imu2MSB = ba[2]
imu2LSB = ba[1]
imuLSB = ba[0]
return imuMSB, imu2MSB, imu2LSB, imuLSB
def convertVelocityComponent(self, velocityComponent):
ba = bytearray(struct.pack("f", velocityComponent))
imuMSB = ba[3]
imu2MSB = ba[2]
imu2LSB = ba[1]
imuLSB = ba[0]
return imuMSB, imu2MSB, imu2LSB, imuLSB
def getQuaternionByteArray(self,currentRigidBodyState):
convertedQuaternion = self.getConvertedQuaternion(currentRigidBodyState)
random32BitTimeSpoof = (1).to_bytes(4,'big',signed=True)
ranTuple = (random32BitTimeSpoof[0], random32BitTimeSpoof[1], random32BitTimeSpoof[2], random32BitTimeSpoof[3])
byteArray = self.convertQuaternionComponent(convertedQuaternion.Q[0])
byteArray += (self.convertQuaternionComponent(convertedQuaternion.Q[1]))
byteArray += (self.convertQuaternionComponent(convertedQuaternion.Q[2]))
byteArray += (self.convertQuaternionComponent(convertedQuaternion.Q[3]))
byteArray += ranTuple
return byteArray
def getPositionByteArray(self,currentRigidBodyState):
convertedPosition = self.getConvertedPosition(currentRigidBodyState)
random32BitTimeSpoof = (1).to_bytes(4,'big',signed=True)
ranTuple = (random32BitTimeSpoof[0], random32BitTimeSpoof[1], random32BitTimeSpoof[2], random32BitTimeSpoof[3])
byteArray = self.convertPositionComponent(convertedPosition.X)
byteArray += self.convertPositionComponent(convertedPosition.Y)
byteArray += self.convertPositionComponent(convertedPosition.Z)
byteArray += ranTuple
return byteArray
def getVelocityByteArray(self,currentRigidBodyState):
convertedVelocity = self.getConvertedVelocity(currentRigidBodyState)
random32BitTimeSpoof = (1).to_bytes(4,'big',signed=True)
ranTuple = (random32BitTimeSpoof[0], random32BitTimeSpoof[1], random32BitTimeSpoof[2], random32BitTimeSpoof[3])
byteArray = self.convertVelocityComponent(convertedVelocity.X)
byteArray += self.convertVelocityComponent(convertedVelocity.Y)
byteArray += self.convertVelocityComponent(convertedVelocity.Z)
byteArray += ranTuple
return byteArray
def getConvertedQuaternion(self, currentRigidBodyState):
conversionQuaternion = Quaternion(axisOfRotation=Vector(1,0,0),angle=3.141592654)
convertedQuaternion = currentRigidBodyState.orientation*conversionQuaternion
return currentRigidBodyState.orientation
def getConvertedPosition(self,currentRigidBodyState):
convertedPosition = Vector(currentRigidBodyState.position.X,currentRigidBodyState.position.Y,-currentRigidBodyState.position.Z)
return convertedPosition
def getConvertedVelocity(self,currentRigidBodyState):
convertedVelocity = Vector(currentRigidBodyState.velocity.X,currentRigidBodyState.velocity.Y,-currentRigidBodyState.velocity.Z)
return convertedVelocity
def writePacket(self,myPacket):
myPacket.computeCheckSum()
self.imuSpoof.write('s'.encode('UTF-8'))
self.imuSpoof.write('n'.encode('UTF-8'))
self.imuSpoof.write('p'.encode('UTF-8'))
self.imuSpoof.write(bytes([myPacket.PTByte]))
self.imuSpoof.write(bytes([myPacket.address]))
self.imuSpoof.write(bytes(myPacket.data))
self.imuSpoof.write(bytes([myPacket.checkSumUpperByte]))
self.imuSpoof.write(bytes([myPacket.checkSumLowerByte]))
def __del__(self):
self.teensyComs.close()
self.imuSpoof.close()
Classes
class HILInterface (quatUpdateRate, posUpdateRate, velUpdateRate, teensyComPort='Com20', imuComPort='Com15', teensyBaudrate=9600, imuBaudrate=57600)
-
Expand source code
class HILInterface: def __init__(self, quatUpdateRate, posUpdateRate, velUpdateRate, teensyComPort = "Com20", imuComPort = "Com15", teensyBaudrate = 9600, imuBaudrate = 57600): self.teensyComs = serial.Serial() self.teensyComs.port = teensyComPort self.teensyComs.baudrate = teensyBaudrate self.teensyComs.timeout = 0.001 self.imuSpoof = serial.Serial() self.imuSpoof.port = imuComPort self.imuSpoof.baudrate = imuBaudrate self.imuSpoof.timeout = 0.001 self.quatUpdatePeriod = 1 / quatUpdateRate self.posUpdatePeriod = 1 / posUpdateRate self.velUpdatePeriod = 1 / velUpdateRate self.startTime = time.time() #Gives milliseconds self.lastUpdateTime = self.startTime self.lastQuaternionUpdateTime = self.startTime self.lastPositionUpdateTime = self.startTime self.lastVelocityUpdateTime = self.startTime self.canardAngles = [0]*4 self.teensyComs.open() self.imuSpoof.open() def sendTeensyCommands(self,command): self.teensyComs.write(command) def getCanardAngles(self): #for i in range(4): #print(self.canardAngles[i]) return self.canardAngles def requestCanardAngles(self): self.teensyComs.write('q'.encode('UTF-8')) time.sleep(0.001) for i in range(4): a = self.teensyComs.readline() if a == b'': #If there was a timeout self.canardAngles[i] = self.canardAngles[i] #If there was a timeout just use the alst iterations angle print("Serial Timeout") else: self.canardAngles[i] = float(a) return self.canardAngles def setupHIL(self,currentRigidBodyState): self.sendTeensyCommands('R'.encode('UTF-8')) self.sendIMUData(currentRigidBodyState,10000) time.sleep(2) self.sendTeensyCommands('L'.encode('UTF-8')) self.startTime = time.time() self.lastUpdateTime = self.startTime #Seconds self.lastQuaternionUpdateTime = self.startTime self.lastPositionUpdateTime = self.startTime self.lastVelocityUpdateTime = self.startTime return def performHIL(self, currentRigidBodyState, simTime): #Send IMU data if the update times have passed self.sendIMUData(currentRigidBodyState, simTime) #This is the delay waiting for the real world to catch up with the simulation while simTime > (time.time() - self.startTime): #print("Stalling") #print('{0:.8f}'.format(time.time()-self.lastUpdateTime)) pass #Send IMU data if the update times have passed self.requestCanardAngles() self.lastUpdateTime = time.time() def sendIMUData(self, currentRigidBodyState, simTime): #print("Hello1") #print(simTime) #print(self.startTime) #print(self.lastQuaternionUpdateTime) #print(self.quatUpdatePeriod) #If the simulation time is greater than the quat update period if((simTime + self.startTime - self.lastQuaternionUpdateTime) > self.quatUpdatePeriod): #print("Hello2") #send quaternion data quaternionByteArray = self.getQuaternionByteArray(currentRigidBodyState) myPacket = packet(109) myPacket.writeData(quaternionByteArray) myPacket.createPTByte(True, True, 3) self.writePacket(myPacket) #print("Sent") self.lastQuaternionUpdateTime = simTime+self.startTime if((simTime + self.startTime - self.lastPositionUpdateTime) > self.posUpdatePeriod): #send position data positionByteArray = self.getPositionByteArray(currentRigidBodyState) myPacket = packet(117) myPacket.writeData(positionByteArray) myPacket.createPTByte(True, True, 4) self.writePacket(myPacket) self.lastPositionUpdateTime = simTime+self.startTime if((simTime+self.startTime - self.lastVelocityUpdateTime) > self.velUpdatePeriod): #send velocity data velocityByteArray = self.getVelocityByteArray(currentRigidBodyState) myPacket = packet(121) myPacket.writeData(velocityByteArray) myPacket.createPTByte(True, True, 4) self.writePacket(myPacket) self.lastVelocityUpdateTime = simTime+self.startTime return def convertQuaternionComponent(self, quaternionComponent): imuDecimalRepresentation = (int)(quaternionComponent*29789.09091) imuBinaryRepresentation = imuDecimalRepresentation.to_bytes(2,'big',signed=True) imuUpperByte = imuBinaryRepresentation[0] imuLowerByte = imuBinaryRepresentation[1] return imuUpperByte, imuLowerByte def convertPositionComponent(self, positionComponent): ba = bytearray(struct.pack("f", positionComponent)) imuMSB = ba[3] imu2MSB = ba[2] imu2LSB = ba[1] imuLSB = ba[0] return imuMSB, imu2MSB, imu2LSB, imuLSB def convertVelocityComponent(self, velocityComponent): ba = bytearray(struct.pack("f", velocityComponent)) imuMSB = ba[3] imu2MSB = ba[2] imu2LSB = ba[1] imuLSB = ba[0] return imuMSB, imu2MSB, imu2LSB, imuLSB def getQuaternionByteArray(self,currentRigidBodyState): convertedQuaternion = self.getConvertedQuaternion(currentRigidBodyState) random32BitTimeSpoof = (1).to_bytes(4,'big',signed=True) ranTuple = (random32BitTimeSpoof[0], random32BitTimeSpoof[1], random32BitTimeSpoof[2], random32BitTimeSpoof[3]) byteArray = self.convertQuaternionComponent(convertedQuaternion.Q[0]) byteArray += (self.convertQuaternionComponent(convertedQuaternion.Q[1])) byteArray += (self.convertQuaternionComponent(convertedQuaternion.Q[2])) byteArray += (self.convertQuaternionComponent(convertedQuaternion.Q[3])) byteArray += ranTuple return byteArray def getPositionByteArray(self,currentRigidBodyState): convertedPosition = self.getConvertedPosition(currentRigidBodyState) random32BitTimeSpoof = (1).to_bytes(4,'big',signed=True) ranTuple = (random32BitTimeSpoof[0], random32BitTimeSpoof[1], random32BitTimeSpoof[2], random32BitTimeSpoof[3]) byteArray = self.convertPositionComponent(convertedPosition.X) byteArray += self.convertPositionComponent(convertedPosition.Y) byteArray += self.convertPositionComponent(convertedPosition.Z) byteArray += ranTuple return byteArray def getVelocityByteArray(self,currentRigidBodyState): convertedVelocity = self.getConvertedVelocity(currentRigidBodyState) random32BitTimeSpoof = (1).to_bytes(4,'big',signed=True) ranTuple = (random32BitTimeSpoof[0], random32BitTimeSpoof[1], random32BitTimeSpoof[2], random32BitTimeSpoof[3]) byteArray = self.convertVelocityComponent(convertedVelocity.X) byteArray += self.convertVelocityComponent(convertedVelocity.Y) byteArray += self.convertVelocityComponent(convertedVelocity.Z) byteArray += ranTuple return byteArray def getConvertedQuaternion(self, currentRigidBodyState): conversionQuaternion = Quaternion(axisOfRotation=Vector(1,0,0),angle=3.141592654) convertedQuaternion = currentRigidBodyState.orientation*conversionQuaternion return currentRigidBodyState.orientation def getConvertedPosition(self,currentRigidBodyState): convertedPosition = Vector(currentRigidBodyState.position.X,currentRigidBodyState.position.Y,-currentRigidBodyState.position.Z) return convertedPosition def getConvertedVelocity(self,currentRigidBodyState): convertedVelocity = Vector(currentRigidBodyState.velocity.X,currentRigidBodyState.velocity.Y,-currentRigidBodyState.velocity.Z) return convertedVelocity def writePacket(self,myPacket): myPacket.computeCheckSum() self.imuSpoof.write('s'.encode('UTF-8')) self.imuSpoof.write('n'.encode('UTF-8')) self.imuSpoof.write('p'.encode('UTF-8')) self.imuSpoof.write(bytes([myPacket.PTByte])) self.imuSpoof.write(bytes([myPacket.address])) self.imuSpoof.write(bytes(myPacket.data)) self.imuSpoof.write(bytes([myPacket.checkSumUpperByte])) self.imuSpoof.write(bytes([myPacket.checkSumLowerByte])) def __del__(self): self.teensyComs.close() self.imuSpoof.close()
Methods
def convertPositionComponent(self, positionComponent)
-
Expand source code
def convertPositionComponent(self, positionComponent): ba = bytearray(struct.pack("f", positionComponent)) imuMSB = ba[3] imu2MSB = ba[2] imu2LSB = ba[1] imuLSB = ba[0] return imuMSB, imu2MSB, imu2LSB, imuLSB
def convertQuaternionComponent(self, quaternionComponent)
-
Expand source code
def convertQuaternionComponent(self, quaternionComponent): imuDecimalRepresentation = (int)(quaternionComponent*29789.09091) imuBinaryRepresentation = imuDecimalRepresentation.to_bytes(2,'big',signed=True) imuUpperByte = imuBinaryRepresentation[0] imuLowerByte = imuBinaryRepresentation[1] return imuUpperByte, imuLowerByte
def convertVelocityComponent(self, velocityComponent)
-
Expand source code
def convertVelocityComponent(self, velocityComponent): ba = bytearray(struct.pack("f", velocityComponent)) imuMSB = ba[3] imu2MSB = ba[2] imu2LSB = ba[1] imuLSB = ba[0] return imuMSB, imu2MSB, imu2LSB, imuLSB
def getCanardAngles(self)
-
Expand source code
def getCanardAngles(self): #for i in range(4): #print(self.canardAngles[i]) return self.canardAngles
def getConvertedPosition(self, currentRigidBodyState)
-
Expand source code
def getConvertedPosition(self,currentRigidBodyState): convertedPosition = Vector(currentRigidBodyState.position.X,currentRigidBodyState.position.Y,-currentRigidBodyState.position.Z) return convertedPosition
def getConvertedQuaternion(self, currentRigidBodyState)
-
Expand source code
def getConvertedQuaternion(self, currentRigidBodyState): conversionQuaternion = Quaternion(axisOfRotation=Vector(1,0,0),angle=3.141592654) convertedQuaternion = currentRigidBodyState.orientation*conversionQuaternion return currentRigidBodyState.orientation
def getConvertedVelocity(self, currentRigidBodyState)
-
Expand source code
def getConvertedVelocity(self,currentRigidBodyState): convertedVelocity = Vector(currentRigidBodyState.velocity.X,currentRigidBodyState.velocity.Y,-currentRigidBodyState.velocity.Z) return convertedVelocity
def getPositionByteArray(self, currentRigidBodyState)
-
Expand source code
def getPositionByteArray(self,currentRigidBodyState): convertedPosition = self.getConvertedPosition(currentRigidBodyState) random32BitTimeSpoof = (1).to_bytes(4,'big',signed=True) ranTuple = (random32BitTimeSpoof[0], random32BitTimeSpoof[1], random32BitTimeSpoof[2], random32BitTimeSpoof[3]) byteArray = self.convertPositionComponent(convertedPosition.X) byteArray += self.convertPositionComponent(convertedPosition.Y) byteArray += self.convertPositionComponent(convertedPosition.Z) byteArray += ranTuple return byteArray
def getQuaternionByteArray(self, currentRigidBodyState)
-
Expand source code
def getQuaternionByteArray(self,currentRigidBodyState): convertedQuaternion = self.getConvertedQuaternion(currentRigidBodyState) random32BitTimeSpoof = (1).to_bytes(4,'big',signed=True) ranTuple = (random32BitTimeSpoof[0], random32BitTimeSpoof[1], random32BitTimeSpoof[2], random32BitTimeSpoof[3]) byteArray = self.convertQuaternionComponent(convertedQuaternion.Q[0]) byteArray += (self.convertQuaternionComponent(convertedQuaternion.Q[1])) byteArray += (self.convertQuaternionComponent(convertedQuaternion.Q[2])) byteArray += (self.convertQuaternionComponent(convertedQuaternion.Q[3])) byteArray += ranTuple return byteArray
def getVelocityByteArray(self, currentRigidBodyState)
-
Expand source code
def getVelocityByteArray(self,currentRigidBodyState): convertedVelocity = self.getConvertedVelocity(currentRigidBodyState) random32BitTimeSpoof = (1).to_bytes(4,'big',signed=True) ranTuple = (random32BitTimeSpoof[0], random32BitTimeSpoof[1], random32BitTimeSpoof[2], random32BitTimeSpoof[3]) byteArray = self.convertVelocityComponent(convertedVelocity.X) byteArray += self.convertVelocityComponent(convertedVelocity.Y) byteArray += self.convertVelocityComponent(convertedVelocity.Z) byteArray += ranTuple return byteArray
def performHIL(self, currentRigidBodyState, simTime)
-
Expand source code
def performHIL(self, currentRigidBodyState, simTime): #Send IMU data if the update times have passed self.sendIMUData(currentRigidBodyState, simTime) #This is the delay waiting for the real world to catch up with the simulation while simTime > (time.time() - self.startTime): #print("Stalling") #print('{0:.8f}'.format(time.time()-self.lastUpdateTime)) pass #Send IMU data if the update times have passed self.requestCanardAngles() self.lastUpdateTime = time.time()
def requestCanardAngles(self)
-
Expand source code
def requestCanardAngles(self): self.teensyComs.write('q'.encode('UTF-8')) time.sleep(0.001) for i in range(4): a = self.teensyComs.readline() if a == b'': #If there was a timeout self.canardAngles[i] = self.canardAngles[i] #If there was a timeout just use the alst iterations angle print("Serial Timeout") else: self.canardAngles[i] = float(a) return self.canardAngles
def sendIMUData(self, currentRigidBodyState, simTime)
-
Expand source code
def sendIMUData(self, currentRigidBodyState, simTime): #print("Hello1") #print(simTime) #print(self.startTime) #print(self.lastQuaternionUpdateTime) #print(self.quatUpdatePeriod) #If the simulation time is greater than the quat update period if((simTime + self.startTime - self.lastQuaternionUpdateTime) > self.quatUpdatePeriod): #print("Hello2") #send quaternion data quaternionByteArray = self.getQuaternionByteArray(currentRigidBodyState) myPacket = packet(109) myPacket.writeData(quaternionByteArray) myPacket.createPTByte(True, True, 3) self.writePacket(myPacket) #print("Sent") self.lastQuaternionUpdateTime = simTime+self.startTime if((simTime + self.startTime - self.lastPositionUpdateTime) > self.posUpdatePeriod): #send position data positionByteArray = self.getPositionByteArray(currentRigidBodyState) myPacket = packet(117) myPacket.writeData(positionByteArray) myPacket.createPTByte(True, True, 4) self.writePacket(myPacket) self.lastPositionUpdateTime = simTime+self.startTime if((simTime+self.startTime - self.lastVelocityUpdateTime) > self.velUpdatePeriod): #send velocity data velocityByteArray = self.getVelocityByteArray(currentRigidBodyState) myPacket = packet(121) myPacket.writeData(velocityByteArray) myPacket.createPTByte(True, True, 4) self.writePacket(myPacket) self.lastVelocityUpdateTime = simTime+self.startTime return
def sendTeensyCommands(self, command)
-
Expand source code
def sendTeensyCommands(self,command): self.teensyComs.write(command)
def setupHIL(self, currentRigidBodyState)
-
Expand source code
def setupHIL(self,currentRigidBodyState): self.sendTeensyCommands('R'.encode('UTF-8')) self.sendIMUData(currentRigidBodyState,10000) time.sleep(2) self.sendTeensyCommands('L'.encode('UTF-8')) self.startTime = time.time() self.lastUpdateTime = self.startTime #Seconds self.lastQuaternionUpdateTime = self.startTime self.lastPositionUpdateTime = self.startTime self.lastVelocityUpdateTime = self.startTime return
def writePacket(self, myPacket)
-
Expand source code
def writePacket(self,myPacket): myPacket.computeCheckSum() self.imuSpoof.write('s'.encode('UTF-8')) self.imuSpoof.write('n'.encode('UTF-8')) self.imuSpoof.write('p'.encode('UTF-8')) self.imuSpoof.write(bytes([myPacket.PTByte])) self.imuSpoof.write(bytes([myPacket.address])) self.imuSpoof.write(bytes(myPacket.data)) self.imuSpoof.write(bytes([myPacket.checkSumUpperByte])) self.imuSpoof.write(bytes([myPacket.checkSumLowerByte]))
class packet (address)
-
Expand source code
class packet: def __init__(self, address): self.PTByte = 0 self.address = address self.data = [] self.checkSumUpperByte = 0 self.checkSumLowerByte = 0 def writeData(self, writtenData): for i in range(len(writtenData)): self.data.append(writtenData[i]) def computeCheckSum(self): total = ord('s') + ord('n') + ord('p') + self.PTByte + self.address for i in range(len(self.data)): total += self.data[i] decimalRepresentation = (int)(total/1.0) binaryRepresentation = decimalRepresentation.to_bytes(2,'big',signed=False) self.checkSumUpperByte = binaryRepresentation[0] self.checkSumLowerByte = binaryRepresentation[1] def createPTByte(self, hasData, isBatch, numRegisters): PTByteValue = 0 if(hasData == True): PTByteValue += 128 if(isBatch == True): PTByteValue += 64 PTByteValue += numRegisters*4 self.PTByte = PTByteValue
Methods
def computeCheckSum(self)
-
Expand source code
def computeCheckSum(self): total = ord('s') + ord('n') + ord('p') + self.PTByte + self.address for i in range(len(self.data)): total += self.data[i] decimalRepresentation = (int)(total/1.0) binaryRepresentation = decimalRepresentation.to_bytes(2,'big',signed=False) self.checkSumUpperByte = binaryRepresentation[0] self.checkSumLowerByte = binaryRepresentation[1]
def createPTByte(self, hasData, isBatch, numRegisters)
-
Expand source code
def createPTByte(self, hasData, isBatch, numRegisters): PTByteValue = 0 if(hasData == True): PTByteValue += 128 if(isBatch == True): PTByteValue += 64 PTByteValue += numRegisters*4 self.PTByte = PTByteValue
def writeData(self, writtenData)
-
Expand source code
def writeData(self, writtenData): for i in range(len(writtenData)): self.data.append(writtenData[i])