def create_execfw_cmd(self):
"""
Creates a command to excute firmware
"""
cmd = 'Execute'
print('CMD >>> %s.' % cmd)
code = self.get_cmd_code(cmd)
EXE_FW_HEADER_LEN = 1
count = EXE_FW_HEADER_LEN
self.init_tx_data()
index = list(self.message_id.values()).index('Control')
msg_id = list(self.message_id.keys())[index]
bitstruct.pack_into_dict(
self.cmd_word_fmt, self.cmd_keys, self.tx_data, 0, {
'cmd': 1,
'rsvd1': 0,
'rsp': 0,
'msg_id': msg_id,
'rsvd2': 0,
'cmd_rsp': code
})
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, count)
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, 0xbe000000)
self.tx_index += 4
return self.tx_data
def create_memread_cmd(self, tuple):
"""
Creates a memory read command with memory addresses
"""
cmd = 'Memory Read'
print('CMD >>> %s.' % cmd)
code = self.get_cmd_code(cmd)
self.init_tx_data()
index = list(self.message_id.values()).index('Control')
msg_id = list(self.message_id.keys())[index]
bitstruct.pack_into_dict(
self.cmd_word_fmt, self.cmd_keys, self.tx_data, 0, {
'cmd': 1,
'rsvd1': 0,
'rsp': 0,
'msg_id': msg_id,
'rsvd2': 0,
'cmd_rsp': code
})
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, len(tuple))
self.tx_index += 4
for index in range(len(tuple)):
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8,
tuple[index])
self.tx_index += 4
return self.tx_data
def create_execfw_cmd(self):
"""
Creates a command to excute firmware
"""
cmd = 'Execute'
print('CMD >>> %s.' % cmd)
code = self.get_cmd_code(cmd)
EXE_FW_HEADER_LEN = 1
count = EXE_FW_HEADER_LEN
self.init_tx_data()
index = list(self.message_id.values()).index('Control')
msg_id = list(self.message_id.keys())[index]
bitstruct.pack_into_dict(self.cmd_word_fmt, self.cmd_keys,
self.tx_data, 0, {'cmd': 1, 'rsvd1': 0, 'rsp': 0,
'msg_id': msg_id, 'rsvd2': 0, 'cmd_rsp': code})
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, count)
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, 0xbe000000)
self.tx_index += 4
return self.tx_data
def send_cmd(self, throttle, brake, steering, stdscr):
data = bytearray(b'\x00' * 3)
bitstruct.pack_into('u8u8u8', data, 0, throttle, brake, steering)
self.cart_ser.write(data)
stdscr.addstr(7, 0, 'Throttle val: ' + str(throttle) + ' ')
stdscr.addstr(8, 0, 'Brake val: ' + str(brake) + ' ')
stdscr.addstr(9, 0, 'Steering val: ' + str(steering) + ' ')
def create_loadfw_cmd(self, size, sha):
"""
Creates a command to load firmware with associated paramters
"""
cmd = 'Load Firmware'
print('CMD >>> %s.' % cmd)
code = self.get_cmd_code(cmd)
FW_NO_EXEC_FLAG = (1 << 26)
SEL_HP_CLK = (1 << 21)
LD_FW_HEADER_LEN = 3
count_flags = FW_NO_EXEC_FLAG | SEL_HP_CLK
count_flags |= (LD_FW_HEADER_LEN + int(len(sha) / 4))
self.init_tx_data()
index = list(self.message_id.values()).index('Control')
msg_id = list(self.message_id.keys())[index]
bitstruct.pack_into_dict(self.cmd_word_fmt, self.cmd_keys,
self.tx_data, 0, {'cmd': 1, 'rsvd1': 0, 'rsp': 0,
'msg_id': msg_id, 'rsvd2': 0, 'cmd_rsp': code})
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, count_flags)
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, 0xbe000000)
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, 0)
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, size)
self.tx_index += 4
self.endian_swap(self.tx_data, self.tx_index, sha)
self.tx_index += len(sha)
return self.tx_data
def apply_authenticator(raw_payload: Union[bytes, bytearray],
dbmsg: Message,
authenticator_fn: Callable[[Message,
bytearray,
int],
bytearray],
freshness_value: int) \
-> bytearray:
"""Given a byte-like object that contains the encoded signals to be
send, compute the full message which ought to be send.
This is basically the concatination of the raw payload, the
truncated freshness value and the truncated authenticator for the
message.
"""
if dbmsg.autosar is None or dbmsg.autosar.secoc is None:
raise SecOCError(f'Message "{dbmsg.name}" is not secured')
secoc_props = dbmsg.autosar.secoc
result = bytearray(raw_payload)
payload_len = secoc_props.payload_length
# get the last N bits of the freshness value. we assume that the
# full freshness value is at most 64 bits.
n_fresh = secoc_props.freshness_bit_length
n_fresh_tx = secoc_props.freshness_tx_bit_length
mask = 0xffffffffffffffff >> (64 - n_fresh_tx)
freshness_header_value = freshness_value & mask
# compute the authentificator value
auth_data = bitstruct.pack(
f'u16' # data ID
f'r{payload_len*8}' # payload to be secured
f'u{n_fresh}', # freshness value
secoc_props.data_id,
raw_payload[:payload_len],
freshness_value)
# compute authenticator and pack it into the result array
auth_value = authenticator_fn(dbmsg, auth_data, freshness_value)
bitstruct.pack_into(f'u{n_fresh_tx}r{secoc_props.auth_tx_bit_length}',
result, payload_len * 8, freshness_header_value,
auth_value)
return result
def send_to_motors(self):
spd = self.cmd_msg.vel
wheel_ang = self.cmd_msg.angle
if wheel_ang > 28:
wheel_ang = 28
if wheel_ang < -42:
wheel_ang = -42
angle = wheel_ang * -12
cur_spd = self.cmd_msg.vel_curr
if spd == 0:
brake = 255
else:
brake = 0
angle = (int)(angle)
data = bytearray(b'\x00' * 6)
bitstruct.pack_into('u8u8u8u8u16', data, 0, 42, 21, spd, brake, angle)
self.speed_ser.write(data)
""" print "speed: " + str(spd) + " angle: " + str(angle) + " cur_spd: " + str(cur_spd)
def create_memread_cmd(self, tuple):
"""
Creates a memory read command with memory addresses
"""
cmd = 'Memory Read'
print('CMD >>> %s.' % cmd)
code = self.get_cmd_code(cmd)
self.init_tx_data()
index = list(self.message_id.values()).index('Control')
msg_id = list(self.message_id.keys())[index]
bitstruct.pack_into_dict(self.cmd_word_fmt, self.cmd_keys,
self.tx_data, 0, {'cmd': 1, 'rsvd1': 0, 'rsp': 0,
'msg_id': msg_id, 'rsvd2': 0, 'cmd_rsp': code})
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8,
len(tuple))
self.tx_index += 4
for index in range(len(tuple)):
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8,
tuple[index])
self.tx_index += 4
return self.tx_data
def create_loadfw_cmd(self, size, sha):
"""
Creates a command to load firmware with associated paramters
"""
cmd = 'Load Firmware'
print('CMD >>> %s.' % cmd)
code = self.get_cmd_code(cmd)
FW_NO_EXEC_FLAG = (1 << 26)
SEL_HP_CLK = (1 << 21)
LD_FW_HEADER_LEN = 3
count_flags = FW_NO_EXEC_FLAG | SEL_HP_CLK
count_flags |= (LD_FW_HEADER_LEN + int(len(sha) / 4))
self.init_tx_data()
index = list(self.message_id.values()).index('Control')
msg_id = list(self.message_id.keys())[index]
bitstruct.pack_into_dict(
self.cmd_word_fmt, self.cmd_keys, self.tx_data, 0, {
'cmd': 1,
'rsvd1': 0,
'rsp': 0,
'msg_id': msg_id,
'rsvd2': 0,
'cmd_rsp': code
})
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8,
count_flags)
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, 0xbe000000)
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, 0)
self.tx_index += 4
bitstruct.pack_into('u32', self.tx_data, self.tx_index * 8, size)
self.tx_index += 4
self.endian_swap(self.tx_data, self.tx_index, sha)
self.tx_index += len(sha)
return self.tx_data
def send_to_motors(self):
#The first time we get a new target speed and angle we must convert it
if self.new_vel:
self.new_vel = False
self.cmd_msg.vel *= 50
self.cmd_msg.vel_curr *= 50
if self.cmd_msg.vel > 254:
self.cmd_msg.vel = 254
if self.cmd_msg.vel < -254:
self.cmd_msg.vel = -254
if self.cmd_msg.vel_curr > 254:
self.cmd_msg.vel_curr = 254
target_speed = int(self.cmd_msg.vel) #float64
current_speed = int(self.cmd_msg.vel_curr) #float64
#adjust the target_angle range from (-45 <-> 45) to (0 <-> 100)
# rospy.loginfo("Angle before adjustment: " + str(self.cmd_msg.angle))
if (self.cmd_msg.angle < -45):
self.cmd_msg.angle = -45
if (self.cmd_msg.angle > 45):
self.cmd_msg.angle = 45
target_angle = 100 - int(((self.cmd_msg.angle + 45) / 90) * 100)
# rospy.loginfo("Angle after range adjustment: " + str(target_angle))
#adjust the target angle additionally using a realtime adjustable testing value
data = (target_speed, current_speed, target_angle)
# rospy.loginfo("Before readied data" + str(data))
data = bytearray(b'\x00' * 5)
#if debug printing is requested print speed and angle info
if self.debug:
self.delay_print -= 1
if self.delay_print <= 0:
self.delay_print = 5
rospy.loginfo("Endpoint Angle: " + str(target_angle))
rospy.loginfo("Endpoint Speed: " + str(target_speed))
# Check for request to stop vehicle
if self.stop:
target_speed = (int(-self.brake))
if (self.drove_since_braking == True):
self.braking_duration = 3
self.drove_since_braking = False
if (self.braking_duration > 0):
self.brake = 255
self.braking_duration -= 1
else:
self.brake = 0
else:
self.drove_since_braking = True
pass
#reset the brake force slowly incase a new stop message arises immediatly
#if the target_speed is negative it actually represents desired braking force
if target_speed < 0:
bitstruct.pack_into('u8u8u8u8u8', data, 0, 42, 21, 0,
abs(target_speed), target_angle)
else:
bitstruct.pack_into('u8u8u8u8u8', data, 0, 42, 21,
abs(target_speed), 0, target_angle)
# rospy.loginfo("Readied data: " + str(data) + "\n")
self.speed_ser.write(data)
#print self.cur_vel
elif keyval == ord('x'):
self.cur_vel = 0
elif keyval == ord('z'):
self.brake(self.cur_vel / 255.0 * 3, stdscr)
self.cur_vel = 0
self.send_cmd(self.cur_vel, 0, 0, stdscr)
self.prev_key = keyval
time.sleep(1/10.)
def brake(self, delay, stdscr):
rate = 10.
steps = delay * rate
for brake in np.linspace(0, 255, steps, endpoint=True):
self.send_cmd(0, int(brake), 0, stdscr)
time.sleep(1./rate)
stdscr.getch()
def send_cmd(self, throttle, brake, steering, stdscr):
angle = 0
data = bytearray(b'\x00' * 6)
bitstruct.pack_into('u8u8u8u8u16', data, 0, 42, 21,
throttle, brake, steering)
self.speed_ser.write(data)
stdscr.addstr(7,0,str(throttle) + " ")
stdscr.addstr(8,0,str(brake) + " ")
if __name__ == "__main__":
teleop()
def gprMax_to_dzt(filename, rx, rxcomponent, centerFreq, distTx_Rx,
trace_step):
import h5py as h5
import os
import sys
import struct
import bitstruct
import datetime
from bitstring import Bits
from scipy import signal
# ------------------------------- Information specified by the user ---------------------------------------
# Specify gprMax file path name
file_path_name = filename
# Specify center frequency (MHz)
center_freq = centerFreq
# Specify Tx-Rx distance
distance = distTx_Rx
# Trace step
trace_step = trace_step
# Choose E-field component
comp = rxcomponent
# ---------------------------------------------------------------------------------------------------------
# Read gprMax data
bscan, _, _ = gprMax_Bscan(filename + '.out', rx, rxcomponent)
data = np.array(bscan)
# Read time step
#file = h5.File(filename[0:-4]+'1.out', 'r')
file = h5.File(filename + '1.out', 'r')
time_step = file.attrs['dt']
file.close()
data = (data * 32767) / np.max(np.abs(data))
data[data > 32767] = 32767
data[data < -32768] = -32768
data = np.round(data)
# Number of samples and traces
[noSamples, noTraces] = np.shape(data)
# Convert time step to ns
time_step = time_step * 10**9
# Sampling frequency (MHz)
sampling_freq = (1 / time_step) * 10**3
# Time window (ns)
time_window = time_step * noSamples
# DZT file name
fileName = filename
# Resample data to 1024 samples
data = signal.resample(data, 1024)
time_step = time_window / np.shape(data)[0]
sampling_freq = (1 / time_step) * 10**3
# ------------------------------------------------ DZT file header -----------------------------------------------------
tag = 255 # 0x00ff if header, 0xfnff for old file Header
dataOffset = 1024 # Constant 1024
noSamples = np.shape(data)[0] # Number of samples
bits = 16 # Bits per data word (8 or 16)
binaryOffset = 32768 # Binary offset (8 bit -> 128, 16 bit -> 32768)
sps = 0 # Scans per second
spm = 1 / trace_step # Scans per metre
mpm = 0 # Meters per mark
position = 0 # Position (ns)
time_window = time_window # Time window (ns)
noScans = 0 # Number of passes for 2D files
dateTime = datetime.datetime.now() # Current datetime
# Date and time created
createdSec = dateTime.second
if createdSec > 29: createdSec = 29
createdMin = dateTime.minute
createdHour = dateTime.hour
createdDay = dateTime.day
createdMonth = dateTime.month
createdYear = dateTime.year - 1980
# Date and time modified
modifiedSec = dateTime.second
if modifiedSec > 29: modifiedSec = 29
modifiedMin = dateTime.minute
modifiedHour = dateTime.hour
modifiedDay = dateTime.day
modifiedMonth = dateTime.month
modifiedYear = dateTime.year - 1980
offsetRG = 0 # Offset to range gain function
sizeRG = 0 # Size of range gain function
offsetText = 0 # Offset to text
sizeText = 0 # Size of text
offsetPH = 0 # Offset to processing history
sizePH = 0 # Size of processing history
noChannels = 1 # Number of channels
epsr = 5 # Average dielectric constant
topPosition = 0 # Top position (m)
vel = (299792458 / np.sqrt(epsr)) * 10**-9
range0 = vel * (time_window / 2) # Range (meters)
xStart = 0 # X start coordinate
xFinish = noTraces * trace_step - trace_step # X finish coordinate
servoLevel = 0 # Gain servo level
reserved = 0 # Reserved
antConfig = 0 # Antenna Configuration
setupConfig = 0 # Setup Configuration
spp = 0 # Scans per pass
noLine = 0 # Line number
yStart = 0 # Y start coordinate
yFinish = 0 # Y finish coordinate
lineOrder = 0
dataType = 2 # Data type
antennaName = 'antName'
if len(antennaName) > 14:
antennaName = antennaName[0:14]
elif len(antennaName) < 14:
antennaName = antennaName.ljust(14)
channelMask = 0 # Channel mask
fName = fileName # File name
if len(fName) > 12:
fName = fName[0:12]
elif len(fName) < 12:
fName = fName.ljust(12)
checkSum = 0 # Check sum for header
# -------------------------------------------------------------------------------------------------------------------
# ----------------------------------------- Convert to bytes and write to file --------------------------------------
# Open file to write
with open(fileName + '.dzt', 'wb') as fid:
# Write header
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, tag)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, dataOffset)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, noSamples)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, bits)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('s16<', dataStruct, 0, binaryOffset)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, sps)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, spm)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, mpm)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, position)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, time_window)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, noScans)
fid.write(dataStruct)
sec = Bits(uint=createdSec, length=5)
min = Bits(uint=createdMin, length=6)
hour = Bits(uint=createdHour, length=5)
day = Bits(uint=createdDay, length=5)
month = Bits(uint=createdMonth, length=4)
year = Bits(uint=createdYear, length=7)
b = Bits().join([year, month, day, hour, min, sec])
createDate = b.tobytes()
fid.write(bitstruct.pack('>r32<', createDate))
sec = Bits(uint=modifiedSec, length=5)
min = Bits(uint=modifiedMin, length=6)
hour = Bits(uint=modifiedHour, length=5)
day = Bits(uint=modifiedDay, length=5)
month = Bits(uint=modifiedMonth, length=4)
year = Bits(uint=modifiedYear, length=7)
b = Bits().join([year, month, day, hour, min, sec])
modifiedDate = b.tobytes()
fid.write(bitstruct.pack('>r32<', modifiedDate))
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, offsetRG)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, sizeRG)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, offsetText)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, sizeText)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, offsetPH)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, sizePH)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, noChannels)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, epsr)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, topPosition)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, range0)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, xStart)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, xFinish)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, servoLevel)
fid.write(dataStruct)
dataStruct = bytearray(3)
bitstruct.pack_into('r24<', dataStruct, 0, reserved)
fid.write(dataStruct)
dataStruct = bytearray(1)
bitstruct.pack_into('u8<', dataStruct, 0, antConfig)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, setupConfig)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, spp)
fid.write(dataStruct)
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, noLine)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, yStart)
fid.write(dataStruct)
dataStruct = bytearray(4)
bitstruct.pack_into('f32<', dataStruct, 0, yFinish)
fid.write(dataStruct)
dataStruct = bytearray(1)
bitstruct.pack_into('u8<', dataStruct, 0, lineOrder)
fid.write(dataStruct)
dataStruct = bytearray(1)
bitstruct.pack_into('r8<', dataStruct, 0, dataType)
fid.write(dataStruct)
fid.write(bitstruct.pack('t14<', antennaName))
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, channelMask)
fid.write(dataStruct)
fid.write(bitstruct.pack('t12<', fName))
dataStruct = bytearray(2)
bitstruct.pack_into('u16<', dataStruct, 0, checkSum)
fid.write(dataStruct)
# Move to 1024 to write data
fid.seek(dataOffset)
data = data + binaryOffset
data = np.array(data, dtype='<H')
fid.write(data.T.astype('<H').tobytes())
# Close file
fid.close()
print('Dzt file has been written!')
