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!')