def __init__(self):
# Open the file descriptors
BUFSIZE = 1024
self.mb100log = open("logs/mb100.log",'wb',BUFSIZE)
self.mb100rcv = serial.Serial("/dev/mb100rover",115200,timeout=0.04)
# Static rotation matrix
self.klingen = sio.loadmat('klingenberg.mat')
self.klingen = sio.loadmat('fjorden.mat')
self.Rn2e = geo.RNED2ECEF(self.klingen['rotlon'], self.klingen['rotlat'])
self.Re2n = self.Rn2e.T
self.pos_of_ned_in_ecef = geo.wgs842ecef(self.klingen['rotlat'], self.klingen['rotlon'])
# Create objects for the kftrack stuff
self.kftrackpath = rospy.Publisher('kftrack', Path, queue_size=3)
self.kftrackmsg = Path()
self.kftrackmsg.header.frame_id = "ned"
# Initialise pose for the graphic path segment for rviz
h = Header()
p = Point(0,0,0)
q = Quaternion(0,0,0,1)
self.kftrackmsg.poses.append(PoseStamped(h, Pose(p, q)))
self.kftrackmsg.poses.append(PoseStamped(h, Pose(p, q)))
self.x_hat = np.zeros(17)
def gps1cb(self, data):
#z = [N E psi u v udot vdot]
# Positoin in NED
# This seems to only be approximate
pos_ecef = geo.wgs842ecef(data.latitude, data.longitude, 0.0)
pos_ned = self.Re2n.dot( pos_ecef - self.pos_of_ned_in_ecef )
self.z[0:2] = np.squeeze( pos_ned[0:2] )
#print((self.z[0], self.z[1]))
# Body velocities
#print('')
#print('track angle: ' + str(data.track_angle))
#print('heading ang: ' + str(self.z[2]))
beta = data.track_angle - self.z[2] # sideslip angle = track_angle - heading
#print(beta)
U_b = np.array([cos(beta), sin(beta)]) * data.SOG
self.z[3:5] = np.squeeze( U_b )
#print(self.z[3:5])
#print('GPS recieved')
self.R[0,0] = self.R_i[0,0]
self.R[1,1] = self.R_i[1,1]
self.R[3,3] = self.R_i[3,3]
self.R[4,4] = self.R_i[4,4]
def __init__(self):
# Open the file descriptors
BUFSIZE = 1024
self.mb100log = open("logs/mb100.log", 'wb', BUFSIZE)
self.mb100rcv = serial.Serial("/dev/mb100rover", 115200, timeout=0.04)
# Static rotation matrix
self.klingen = sio.loadmat('klingenberg.mat')
self.fjorden = sio.loadmat('fjorden.mat')
self.Rn2e = geo.RNED2ECEF(self.klingen['rotlon'],
self.klingen['rotlat'])
self.Re2n = self.Rn2e.T
self.pos_of_ned_in_ecef = geo.wgs842ecef(self.klingen['rotlat'],
self.klingen['rotlon'])
# Create objects for the kftrack stuff
self.kftrackpath = rospy.Publisher('kftrack', Path, queue_size=3)
self.kftrackmsg = Path()
self.kftrackmsg.header.frame_id = "ned"
# Initialise pose for the graphic path segment for rviz
h = Header()
p = Point(0, 0, 0)
q = Quaternion(0, 0, 0, 1)
self.kftrackmsg.poses.append(PoseStamped(h, Pose(p, q)))
self.kftrackmsg.poses.append(PoseStamped(h, Pose(p, q)))
self.x_hat = np.zeros(17)
def __init__(self):
self.thrustdiff = 0
self.tau = np.zeros(5) # input vector
self.x = np.zeros(17) # state vector
self.x[0] = -40
self.x[1] = -50
self.x_hat = self.x
self.v = np.array([0.1,0.1,13.5969e-006,0.2,0.2,0.00033,0.00033])#Measurement,noise
self.z = np.zeros(7)
self.P_plus = np.zeros([17,17])
self.R = np.diag(self.v)
self.R_i = np.diag(self.v)
self.old_z = np.ones(7) # Used for calculation of speed over ground and track angle for the GPS
# Construct kalmanfilterfoo, because it contains the aaushipsimmodel function
self.f = kfoo.KF()
self.roll = 0
self.pitch = 0
self.yaw = 0
self.rightthruster = 0
self.leftthruster = 0
rospy.init_node('simulation_node')
self.r = rospy.Rate(20) # Hz
self.sub = rospy.Subscriber('lli_input', LLIinput, self.llicb)
self.pub = rospy.Publisher('kf_states', Float64MultiArray, queue_size=1) # This should eventually be removed when the kf-node is tested against this
self.subahrs = rospy.Subscriber('attitude', Quaternion, self.ahrscb) # Should be removed from here when the kf-node is tested against this
self.pubimu = rospy.Publisher('imu', ADIS16405, queue_size=1, latch=True)
self.trackpath = rospy.Publisher('track', Path, queue_size=3)
self.pubgps1 = rospy.Publisher('gps1', GPS, queue_size=1, latch=False)
# Construct variables for messages
self.imumsg = ADIS16405()
self.pubmsg = Float64MultiArray()
self.trackmsg = Path()
self.trackmsg.header.frame_id = "ned"
self.gpsmsg = GPS()
# Load external staitc map and path data
self.klingen = sio.loadmat('klingenberg.mat')
self.path = sio.loadmat('../../../../../matlab/2mmargintrack.mat')
# Static rotation matrix
self.Rn2e = geo.RNED2ECEF(self.klingen['rotlon'], self.klingen['rotlat'])
self.pos_of_ned_in_ecef = geo.wgs842ecef(self.klingen['rotlat'], self.klingen['rotlon'])
# Variables for the thrusters
self.leftthruster = 0.0
self.rightthruster = 0.0
h = Header()
q = Quaternion(0,0,0,1)
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 __init__(self):
# Load discretised model constants
self.ssmat = sio.loadmat('../../../../../matlab/ssaauship.mat')
self.tau = np.zeros(5) # input vector
self.x = np.zeros(17) # state vector
self.x[0] = 20
self.x_hat = self.x
self.x_hat[0] = -40
self.x_hat[1] = -50
# Measurement noise vector and covarince matrix
self.v = np.array([0.1,0.1,13.5969e-006,0.2,0.2,0.033,0.033])#Measurement,noise
self.P_plus = np.zeros([17,17])
self.R = np.diag(self.v)
self.R_i = np.diag(self.v)
# Initialisation of thruster input variables
self.rightthruster = 0
self.leftthruster = 0
# Construct kalmanfilterfoo, because it contains the aaushipsimmodel function
self.f = kfoo.KF()
# Process noise vector and covariance matrix
self.w = np.array([0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.01, 0.01, 0.01, 0.01, 0.01, 0.033, 0.033, 0.033, 0.033, 0.033])
self.Q = np.diag(self.w)
self.no_of_states = 17
# Measurement vector
self.z = np.zeros(7)
self.z[0] = 20
self.kftrackpath = rospy.Publisher('kftrack', Path, queue_size=3)
self.kftrackmsg = Path()
self.kftrackmsg.header.frame_id = "ned"
# Static rotation matrix
self.klingen = sio.loadmat('klingenberg.mat')
self.Rn2e = geo.RNED2ECEF(self.klingen['rotlon'], self.klingen['rotlat'])
self.Re2n = self.Rn2e.T
self.pos_of_ned_in_ecef = geo.wgs842ecef(self.klingen['rotlat'], self.klingen['rotlon'])
self.fjorden = sio.loadmat('fjorden.mat')
# Define the path poses for the map to display in rviz
self.refmsg = Path()
self.refmsg.header.frame_id = "ned"
self.keepoutmsg = Path()
self.keepoutmsg.header.frame_id = "ned"
q = Quaternion(0,0,0,1)
h = Header()
offset = 3
for i in self.klingen['outer']:
p = Point(i[0]-offset,i[1],0)
self.refmsg.poses.append(PoseStamped(h, Pose(p, q)))
for i in self.klingen['inner']:
p = Point(i[0]-offset,i[1],0)
self.keepoutmsg.poses.append(PoseStamped(h, Pose(p, q)))
for i in self.fjorden['all']:
p = Point(i[0]-offset,i[1],0)
self.keepoutmsg.poses.append(PoseStamped(h, Pose(p, q)))
# Topics
self.subgps1 = rospy.Subscriber('gps1', GPS, self.gps1cb)
self.subgps2 = rospy.Subscriber('gps2', GPS, self.gps2cb)
self.subimu = rospy.Subscriber('imu', ADIS16405, self.imucb)
self.subahrs = rospy.Subscriber('attitude', Quaternion, self.ahrscb)
self.sub = rospy.Subscriber('lli_input', LLIinput, self.llicb)
self.pub = rospy.Publisher('kf_statesnew', Float64MultiArray, queue_size=1)
self.refpath = rospy.Publisher('refpath', Path, queue_size=3, latch=True)
self.keepoutpath = rospy.Publisher('keepout', Path, queue_size=3, latch=True)
# Initialise pose for the graphic path segment for rviz
h = Header()
p = Point(0,0,0)
q = Quaternion(0,0,0,1)
self.kftrackmsg.poses.append(PoseStamped(h, Pose(p, q)))
self.kftrackmsg.poses.append(PoseStamped(h, Pose(p, q)))
# Initialize common variables
self.roll = 0
self.pitch = 0
self.yaw = 0
self.rightthruster = 0
self.leftthruster = 0
rospy.init_node('klamanfilter_node')
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 run(self):
# Example of data
#line = "$PASHR,POS,1,10,134035.35,5700.8745400,N,00959.1551112,E,059.318,10.4,077.3,000.459,+000.069,1.7,0.9,1.4,0.9,Hp23*30"
#print(self.parse(line))
pub = rospy.Publisher('kf_statesnew', Float64MultiArray, queue_size=1)
rospy.init_node('mb100')
r = rospy.Rate(200)
# Do the publishing of the mission boundary path and keeoput
# zone
self.refpath = rospy.Publisher('refpath',
Path,
queue_size=3,
latch=True)
self.keepoutpath = rospy.Publisher('keepout',
Path,
queue_size=3,
latch=True)
# Define the path poses for the map to display in rviz
self.refmsg = Path()
self.refmsg.header.frame_id = "ned"
self.keepoutmsg = Path()
self.keepoutmsg.header.frame_id = "ned"
q = Quaternion(0, 0, 0, 1)
h = Header()
offset = 3
for i in self.klingen['outer']:
p = Point(i[0] - offset, i[1], 0)
self.refmsg.poses.append(PoseStamped(h, Pose(p, q)))
for i in self.klingen['inner']:
p = Point(i[0] - offset, i[1], 0)
self.keepoutmsg.poses.append(PoseStamped(h, Pose(p, q)))
for i in self.fjorden['all']:
p = Point(i[0] - offset, i[1], 0)
self.keepoutmsg.poses.append(PoseStamped(h, Pose(p, q)))
self.refpath.publish(self.refmsg)
self.keepoutpath.publish(self.keepoutmsg)
modecount = 0
while not rospy.is_shutdown():
# Grabbing the data
try:
data = self.mb100rcv.readline()
if data != "":
data = data.rstrip()
parsed = self.parse(data)
self.pubmsg = Float64MultiArray()
# Headpoint of trail track
p = Point(self.x_hat[0], self.x_hat[1], 0.0)
q = Quaternion(0, 0, 0, 1)
self.kftrackmsg.poses[0] = PoseStamped(
Header(), Pose(p, q))
# Positoin in NED
pos_ecef = geo.wgs842ecef(parsed['lat'], parsed['lon'],
0.0)
pos_ned = self.Re2n.dot(pos_ecef - self.pos_of_ned_in_ecef)
self.x_hat[0:2] = np.squeeze(pos_ned[0:2])
#TODO Not exactly the heading, should probably use the heading from
# the AHRS filter.
self.x_hat[6] = parsed['cog']
#TODO When implementing for multi control, e.g. the duckling, then
# it is probably of interest to calculate the u and v speeds
# also.
# Fill in the message to be published in ROS
for a in self.x_hat:
self.pubmsg.data.append(a)
pub.publish(self.pubmsg)
# Endpoint of trail track
p = Point(self.x_hat[0], self.x_hat[1], 0.0)
self.kftrackmsg.poses[1] = PoseStamped(
Header(), Pose(p, q))
self.kftrackpath.publish(self.kftrackmsg)
if modecount >= 20:
modecount = 0
print("Position mode from MB100: " +
str(parsed['posmode']))
modecount = modecount + 1
except Exception:
pass
# End of loop, wait to keep the rate
r.sleep()
print("Closing MB100 node")
self.mb100log.close()
self.mb100rcv.close()
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 run(self):
# Example of data
#line = "$PASHR,POS,1,10,134035.35,5700.8745400,N,00959.1551112,E,059.318,10.4,077.3,000.459,+000.069,1.7,0.9,1.4,0.9,Hp23*30"
#print(self.parse(line))
pub = rospy.Publisher('kf_statesnew', Float64MultiArray, queue_size=1)
rospy.init_node('mb100')
r = rospy.Rate(200)
# Do the publishing of the mission boundary path and keeoput
# zone
self.refpath = rospy.Publisher('refpath', Path, queue_size=3, latch=True)
self.keepoutpath = rospy.Publisher('keepout', Path, queue_size=3, latch=True)
# Define the path poses for the map to display in rviz
self.refmsg = Path()
self.refmsg.header.frame_id = "ned"
self.keepoutmsg = Path()
self.keepoutmsg.header.frame_id = "ned"
q = Quaternion(0,0,0,1)
h = Header()
offset = 3
for i in self.klingen['outer']:
p = Point(i[0]-offset,i[1],0)
self.refmsg.poses.append(PoseStamped(h, Pose(p, q)))
for i in self.klingen['inner']:
p = Point(i[0]-offset,i[1],0)
self.keepoutmsg.poses.append(PoseStamped(h, Pose(p, q)))
for i in self.fjorden['all']:
p = Point(i[0]-offset,i[1],0)
self.keepoutmsg.poses.append(PoseStamped(h, Pose(p, q)))
self.refpath.publish(self.refmsg)
self.keepoutpath.publish(self.keepoutmsg)
modecount = 0
while not rospy.is_shutdown():
# Grabbing the data
try:
data = self.mb100rcv.readline()
if data != "":
data = data.rstrip()
parsed = self.parse(data)
self.pubmsg = Float64MultiArray()
# Headpoint of trail track
p = Point(self.x_hat[0],self.x_hat[1],0.0)
q = Quaternion(0,0,0,1)
self.kftrackmsg.poses[0] = PoseStamped(Header(), Pose(p, q))
# Positoin in NED
pos_ecef = geo.wgs842ecef(parsed['lat'], parsed['lon'], 0.0)
pos_ned = self.Re2n.dot( pos_ecef - self.pos_of_ned_in_ecef )
self.x_hat[0:2] = np.squeeze( pos_ned[0:2] )
#TODO Not exactly the heading, should probably use the heading from
# the AHRS filter.
self.x_hat[6] = parsed['cog']
#TODO When implementing for multi control, e.g. the duckling, then
# it is probably of interest to calculate the u and v speeds
# also.
# Fill in the message to be published in ROS
for a in self.x_hat:
self.pubmsg.data.append(a)
pub.publish(self.pubmsg)
# Endpoint of trail track
p = Point(self.x_hat[0],self.x_hat[1],0.0)
self.kftrackmsg.poses[1] = PoseStamped(Header(), Pose(p, q))
self.kftrackpath.publish(self.kftrackmsg)
if modecount >= 20:
modecount = 0
print("Position mode from MB100: " + str(parsed['posmode']))
modecount = modecount + 1
except Exception:
pass
# End of loop, wait to keep the rate
r.sleep()
print("Closing MB100 node")
self.mb100log.close()
self.mb100rcv.close()
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,
