def run(self):
GPGGA_received = False
GPRMC_received = False
gps_msg = GPS()
while not rospy.is_shutdown():
try:
line = self.gps2.readline()
if line:
line = line.rstrip()
data_arr = line.split(',')
if data_arr[0] == "$GPGGA":
self.log_file.write(line + '\n')
GPGGA_received = True
#print data_arr[1]
gps_msg.time = int(float(data_arr[1]))
#print "time:", int(float(data_arr[1]))
lat, lng = gpsfunctions.nmea2decimal(float(data_arr[2]), data_arr[3], float(data_arr[4]), data_arr[5])
gps_msg.latitude = lat
gps_msg.longitude = lng
#print "long:", float(data_arr[4])
gps_msg.fix = int(data_arr[6])
#print "fix:", int(data_arr[6])
gps_msg.sats = int(data_arr[7])
#print "sats:", int(data_arr[7])
gps_msg.HDOP = float(data_arr[8])
#print "HDOP:", float(data_arr[8])
gps_msg.altitude = float(data_arr[9])
#print "AMSL:", float(data_arr[9])
gps_msg.height = float(data_arr[11])
elif data_arr[0] == "$GPRMC":
self.log_file.write(line + '\n')
GPRMC_received = True
gps_msg.SOG = float(data_arr[7])
gps_msg.track_angle = float(data_arr[8])
gps_msg.date = int(data_arr[9])
if GPGGA_received and GPRMC_received:
# send packet
print "GPS_MSG:", gps_msg
self.pub.publish(gps_msg)
gps_msg = GPS()
GPGGA_received = False
GPGGA_received = False
except:
pass
self.log_file.close()
def run(self):
GPGGA_received = False
GPRMC_received = False
gps_msg = GPS()
while not rospy.is_shutdown():
try:
line = self.gps2.readline()
if line:
line = line.rstrip()
data_arr = line.split(',')
if data_arr[0] == "$GPGGA":
self.log_file.write(line + '\n')
GPGGA_received = True
#print data_arr[1]
gps_msg.time = int(float(data_arr[1]))
#print "time:", int(float(data_arr[1]))
lat, lng = gpsfunctions.nmea2decimal(float(data_arr[2]), data_arr[3], float(data_arr[4]), data_arr[5])
gps_msg.latitude = lat
gps_msg.longitude = lng
#print "long:", float(data_arr[4])
gps_msg.fix = int(data_arr[6])
#print "fix:", int(data_arr[6])
gps_msg.sats = int(data_arr[7])
#print "sats:", int(data_arr[7])
gps_msg.HDOP = float(data_arr[8])
#print "HDOP:", float(data_arr[8])
gps_msg.altitude = float(data_arr[9])
#print "AMSL:", float(data_arr[9])
gps_msg.height = float(data_arr[11])
elif data_arr[0] == "$GPRMC":
self.log_file.write(line + '\n')
GPRMC_received = True
gps_msg.SOG = float(data_arr[7])
gps_msg.track_angle = float(data_arr[8])
gps_msg.date = int(data_arr[9])
if GPGGA_received and GPRMC_received:
# send packet
print "GPS_MSG:", gps_msg
self.pub.publish(gps_msg)
gps_msg = GPS()
GPGGA_received = False
GPGGA_received = False
except:
pass
self.log_file.close()
def parse(self, line):
self.mb100log.write(line + ',' + str(time.time()) + "\r\n")
line = line.split(',')
# Make sure that there is no empty strings, which will make
# the float() and int() conversions fail, which in turn will
# make the parsing except silently, because of the try
# catching in the self.run().
for k in range(len(line)):
if line[k] == '':
line[k] = 0
rospy.logwarn(
"Empty element sin the $PASHR,POS message, do not sail.")
# Note that is might not be the best idea to set it to zero
# only. We might need to handle this downstream, such that we
# don't change them maybe.
if line[0] == "$PASHR" and line[1] == "POS":
mb100 = {}
mb100['posmode'] = int(line[2])
mb100['sats'] = int(line[3])
mb100['utctime'] = float(line[4])
[latdec, londec] = (geo.nmea2decimal(float(line[5]), line[6],
float(line[7]), line[8]))
mb100['lat'] = latdec * pi / 180.0
mb100['lon'] = londec * pi / 180.0
mb100['alt'] = float(line[9])
#TODO Fix this, when the the float() gets and empty stirng, it will fail the try in the self.run()
#print(line[9])
#print("woo")
#print(type(line[10]))
#mb100['ageofdiff'] = float(line[10])
#print(line[10])
mb100['cog'] = float(line[11]) * pi / 180.0
mb100['sog'] = float(line[12]) * 0.514444444
mb100['verticalvel'] = float(line[13])
mb100['pdop'] = float(line[14])
mb100['hdop'] = float(line[15])
mb100['vdop'] = float(line[16])
mb100['tdop'] = float(line[17])
mb100['crc'] = line[18]
return mb100
elif line[
0] == "RomBOOT": # TODO the string the gps sends when booting
rospy.logwarn(
"It seems like the MB100 has restarted. Is the powersupply ok?"
)
return None
else:
return None
def parse(self,line):
self.mb100log.write(line + ',' + str(time.time()) + "\r\n")
line = line.split(',')
# Make sure that there is no empty strings, which will make
# the float() and int() conversions fail, which in turn will
# make the parsing except silently, because of the try
# catching in the self.run().
for k in range(len(line)):
if line[k] == '':
line[k] = 0
rospy.logwarn("Empty element sin the $PASHR,POS message, do not sail.")
# Note that is might not be the best idea to set it to zero
# only. We might need to handle this downstream, such that we
# don't change them maybe.
if line[0] == "$PASHR" and line[1] == "POS":
mb100 = {}
mb100['posmode'] = int(line[2])
mb100['sats'] = int(line[3])
mb100['utctime'] = float(line[4])
[latdec, londec] = (geo.nmea2decimal(float(line[5]),line[6],float(line[7]),line[8]))
mb100['lat'] = latdec*pi/180.0
mb100['lon'] = londec*pi/180.0
mb100['alt'] = float(line[9])
#TODO Fix this, when the the float() gets and empty stirng, it will fail the try in the self.run()
#print(line[9])
#print("woo")
#print(type(line[10]))
#mb100['ageofdiff'] = float(line[10])
#print(line[10])
mb100['cog'] = float(line[11])*pi/180.0
mb100['sog'] = float(line[12])*0.514444444
mb100['verticalvel'] = float(line[13])
mb100['pdop'] = float(line[14])
mb100['hdop'] = float(line[15])
mb100['vdop'] = float(line[16])
mb100['tdop'] = float(line[17])
mb100['crc'] = line[18]
return mb100
elif line[0] == "RomBOOT": # TODO the string the gps sends when booting
rospy.logwarn("It seems like the MB100 has restarted. Is the powersupply ok?")
return None
else:
return None
def parse(self, packet):
# print packet
try:
if ord(packet["DevID"]) == 0: # LLI data
if ord(packet["MsgID"]) == 13: # Battery monitor sample
print ("BANK1:\t")
self.bank1[0] = (ord(packet["Data"][0]) << 8 | ord(packet["Data"][1])) * (0.25 * 3.0 / 1.43)
self.bank1[1] = (ord(packet["Data"][2]) << 8 | ord(packet["Data"][3])) * (0.25 * 3.0 / 1.43)
self.bank1[2] = (ord(packet["Data"][4]) << 8 | ord(packet["Data"][5])) * (0.25 * 3.0 / 1.43)
self.bank1[3] = (ord(packet["Data"][6]) << 8 | ord(packet["Data"][7])) * (0.25 * 3.0 / 1.43)
print (self.bank1)
print ("BANK2:\t")
self.bank2[0] = (ord(packet["Data"][8]) << 8 | ord(packet["Data"][9])) * (0.25 * 3.0 / 1.43)
self.bank2[1] = (ord(packet["Data"][10]) << 8 | ord(packet["Data"][11])) * (0.25 * 3.0 / 1.43)
self.bank2[2] = (ord(packet["Data"][12]) << 8 | ord(packet["Data"][13])) * (0.25 * 3.0 / 1.43)
self.bank2[3] = (ord(packet["Data"][14]) << 8 | ord(packet["Data"][15])) * (0.25 * 3.0 / 1.43)
print (self.bank2)
# Measured offsets
self.bank1[0] = self.bank1[0] + 97.33
self.bank1[1] = self.bank1[1] + 143.67
self.bank1[2] = self.bank1[2] + 138.67
self.bank1[3] = self.bank1[3] + 124.33
self.bank2[0] = self.bank2[0] + 111.33
self.bank2[1] = self.bank2[1] + 130.67
self.bank2[2] = self.bank2[2] + 143.67
self.bank2[3] = self.bank2[3] + 111.33
"""
if min(self.bank1) < 3600.0:
self.bank1 = [3600.0, 3600.0, 3600.0, 3600.0]
if min(self.bank2) < 3600.0:
self.bank2 = [3600.0, 3600.0, 3600.0, 3600.0]
if max(self.bank1) > 4200.0:
self.bank1 = [4200.0, 4200.0, 4200.0, 4200.0]
if max(self.bank2) > 4200.0:
self.bank2 = [4200.0, 4200.0, 4200.0, 4200.0]
"""
if not rospy.is_shutdown():
self.pub_bm.publish(self.bank1, self.bank2)
if ord(packet["DevID"]) == 20: # IMU data
if ord(packet["MsgID"]) == 14: # Burst read reduced
# self.accelburst = self.accelburst + 1
# print "IMU: " + str(self.accelburst)
# print "IMU"
meas = numpy.zeros((9, 2))
accelnr = 0
order = [7, 1, 4, 6, 6]
if self.mergedata:
pass
self.mergedata = False
# print "IMU!"
try:
# print "IMU"
"""The structure of the packet is
Zgyro
X acc
Y acc
X Mag
Y Mag
ADC
"""
# types = ['ADC','Ymag', 'Xmag', 'Yacc', 'Xacc', 'Zgyro']
# self.accellog.write("".join(packet['Data']) + "\r\n")
measurements = []
for i in range(len(packet["Data"])):
if (i & 1) == 1:
tempval = packet["Data"][i - 1 : i + 1]
tempval.reverse()
val = 0
try:
val = struct.unpack("h", "".join(tempval))
except:
pass
self.accellog.write(str(val[0]) + ", ")
self.fulllog.write(str(val[0]) + ", ")
measurements.append(val[0])
# print val[0]
# print measurements[5]
# print measurements
self.accellog.write(str(time.time()) + "\r\n")
self.fulllog.write(str(time.time()) + "\r\n")
if abs(measurements[5]) < 10: # Check that the grounded ADC doesn't return a high value
# Calculate heading from magnetometer:
heading = 0
if -measurements[3] > 0:
heading = (
(90 - atan(float(-measurements[4]) / float(-measurements[3])) * 180 / pi) * pi / 180
)
elif -measurements[3] < 0:
heading = (
(270 - atan(float(-measurements[4]) / float(-measurements[3])) * 180 / pi)
* pi
/ 180
)
else:
if -measurements[4] < 0:
heading = pi
else:
heading = 0
# heading = -(2*pi-heading-pi/2)
heading = -heading
# print chr(27) + "[2J"
# print "[" + str(measurements[4]) + ", " + str(measurements[3]) + "]\t Theta: " + str(heading) + "\t Time:" + str(time.time())
accx = -measurements[2] * self.accconst
accy = -measurements[1] * self.accconst
gyroz = -measurements[0] * self.gyroconst
self.state[2] = [accx, 1]
self.state[5] = [accy, 1]
self.state[6] = [heading, 1]
self.state[7] = [gyroz, 1]
# print self.state
for i in range(numpy.size(self.state, 0)):
for j in range(numpy.size(self.state, 1)):
self.measureddata[i, j] = self.state[i, j]
# measstate = self.state
# print chr(27) + "[2J"
# print self.measureddata
self.state[:, 1] = 0
except Exception as e:
print e
print "exception"
# print "IMU BURST!"
pass
elif ord(packet["MsgID"]) == 13: # Burst read
# print "Burst read"
measurements = []
for i in range(len(packet["Data"])):
if (i & 1) == 1:
tempval = packet["Data"][i - 1 : i + 1]
tempval.reverse()
val = 0
try:
val = struct.unpack("h", "".join(tempval))
except:
pass
self.accellog.write(str(val[0]) + ", ")
self.fulllog.write(str(val[0]) + ", ")
measurements.append(val[0])
self.accellog.write(str(time.time()) + "\r\n")
self.fulllog.write(str(time.time()) + "\r\n")
self.state[0] = [measurements[0], 1] # supply
self.state[1] = [measurements[1], 1] # xacc
self.state[2] = [measurements[2], 1] # yacc
self.state[3] = [measurements[3], 1] # zacc
self.state[4] = [measurements[4], 1] # xgyro
self.state[5] = [measurements[5], 1] # ygyro
self.state[6] = [measurements[6], 1] # zgyro
self.state[7] = [measurements[7], 1] # xmag
self.state[8] = [measurements[8], 1] # ymag
self.state[9] = [measurements[9], 1] # zmag
self.state[10] = [measurements[10], 1] # temp
self.state[11] = [measurements[11], 1] # adc
# Writing to our "output" object measureddata
for i in range(numpy.size(self.state, 0)):
for j in range(numpy.size(self.state, 1)):
self.measureddata[i, j] = self.state[i, j]
self.state = numpy.zeros((12, 2))
elif ord(packet["MsgID"]) == 15: # Reduced ADIS data, RF test
self.n_rec += 1
msgnr = ord(packet["Data"][0])
# print msgnr
self.plog.write(str(self.n_rec) + ", ")
self.plog.write(str(msgnr))
self.plog.write(", 0\n")
elif ord(packet["DevID"]) == 30: # GPS1 data
# print "GPS!"
# time.sleep(1)
if ord(packet["MsgID"]) == 6: # This is what the LII sends for the moment
# ['$GPGGA', '133635.000', '5700.8791', 'N', '00959.1707', 'E', '1', '7', '1.23', '42.0', 'M', '42.5', 'M', '', '*6E\r\n']
# ['$GPRMC', '133635.000', 'A', '5700.8791', 'N', '00959.1707', 'E', '0.20', '263.57', '040914', '', '', 'A*61\r\n']
# We expect to get both GPGGA and GPRMC at every GPS sample. This is in two messages.
# The GPRMC message is the plast one, so we publish our hybrid GPS message in that if.
# print str("".join(packet['Data']))
content = "".join(packet["Data"]).split(",")
print (content)
if content[0] == "$GPGGA":
# The GPGGA packet contain the following information:
# [0] Message type ($GPGGA)
# [1] Time
# [2] Latitude
# [3] N or S (N)
# [4] Longitude
# [5] E or W (E)
# [6] Fix quality (expect 1 = GPS fix single)
# [7] Number of satellites being tracked
# [8] HDOP
# [9] Altitude, above mean sea level
# [10] Unit for altitude M = meters
# [11] Height of geoid (mean sea level) above WGS84 ellipsoid
# [12] Unit of heigt M = meters
# [13] DGPS stuff, ignore
# [14] DGPS stuff, ignore
# [15] Checksum
print ("GGA")
if content[11] == "":
print ("GGA parsing, no fix")
else:
self.gpsmsg.fix = int(content[6])
self.gpsmsg.sats = int(content[7])
self.gpsmsg.HDOP = float(content[8])
self.gpsmsg.altitude = float(content[9])
self.gpsmsg.height = float(content[11])
if content[0] == "$GPRMC" and content[2] == "A":
# print ",".join("".join(packet['Data']).split(',')[1:8])
print ("RMC")
# The GPRMC packet contain the following information:
# [0] Message type ($GPRMC)
# [1] Timestamp
# [2] A for valid, V for invalid (only valid packets gets send)
# [3] Latitude
# [4] N or S (N)
# [5] Longitude
# [6] E or W (E)
# [7] Speed over ground
# [8] Track angle
# [9] Date
# [10] Magnetic variation value [not existant on this cheap GPS]
# [11] Magnetic variation direction [not existant on this cheap GPS]
if content[2] == "A":
print ("Wee, we have a valid $GPRMC fix")
self.gpslog.write(",".join(content) + ", " + str(time.time()) + "\r\n")
self.fulllog.write(",".join(content) + ", " + str(time.time()) + "\r\n")
# print content
speed = float(content[7]) * 0.514444444 # * 0 + 100
# print str(speed) + " m/s"
# Caculate decimal degrees
[latdec, londec] = gpsfunctions.nmea2decimal(
float(content[3]), content[4], float(content[5]), content[6]
)
print (latdec, londec)
latdec = latdec * pi / 180
londec = londec * pi / 180
# Old code for rotating the position into the local NED frame
if self.centerlat == 0 and self.centerlon == 0:
self.rot = gpsfunctions.get_rot_matrix(float(latdec), float(londec))
self.centerlat = float(latdec)
self.centerlon = float(londec)
pos = self.rot * (
gpsfunctions.wgs842ecef(float(latdec), float(londec))
- gpsfunctions.wgs842ecef(float(self.centerlat), float(self.centerlon))
)
# print pos
# Legacy stuff
self.state[0] = [float(pos[0, 0]), 1]
# self.state[0] = [10,1]
self.state[1] = [speed, 1]
self.state[3] = [float(pos[1, 0]), 1]
# self.state[3] = [5,1]
# With [0:6] we ignore ".000" in the NMEA string.
# It is always zero for GPS1 anyways.
self.gpsmsg.time = int(content[1][0:6])
self.gpsmsg.latitude = latdec
self.gpsmsg.longitude = londec
self.gpsmsg.track_angle = float(content[8])
self.gpsmsg.date = int(content[9])
self.gpsmsg.SOG = speed
self.pub_gps.publish(self.gpsmsg)
elif ord(packet["MsgID"]) == 31:
self.n_rec += 1
msgnr = ord(packet["Data"][0])
self.plog.write(str(self.n_rec) + ", ")
self.plog.write("0, ")
self.plog.write(str(msgnr))
self.plog.write("\n")
else:
# print packet
print "gfoo"
except Exception as e:
self.excount += 1
print " " + str(self.excount)
print e
def parse(self,packet):
#print packet
try:
if(ord(packet['DevID']) == 20):
if(ord(packet['MsgID']) == 14):
#self.accelburst = self.accelburst + 1
#print "IMU: " + str(self.accelburst)
#print "IMU"
meas = numpy.zeros((9,2))
accelnr = 0
order = [7,1,4,6,6]
if self.mergedata:
pass
self.mergedata = False
#print "IMU!"
try:
#print "IMU"
'''The structure of the packet is
Zgyro
X acc
Y acc
X Mag
Y Mag
ADC
'''
#types = ['ADC','Ymag', 'Xmag', 'Yacc', 'Xacc', 'Zgyro']
#self.accellog.write("".join(packet['Data']) + "\r\n")
measurements = []
for i in range(len(packet['Data'])):
if ((i & 1) == 1):
tempval = packet['Data'][i-1:i+1]
tempval.reverse()
val = 0
try:
val = struct.unpack('h', "".join(tempval))
except:
pass
self.accellog.write(str(val[0]) + ", ")
self.fulllog.write(str(val[0]) + ", ")
measurements.append(val[0])
#print val[0]
#print measurements[5]
#print measurements
self.accellog.write(str(time.time()) + "\r\n")
self.fulllog.write(str(time.time()) + "\r\n")
if abs(measurements[5]) < 10: #Check that the grounded ADC doesn't return a high value
#Calculate heading from magnetometer:
heading = 0
if -measurements[3] > 0:
heading = (90 - atan(float(-measurements[4])/float(-measurements[3]))*180/pi)*pi/180
elif -measurements[3] < 0:
heading = (270 - atan(float(-measurements[4])/float(-measurements[3]))*180/pi)*pi/180
else:
if -measurements[4] < 0:
heading = pi
else:
heading = 0
#heading = -(2*pi-heading-pi/2)
heading = -heading
#print chr(27) + "[2J"
#print "[" + str(measurements[4]) + ", " + str(measurements[3]) + "]\t Theta: " + str(heading) + "\t Time:" + str(time.time())
accx = -measurements[2] * self.accconst
accy = -measurements[1] * self.accconst
gyroz = -measurements[0] * self.gyroconst
self.state[2] = [accx, 1]
self.state[5] = [accy, 1]
self.state[6] = [heading, 1]
self.state[7] = [gyroz, 1]
#print self.state
for i in range(numpy.size(self.state,0)):
for j in range(numpy.size(self.state,1)):
self.measureddata[i,j] = self.state[i,j]
#measstate = self.state
#print chr(27) + "[2J"
#print self.measureddata
self.state[:,1] = 0
'''
for i in range(len(packet['Data'])):
#print packet['Data'][i] +"\t (" + str(ord(packet['Data'][i])) + ")\t [" + hex(ord(packet['Data'][i])) + "]"
#print str(packet['Data'][i-1:i+1])
if ((i & 1) == 1): #Take every other value (Where the lower bit is 1)
tempval = packet['Data'][i-1:i+1] #Combine 2 of the numbers in to 1 value
tempval.reverse() #reverse them, to have the endian right.
#print str("".join(tempval))
val = 0
try:
val = struct.unpack('h', "".join(tempval))
except:
pass
#val = struct.unpack('h', "".join(packet['Data'][i-1:i+1]))
''#The structure of the packet is
#Zgyro
#X acc
#Y acc
#X Mag
#Y Mag
#ADC
''
self.accelburst[accelnr] = val[0]
try:
meas[order[accelnr]][0] = val[0]
meas[order[accelnr]][1] = 1
except IndexError:
pass
accelnr = accelnr + 1
#print str(val[0])
self.accelburst[accelnr] = packet['Time']
if(abs(self.accelburst[accelnr-1]) > 100):
print packet
print self.accelburst
#print self.accelburst
else:
self.q.put(meas)
self.writer.writerow(self.accelburst)
except Exception as e:
print e
'''
except Exception as e:
print e
#print "IMU BURST!"
pass
elif(ord(packet['MsgID']) == 13):
tmeasurements = []
measurements = []
for i in range(len(packet['Data'])):
if ((i & 1) == 1):
tempval = packet['Data'][i-1:i+1]
tempval.reverse()
val = 0
try:
val = struct.unpack('h', "".join(tempval))
except:
pass
self.accellog.write(str(val[0]) + ", ")
self.fulllog.write(str(val[0]) + ", ")
tmeasurements.append(val[0])
#print val[0]
#print measurements[5]
'''
[0] Supply
[1] X Gyro
[2] Y Gyro
[3] Z Gyro
[4] X
[5] Y
[6] Z Acc
[7] X
[8] Y
[9] Z Mag
[10] Temp
[11] ADC
What we want:
Zgyro
X acc
Y acc
X Mag
Y Mag
ADC
'''
#print
#print "READY"
measurements.append(tmeasurements[3])
measurements.append(tmeasurements[4])
measurements.append(tmeasurements[5])
measurements.append(tmeasurements[7])
measurements.append(tmeasurements[8])
measurements.append(tmeasurements[11])
print measurements
self.accellog.write(str(time.time()) + "\r\n")
self.fulllog.write(str(time.time()) + "\r\n")
if abs(measurements[5]) < 10: #Check that the grounded ADC doesn't return a high value
#Calculate heading from magnetometer:
heading = 0
if -measurements[3] > 0:
heading = (90 - atan(float(-measurements[4])/float(-measurements[3]))*180/pi)*pi/180
elif -measurements[3] < 0:
heading = (270 - atan(float(-measurements[4])/float(-measurements[3]))*180/pi)*pi/180
else:
if -measurements[4] < 0:
heading = pi
else:
heading = 0
#heading = -(2*pi-heading-pi/2)
heading = -heading
#print chr(27) + "[2J"
#print "[" + str(measurements[4]) + ", " + str(measurements[3]) + "]\t Theta: " + str(heading) + "\t Time:" + str(time.time())
accx = -measurements[2] * self.accconst
accy = -measurements[1] * self.accconst
gyroz = measurements[0] * self.gyroconst
self.state[2] = [accx, 1]
self.state[5] = [accy, 1]
self.state[6] = [heading, 1]
self.state[7] = [gyroz, 1]
#print self.state
for i in range(numpy.size(self.state,0)):
for j in range(numpy.size(self.state,1)):
self.measureddata[i,j] = self.state[i,j]
#measstate = self.state
#print chr(27) + "[2J"
#print self.measureddata
self.state = numpy.zeros((9,2))
elif(ord(packet['MsgID']) == 15):
self.n_rec += 1
msgnr = ord(packet['Data'][0])
#print msgnr
self.plog.write(str(self.n_rec) + ", ")
self.plog.write(str(msgnr))
self.plog.write(", 0\n")
elif (ord(packet['DevID']) == 30):
#print "GPS!"
#time.sleep(1)
if(ord(packet['MsgID']) == 6):
#print str("".join(packet['Data']))
content = "".join(packet['Data']).split(',')
if content[0] == "$GPRMC" and content[2] == 'A':
#print ",".join("".join(packet['Data']).split(',')[1:8])
content = content[1:8]
# The GPRMC packet contain the following information:
# [0] Timestamp
# [1] A for valid, V for invalid (only valid packets gets send)
# [2] Latitude
# [3] N or S (N)
# [4] Longitude
# [5] E or W (E)
# [6] Speed over ground
#print content[6]
'''
if content[1] == 'A' :
self.gpsinvalid += 1
if 42 <= self.gpsinvalid <= 67:
content[1] = 'V'
print "Gps invalid!"
'''
if content[1] == 'A' :
self.gpslog.write(",".join(content) + ", " + str(time.time()) + "\r\n")
self.fulllog.write(",".join(content) + ", " + str(time.time()) + "\r\n")
#print content
speed = float(content[6]) * 0.514444444 #* 0 + 100
#print str(speed) + " m/s"
[latdec, londec] = (gpsfunctions.nmea2decimal(float(content[2]),content[3],float(content[4]),content[5]))
latdec = latdec*pi/180
londec = londec*pi/180
if self.centerlat == 0 and self.centerlon == 0:
self.rot=gpsfunctions.get_rot_matrix(float(latdec),float(londec))
self.centerlat = float(latdec)
self.centerlon = float(londec)
pos = self.rot * (gpsfunctions.wgs842ecef(float(latdec),float(londec))-gpsfunctions.wgs842ecef(float(self.centerlat),float(self.centerlon)))
#print pos
#print pos
self.state[0] = [float(pos[0,0]), 1]
#self.state[0] = [10,1]
self.state[1] = [speed, 1]
self.state[3] = [float(pos[1,0]), 1]
#self.state[3] = [5,1]
elif(ord(packet['MsgID']) == 31):
self.n_rec += 1
msgnr = ord(packet['Data'][0])
self.plog.write(str(self.n_rec) + ", ")
self.plog.write("0, ")
self.plog.write(str(msgnr))
self.plog.write("\n")
'''
self.mergedata = True
self.gpspacket += 1
#print "GPS: " + str(self.gpspacket)
#print "".join(packet['Data']),
self.gpslog.write("".join(packet['Data']))
#self.gpswriter.writerow("".join(packet['Data']))
#print "Logged"
if("".join(packet['Data'][1:6]) == "GPGGA"):
gpgga = nmea.GPGGA()
tempstr = "".join(packet['Data'])
gpgga.parse(tempstr)
#print "Timestamp:" + gpgga.timestamp
''try:
deltat = int(float(gpgga.timestamp))-self.prevtime
print deltat
self.prevtime = int(float(gpgga.timestamp))
except Exception as e:
print e''
gpsd = [gpgga.timestamp, gpgga.latitude, gpgga.longitude, packet['Time']]
#self.gpswriter.writerow(gpsd)
elif("".join(packet['Data'][1:6]) == "GPRMC"):
gprmc = nmea.GPRMC()
tempstr = "".join(packet['Data'])
gprmc.parse(tempstr)
self.gpsdata[0] = gprmc.timestamp
self.gpsdata[1] = gprmc.lat
self.gpsdata[2] = gprmc.lon
self.gpsdata[3] = gprmc.spd_over_grnd
#self.gpsdata[4] = gprmc.true_course
#self.gpsdata[5] = gprmc.datestamp
#self.gpsdata[6] = gprmc.mag_variation
self.gpsdata[7] = packet['Time']
# print self.gpsdata
#print self.gpsdata
#self.gpswriter.writerow(self.gpsdata)
'''
else:
print packet
except Exception as e:
self.excount += 1
print " "+ str(self.excount)
print e,
def parse(self,packet):
#print packet
try:
if(ord(packet['DevID']) == 0): # LLI data
if(ord(packet['MsgID']) == 13): # Battery monitor sample
print("BANK1:\t")
self.bank1[0] = (ord(packet['Data'][0]) << 8 | ord(packet['Data'][1]))*(0.25*3.0/1.43)
self.bank1[1] = (ord(packet['Data'][2]) << 8 | ord(packet['Data'][3]))*(0.25*3.0/1.43)
self.bank1[2] = (ord(packet['Data'][4]) << 8 | ord(packet['Data'][5]))*(0.25*3.0/1.43)
self.bank1[3] = (ord(packet['Data'][6]) << 8 | ord(packet['Data'][7]))*(0.25*3.0/1.43)
print(self.bank1)
print("BANK2:\t")
self.bank2[0] = (ord(packet['Data'][8]) << 8 | ord(packet['Data'][9]))*(0.25*3.0/1.43)
self.bank2[1] = (ord(packet['Data'][10]) << 8 | ord(packet['Data'][11]))*(0.25*3.0/1.43)
self.bank2[2] = (ord(packet['Data'][12]) << 8 | ord(packet['Data'][13]))*(0.25*3.0/1.43)
self.bank2[3] = (ord(packet['Data'][14]) << 8 | ord(packet['Data'][15]))*(0.25*3.0/1.43)
print(self.bank2)
# Measured offsets
self.bank1[0] = self.bank1[0]+97.33
self.bank1[1] = self.bank1[1]+143.67
self.bank1[2] = self.bank1[2]+138.67
self.bank1[3] = self.bank1[3]+124.33
self.bank2[0] = self.bank2[0]+111.33
self.bank2[1] = self.bank2[1]+130.67
self.bank2[2] = self.bank2[2]+143.67
self.bank2[3] = self.bank2[3]+111.33
'''
if min(self.bank1) < 3600.0:
self.bank1 = [3600.0, 3600.0, 3600.0, 3600.0]
if min(self.bank2) < 3600.0:
self.bank2 = [3600.0, 3600.0, 3600.0, 3600.0]
if max(self.bank1) > 4200.0:
self.bank1 = [4200.0, 4200.0, 4200.0, 4200.0]
if max(self.bank2) > 4200.0:
self.bank2 = [4200.0, 4200.0, 4200.0, 4200.0]
'''
if not rospy.is_shutdown():
self.pub_bm.publish(self.bank1,self.bank2)
if(ord(packet['DevID']) == 20): # IMU data
if(ord(packet['MsgID']) == 14): # Burst read reduced
#self.accelburst = self.accelburst + 1
#print "IMU: " + str(self.accelburst)
#print "IMU"
meas = numpy.zeros((9,2))
accelnr = 0
order = [7,1,4,6,6]
if self.mergedata:
pass
self.mergedata = False
#print "IMU!"
try:
#print "IMU"
'''The structure of the packet is
Zgyro
X acc
Y acc
X Mag
Y Mag
ADC
'''
#types = ['ADC','Ymag', 'Xmag', 'Yacc', 'Xacc', 'Zgyro']
#self.accellog.write("".join(packet['Data']) + "\r\n")
measurements = []
for i in range(len(packet['Data'])):
if ((i & 1) == 1):
tempval = packet['Data'][i-1:i+1]
tempval.reverse()
val = 0
try:
val = struct.unpack('h', "".join(tempval))
except:
pass
self.accellog.write(str(val[0]) + ", ")
self.fulllog.write(str(val[0]) + ", ")
measurements.append(val[0])
#print val[0]
#print measurements[5]
#print measurements
self.accellog.write(str(time.time()) + "\r\n")
self.fulllog.write(str(time.time()) + "\r\n")
if abs(measurements[5]) < 10: #Check that the grounded ADC doesn't return a high value
#Calculate heading from magnetometer:
heading = 0
if -measurements[3] > 0:
heading = (90 - atan(float(-measurements[4])/float(-measurements[3]))*180/pi)*pi/180
elif -measurements[3] < 0:
heading = (270 - atan(float(-measurements[4])/float(-measurements[3]))*180/pi)*pi/180
else:
if -measurements[4] < 0:
heading = pi
else:
heading = 0
#heading = -(2*pi-heading-pi/2)
heading = -heading
#print chr(27) + "[2J"
#print "[" + str(measurements[4]) + ", " + str(measurements[3]) + "]\t Theta: " + str(heading) + "\t Time:" + str(time.time())
accx = -measurements[2] * self.accconst
accy = -measurements[1] * self.accconst
gyroz = -measurements[0] * self.gyroconst
self.state[2] = [accx, 1]
self.state[5] = [accy, 1]
self.state[6] = [heading, 1]
self.state[7] = [gyroz, 1]
#print self.state
for i in range(numpy.size(self.state,0)):
for j in range(numpy.size(self.state,1)):
self.measureddata[i,j] = self.state[i,j]
#measstate = self.state
#print chr(27) + "[2J"
#print self.measureddata
self.state[:,1] = 0
except Exception as e:
print e
print "exception"
#print "IMU BURST!"
pass
elif(ord(packet['MsgID']) == 13): # Burst read
# print "Burst read"
measurements = []
for i in range(len(packet['Data'])):
if ((i & 1) == 1):
tempval = packet['Data'][i-1:i+1]
tempval.reverse()
val = 0
try:
val = struct.unpack('h', "".join(tempval))
except:
pass
self.accellog.write(str(val[0]) + ", ")
self.fulllog.write(str(val[0]) + ", ")
measurements.append(val[0])
self.accellog.write(str(time.time()) + "\r\n")
self.fulllog.write(str(time.time()) + "\r\n")
self.state[0] = [measurements[0], 1] #supply
self.state[1] = [measurements[1], 1] #xacc
self.state[2] = [measurements[2], 1] #yacc
self.state[3] = [measurements[3], 1] #zacc
self.state[4] = [measurements[4], 1] #xgyro
self.state[5] = [measurements[5], 1] #ygyro
self.state[6] = [measurements[6], 1] #zgyro
self.state[7] = [measurements[7], 1] #xmag
self.state[8] = [measurements[8], 1] #ymag
self.state[9] = [measurements[9], 1] #zmag
self.state[10] = [measurements[10], 1] #temp
self.state[11] = [measurements[11], 1] #adc
# Writing to our "output" object measureddata
for i in range(numpy.size(self.state,0)):
for j in range(numpy.size(self.state,1)):
self.measureddata[i,j] = self.state[i,j]
self.state = numpy.zeros((12,2))
elif(ord(packet['MsgID']) == 15): # Reduced ADIS data, RF test
self.n_rec += 1
msgnr = ord(packet['Data'][0])
#print msgnr
self.plog.write(str(self.n_rec) + ", ")
self.plog.write(str(msgnr))
self.plog.write(", 0\n")
elif (ord(packet['DevID']) == 30): # GPS1 data
#print "GPS!"
#time.sleep(1)
if(ord(packet['MsgID']) == 6): # This is what the LII sends for the moment
#['$GPGGA', '133635.000', '5700.8791', 'N', '00959.1707', 'E', '1', '7', '1.23', '42.0', 'M', '42.5', 'M', '', '*6E\r\n']
#['$GPRMC', '133635.000', 'A', '5700.8791', 'N', '00959.1707', 'E', '0.20', '263.57', '040914', '', '', 'A*61\r\n']
# We expect to get both GPGGA and GPRMC at every GPS sample. This is in two messages.
# The GPRMC message is the plast one, so we publish our hybrid GPS message in that if.
#print str("".join(packet['Data']))
content = "".join(packet['Data']).split(',')
print(content)
if content[0] == "$GPGGA":
# The GPGGA packet contain the following information:
# [0] Message type ($GPGGA)
# [1] Time
# [2] Latitude
# [3] N or S (N)
# [4] Longitude
# [5] E or W (E)
# [6] Fix quality (expect 1 = GPS fix single)
# [7] Number of satellites being tracked
# [8] HDOP
# [9] Altitude, above mean sea level
# [10] Unit for altitude M = meters
# [11] Height of geoid (mean sea level) above WGS84 ellipsoid
# [12] Unit of heigt M = meters
# [13] DGPS stuff, ignore
# [14] DGPS stuff, ignore
# [15] Checksum
print("GGA")
if content[11] == '':
print('GGA parsing, no fix')
else:
self.gpsmsg.fix = int(content[6])
self.gpsmsg.sats = int(content[7])
self.gpsmsg.HDOP = float(content[8])
self.gpsmsg.altitude = float(content[9])
self.gpsmsg.height = float(content[11])
if content[0] == "$GPRMC" and content[2] == 'A':
#print ",".join("".join(packet['Data']).split(',')[1:8])
print("RMC")
# The GPRMC packet contain the following information:
# [0] Message type ($GPRMC)
# [1] Timestamp
# [2] A for valid, V for invalid (only valid packets gets send)
# [3] Latitude
# [4] N or S (N)
# [5] Longitude
# [6] E or W (E)
# [7] Speed over ground
# [8] Track angle
# [9] Date
# [10] Magnetic variation value [not existant on this cheap GPS]
# [11] Magnetic variation direction [not existant on this cheap GPS]
if content[2] == 'A' :
print("Wee, we have a valid $GPRMC fix")
self.gpslog.write(",".join(content) + ", " + str(time.time()) + "\r\n")
self.fulllog.write(",".join(content) + ", " + str(time.time()) + "\r\n")
#print content
speed = float(content[7]) * 0.514444444 #* 0 + 100
#print str(speed) + " m/s"
# Caculate decimal degrees
[latdec, londec] = (gpsfunctions.nmea2decimal(float(content[3]),content[4],float(content[5]),content[6]))
print(latdec,londec)
latdec = latdec*pi/180
londec = londec*pi/180
# Old code for rotating the position into the local NED frame
if self.centerlat == 0 and self.centerlon == 0:
self.rot=gpsfunctions.get_rot_matrix(float(latdec),float(londec))
self.centerlat = float(latdec)
self.centerlon = float(londec)
pos = self.rot * (gpsfunctions.wgs842ecef(float(latdec),float(londec))-gpsfunctions.wgs842ecef(float(self.centerlat),float(self.centerlon)))
#print pos
# Legacy stuff
self.state[0] = [float(pos[0,0]), 1]
#self.state[0] = [10,1]
self.state[1] = [speed, 1]
self.state[3] = [float(pos[1,0]), 1]
#self.state[3] = [5,1]
# With [0:6] we ignore ".000" in the NMEA string.
# It is always zero for GPS1 anyways.
self.gpsmsg.time = int(content[1][0:6])
self.gpsmsg.latitude = latdec
self.gpsmsg.longitude = londec
self.gpsmsg.track_angle = float(content[8])
self.gpsmsg.date = int(content[9])
self.gpsmsg.SOG = speed
self.pub_gps.publish(self.gpsmsg)
elif(ord(packet['MsgID']) == 31):
self.n_rec += 1
msgnr = ord(packet['Data'][0])
self.plog.write(str(self.n_rec) + ", ")
self.plog.write("0, ")
self.plog.write(str(msgnr))
self.plog.write("\n")
else:
#print packet
print "gfoo"
except Exception as e:
self.excount += 1
print " "+ str(self.excount)
print e
def parse(self,packet):
#print packet
try:
if(ord(packet['DevID']) == 20):
if(ord(packet['MsgID']) == 14):
#self.accelburst = self.accelburst + 1
#print "IMU: " + str(self.accelburst)
#print "IMU"
meas = numpy.zeros((9,2))
accelnr = 0
order = [7,1,4,6,6]
if self.mergedata:
pass
self.mergedata = False
#print "IMU!"
try:
#print "IMU"
'''The structure of the packet is
Zgyro
X acc
Y acc
X Mag
Y Mag
ADC
'''
#types = ['ADC','Ymag', 'Xmag', 'Yacc', 'Xacc', 'Zgyro']
#self.accellog.write("".join(packet['Data']) + "\r\n")
measurements = []
for i in range(len(packet['Data'])):
if ((i & 1) == 1):
tempval = packet['Data'][i-1:i+1]
tempval.reverse()
val = 0
try:
val = struct.unpack('h', "".join(tempval))
except:
pass
self.accellog.write(str(val[0]) + ", ")
self.fulllog.write(str(val[0]) + ", ")
measurements.append(val[0])
#print val[0]
#print measurements[5]
#print measurements
self.accellog.write(str(time.time()) + "\r\n")
self.fulllog.write(str(time.time()) + "\r\n")
if abs(measurements[5]) < 10: #Check that the grounded ADC doesn't return a high value
#Calculate heading from magnetometer:
heading = 0
if -measurements[3] > 0:
heading = (90 - atan(float(-measurements[4])/float(-measurements[3]))*180/pi)*pi/180
elif -measurements[3] < 0:
heading = (270 - atan(float(-measurements[4])/float(-measurements[3]))*180/pi)*pi/180
else:
if -measurements[4] < 0:
heading = pi
else:
heading = 0
#heading = -(2*pi-heading-pi/2)
heading = -heading
#print chr(27) + "[2J"
#print "[" + str(measurements[4]) + ", " + str(measurements[3]) + "]\t Theta: " + str(heading) + "\t Time:" + str(time.time())
accx = -measurements[2] * self.accconst
accy = -measurements[1] * self.accconst
gyroz = -measurements[0] * self.gyroconst
self.state[2] = [accx, 1]
self.state[5] = [accy, 1]
self.state[6] = [heading, 1]
self.state[7] = [gyroz, 1]
#print self.state
for i in range(numpy.size(self.state,0)):
for j in range(numpy.size(self.state,1)):
self.measureddata[i,j] = self.state[i,j]
#measstate = self.state
#print chr(27) + "[2J"
#print self.measureddata
self.state[:,1] = 0
except Exception as e:
print e
print "exception"
#print "IMU BURST!"
pass
elif(ord(packet['MsgID']) == 13):
tmeasurements = []
measurements = []
for i in range(len(packet['Data'])):
if ((i & 1) == 1):
tempval = packet['Data'][i-1:i+1]
tempval.reverse()
val = 0
try:
val = struct.unpack('h', "".join(tempval))
except:
pass
self.accellog.write(str(val[0]) + ", ")
self.fulllog.write(str(val[0]) + ", ")
tmeasurements.append(val[0])
#print val[0]
#print measurements[5]
'''
[0] Supply
[1] X Gyro
[2] Y Gyro
[3] Z Gyro
[4] X
[5] Y
[6] Z Acc
[7] X
[8] Y
[9] Z Mag
[10] Temp
[11] ADC
What we want:
Zgyro
X acc
Y acc
X Mag
Y Mag
ADC
'''
#print
#print "READY"
measurements.append(tmeasurements[3])
measurements.append(tmeasurements[4])
measurements.append(tmeasurements[5])
measurements.append(tmeasurements[7])
measurements.append(tmeasurements[8])
measurements.append(tmeasurements[11])
print measurements
self.accellog.write(str(time.time()) + "\r\n")
self.fulllog.write(str(time.time()) + "\r\n")
if abs(measurements[5]) < 10: #Check that the grounded ADC doesn't return a high value
#Calculate heading from magnetometer:
heading = 0
if -measurements[3] > 0:
heading = (90 - atan(float(-measurements[4])/float(-measurements[3]))*180/pi)*pi/180
elif -measurements[3] < 0:
heading = (270 - atan(float(-measurements[4])/float(-measurements[3]))*180/pi)*pi/180
else:
if -measurements[4] < 0:
heading = pi
else:
heading = 0
#heading = -(2*pi-heading-pi/2)
heading = -heading
#print chr(27) + "[2J"
#print "[" + str(measurements[4]) + ", " + str(measurements[3]) + "]\t Theta: " + str(heading) + "\t Time:" + str(time.time())
accx = -measurements[2] * self.accconst
accy = -measurements[1] * self.accconst
gyroz = measurements[0] * self.gyroconst
self.state[2] = [accx, 1]
self.state[5] = [accy, 1]
self.state[6] = [heading, 1]
self.state[7] = [gyroz, 1]
#print self.state
for i in range(numpy.size(self.state,0)):
for j in range(numpy.size(self.state,1)):
self.measureddata[i,j] = self.state[i,j]
#measstate = self.state
#print chr(27) + "[2J"
#print self.measureddata
self.state = numpy.zeros((9,2))
elif(ord(packet['MsgID']) == 15): # Reduced ADIS data
self.n_rec += 1
msgnr = ord(packet['Data'][0])
#print msgnr
self.plog.write(str(self.n_rec) + ", ")
self.plog.write(str(msgnr))
self.plog.write(", 0\n")
elif (ord(packet['DevID']) == 30): # GPS data
#print "GPS!"
#time.sleep(1)
if(ord(packet['MsgID']) == 6):
#print str("".join(packet['Data']))
content = "".join(packet['Data']).split(',')
if content[0] == "$GPRMC" and content[2] == 'A':
#print ",".join("".join(packet['Data']).split(',')[1:8])
content = content[1:8]
# The GPRMC packet contain the following information:
# [0] Timestamp
# [1] A for valid, V for invalid (only valid packets gets send)
# [2] Latitude
# [3] N or S (N)
# [4] Longitude
# [5] E or W (E)
# [6] Speed over ground
#print content[6]
'''
if content[1] == 'A' :
self.gpsinvalid += 1
if 42 <= self.gpsinvalid <= 67:
content[1] = 'V'
print "Gps invalid!"
'''
if content[1] == 'A' :
self.gpslog.write(",".join(content) + ", " + str(time.time()) + "\r\n")
self.fulllog.write(",".join(content) + ", " + str(time.time()) + "\r\n")
#print content
speed = float(content[6]) * 0.514444444 #* 0 + 100
#print str(speed) + " m/s"
[latdec, londec] = (gpsfunctions.nmea2decimal(float(content[2]),content[3],float(content[4]),content[5]))
latdec = latdec*pi/180
londec = londec*pi/180
if self.centerlat == 0 and self.centerlon == 0:
self.rot=gpsfunctions.get_rot_matrix(float(latdec),float(londec))
self.centerlat = float(latdec)
self.centerlon = float(londec)
pos = self.rot * (gpsfunctions.wgs842ecef(float(latdec),float(londec))-gpsfunctions.wgs842ecef(float(self.centerlat),float(self.centerlon)))
#print pos
#print pos
self.state[0] = [float(pos[0,0]), 1]
#self.state[0] = [10,1]
self.state[1] = [speed, 1]
self.state[3] = [float(pos[1,0]), 1]
#self.state[3] = [5,1]
elif(ord(packet['MsgID']) == 31):
self.n_rec += 1
msgnr = ord(packet['Data'][0])
self.plog.write(str(self.n_rec) + ", ")
self.plog.write("0, ")
self.plog.write(str(msgnr))
self.plog.write("\n")
elif (ord(packet['DevID']) == 0): # LLI data
print "LLI IS COMMING TO TOWN**************************"
else:
#print packet
print "gfoo"
except Exception as e:
self.excount += 1
print " "+ str(self.excount)
print e,