00001
00002 import serial
00003 import struct
00004 import math
00005
00006 """
00007 Simple serial (RS232) driver for the CH Robotics UM6 IMU
00008
00009 NOTE: this driver expects a number of things to be true:
00010 1. Broadcast mode is enabled
00011 2. QUATERNION output is enabled
00012 2. EULER output is enabled
00013 2. ANGULAR VELOCITY output is enabled
00014 2. LINEAR ACCEL output is enabled
00015
00016 Use CHRobotics Serial Interface program
00017 to set the appropriate outputs
00018
00019 see main() example at end of file for usage
00020
00021 """
00022 class Um6Drv:
00023
00024
00025 DATA_QUATERNION = 0x01
00026 DATA_ROLL_PITCH_YAW = 0x02
00027 DATA_ANGULAR_VEL = 0x04
00028 DATA_LINEAR_ACCEL = 0x08
00029
00030
00031 UM6_GYRO_PROC_XY = 0x5C
00032 UM6_GYRO_PROC_Z = 0x5D
00033 UM6_ACCEL_PROC_XY = 0x5E
00034 UM6_ACCEL_PROC_Z = 0x5F
00035 UM6_EULER_PHI_THETA = 0x62
00036 UM6_EULER_PSI = 0x63
00037 UM6_QUAT_AB = 0x64
00038 UM6_QUAT_CD = 0x65
00039
00040
00041 CMD_ZERO_GYROS = 0xAC
00042 CMD_RESET_EKF = 0xAD
00043 CMD_SET_ACCEL_REF = 0xAF
00044 CMD_SET_MAG_REF = 0xB0
00045 CMD_BAD_CHECKSUM = 0xFD
00046
00047
00048 cmd_cb = { CMD_ZERO_GYROS: None,
00049 CMD_RESET_EKF: None,
00050 CMD_SET_ACCEL_REF: None,
00051 CMD_SET_MAG_REF: None,
00052 CMD_BAD_CHECKSUM: None }
00053
00054
00055 PKT_TYPE_IDX = 0
00056 PKT_ADDR_IDX = 1
00057 PKT_DATA_IDX = 2
00058
00059
00060 SCALE_QUAT = 0.0000335693
00061 SCALE_EULER = 0.0109863
00062 SCALE_GYRO = 0.0610352
00063 SCALE_ACCEL = 0.000183105
00064
00065 NO_DATA_PACKET = 0x00
00066
00067 quaternion = []
00068 wip_quat = []
00069
00070 valid = {'quaternion': False,
00071 'lin_acc_x': False,
00072 'lin_acc_y': False,
00073 'lin_acc_z': False,
00074 'ang_vel_x': False,
00075 'ang_vel_y': False,
00076 'ang_vel_z': False,
00077 'yaw': False,
00078 'roll': False,
00079 'pitch': False }
00080
00081 def __init__(self, port, outputDataMask, cb=None):
00082 self.ser = serial.Serial(port,
00083 baudrate=115200,
00084 bytesize=8, parity='N',
00085 stopbits=1, timeout=None)
00086 self.data_callback = cb
00087 self.output = outputDataMask
00088
00089 def __del__(self):
00090 self.ser.close()
00091
00092 def update(self):
00093 pkt = self.readPacket()
00094 if (pkt != Um6Drv.NO_DATA_PACKET):
00095 self.decodePacket(pkt)
00096
00097 def syncToHeader(self):
00098 while (self.ser.inWaiting() > 0):
00099 if (self.ser.read()=='s' and
00100 self.ser.read()=='n' and
00101 self.ser.read()=='p'):
00102 return 1
00103 return 0
00104
00105 def sendCommand(self, cmd, callback):
00106 self.cmd_cb[cmd] = callback
00107 pkt = [0, cmd]
00108 self.writePacket(pkt)
00109
00110 def writePacket(self, pkt):
00111
00112
00113 self.ser.write('s')
00114 self.ser.write('n')
00115 self.ser.write('p')
00116
00117 chkSum = (self.chToByte("s") +
00118 self.chToByte("n") +
00119 self.chToByte("p"))
00120
00121 for i in range(0, len(pkt)):
00122 self.ser.write(chr(pkt[i]))
00123 chkSum += pkt[i]
00124
00125 strChkSum = ("%s"%(chkSum))
00126
00127 high = (chkSum >> 8) & 0x00FF
00128 low = 0x00FF & chkSum
00129
00130 self.ser.write(chr(high))
00131 self.ser.write(chr(low))
00132
00133 def readPacket(self):
00134 if (not self.syncToHeader()):
00135 return Um6Drv.NO_DATA_PACKET
00136
00137 packetType = self.chToByte(self.ser.read())
00138
00139 hasData = packetType >> 7
00140 isBatch = (packetType >> 6) & 0x01
00141 batchLen = (packetType >> 2) & 0x0F
00142
00143
00144 addr = self.chToByte(self.ser.read())
00145
00146 calcChkSum = (self.chToByte("s") +
00147 self.chToByte("n") +
00148 self.chToByte("p") +
00149 packetType + addr)
00150
00151 packet = []
00152 packet.append(packetType)
00153 packet.append(addr)
00154
00155 for i in range(0, batchLen * 4):
00156 byte = self.chToByte(self.ser.read())
00157 packet.append(byte)
00158 calcChkSum += byte
00159
00160 high = self.chToByte(self.ser.read())
00161 low = self.chToByte(self.ser.read())
00162
00163 chkSum = self.bytesToShort(high,low)
00164
00165 if (calcChkSum != chkSum):
00166 print "Read Pkt: BAD CHECKSUM"
00167
00168 return packet
00169
00170 def decodePacket(self, pkt):
00171 pt = pkt[Um6Drv.PKT_TYPE_IDX]
00172 batchLen = (pt >> 2) & 0x0F
00173 startAddr = pkt[Um6Drv.PKT_ADDR_IDX]
00174 dataIdx = Um6Drv.PKT_DATA_IDX
00175
00176 addr = startAddr
00177
00178 result = 0 == (pt & 0x01)
00179
00180 try:
00181 if (self.cmd_cb[addr] is not None):
00182 self.cmd_cb[addr](addr, result)
00183 except KeyError:
00184 self.cmd_cb[addr] = None
00185
00186 if (addr == Um6Drv.CMD_BAD_CHECKSUM):
00187 print "Rx Packet: BAD CHECKSUM"
00188
00189 for i in range(0, batchLen):
00190 self.parseAndSendData(addr+i, pkt[dataIdx+(i*4):dataIdx+((i+1)*4)])
00191
00192 def parseAndSendData(self, addr, data):
00193 if (addr == Um6Drv.UM6_GYRO_PROC_XY):
00194 self.ang_vel_x = ((self.bytesToShort(data[0],data[1])) *
00195 Um6Drv.SCALE_GYRO)
00196 self.ang_vel_y = ((self.bytesToShort(data[2],data[3])) *
00197 Um6Drv.SCALE_GYRO)
00198 self.valid['ang_vel_x'] = True
00199 self.valid['ang_vel_y'] = True
00200
00201 if (addr == Um6Drv.UM6_GYRO_PROC_Z):
00202 self.ang_vel_z = ((self.bytesToShort(data[0],data[1])) *
00203 Um6Drv.SCALE_GYRO)
00204 self.valid['ang_vel_z'] = True
00205
00206 if (addr == Um6Drv.UM6_ACCEL_PROC_XY):
00207 self.lin_acc_x = ((self.bytesToShort(data[0],data[1])) *
00208 Um6Drv.SCALE_ACCEL)
00209 self.lin_acc_y = ((self.bytesToShort(data[2],data[3])) *
00210 Um6Drv.SCALE_ACCEL)
00211 self.valid['lin_acc_x'] = True
00212 self.valid['lin_acc_y'] = True
00213
00214 if (addr == Um6Drv.UM6_ACCEL_PROC_Z):
00215 self.lin_acc_z = ((self.bytesToShort(data[0],data[1])) *
00216 Um6Drv.SCALE_ACCEL)
00217 self.valid['lin_acc_z'] = True
00218
00219 if (addr == Um6Drv.UM6_QUAT_AB):
00220 a = (self.bytesToShort(data[0],data[1]))*Um6Drv.SCALE_QUAT
00221 b = (self.bytesToShort(data[2],data[3]))*Um6Drv.SCALE_QUAT
00222 self.wip_quat.append(a)
00223 self.wip_quat.append(b)
00224
00225 if (addr == Um6Drv.UM6_EULER_PHI_THETA):
00226 self.roll = (self.bytesToShort(data[0],data[1]))*Um6Drv.SCALE_EULER
00227 self.pitch = (self.bytesToShort(data[2],data[3]))*Um6Drv.SCALE_EULER
00228 self.valid['roll'] = True
00229 self.valid['pitch'] = True
00230
00231 if (addr == Um6Drv.UM6_EULER_PSI):
00232 self.yaw = (self.bytesToShort(data[0],data[1]))*Um6Drv.SCALE_EULER
00233 self.valid['yaw'] = True
00234
00235 if (addr == Um6Drv.UM6_QUAT_CD):
00236 c = (self.bytesToShort(data[0],data[1]))*Um6Drv.SCALE_QUAT
00237 d = (self.bytesToShort(data[2],data[3]))*Um6Drv.SCALE_QUAT
00238 self.wip_quat.append(c)
00239 self.wip_quat.append(d)
00240 self.quaternion = []
00241 self.quaternion.append(self.wip_quat[0])
00242 self.quaternion.append(self.wip_quat[1])
00243 self.quaternion.append(self.wip_quat[2])
00244 self.quaternion.append(self.wip_quat[3])
00245 self.wip_quat = []
00246 self.valid['quaternion'] = True
00247
00248 if (self.valid['quaternion'] == True and
00249 self.valid['ang_vel_x'] == True and
00250 self.valid['ang_vel_y'] == True and
00251 self.valid['ang_vel_z'] == True and
00252 self.valid['lin_acc_x'] == True and
00253 self.valid['lin_acc_y'] == True and
00254 self.valid['lin_acc_z'] == True and
00255 self.valid['pitch'] == True and
00256 self.valid['roll'] == True and
00257 self.valid['yaw'] == True):
00258
00259 self.valid['quaternion'] = False
00260 self.valid['ang_vel_x'] = False
00261 self.valid['ang_vel_y'] = False
00262 self.valid['ang_vel_z'] = False
00263 self.valid['lin_acc_x'] = False
00264 self.valid['lin_acc_y'] = False
00265 self.valid['lin_acc_z'] = False
00266 self.valid['pitch'] = False
00267 self.valid['roll'] = False
00268 self.valid['yaw'] = False
00269
00270 results = {}
00271 if ((self.output & Um6Drv.DATA_QUATERNION) != 0):
00272 results['DATA_QUATERNION'] = self.quaternion
00273 if ((self.output & Um6Drv.DATA_ANGULAR_VEL) != 0):
00274 results['DATA_ANGULAR_VEL'] = [self.ang_vel_x,
00275 self.ang_vel_y,
00276 self.ang_vel_z]
00277 if ((self.output & Um6Drv.DATA_LINEAR_ACCEL) != 0):
00278 results['DATA_LINEAR_ACCEL'] = [self.lin_acc_x,
00279 self.lin_acc_y,
00280 self.lin_acc_z]
00281 if ((self.output & Um6Drv.DATA_ROLL_PITCH_YAW) != 0):
00282 results['DATA_ROLL_PITCH_YAW'] = [self.roll,
00283 self.pitch,
00284 self.yaw]
00285 if (self.data_callback is not None):
00286 self.data_callback(results)
00287
00288 def bytesToShort(self, high, low):
00289
00290 return struct.unpack("h", chr(low) + chr(high))[0]
00291
00292 def chToByte(self, ch):
00293
00294 return struct.unpack("B", ch)[0]
00295
00296 if __name__ == '__main__':
00297
00298
00299
00300 def data_cb(data): print "rx pkt"
00301 def cmd_cb(cmd, result): print "CMD %s Success: %s"%(cmd,result)
00302
00303
00304 dataMask = Um6Drv.DATA_QUATERNION | Um6Drv.DATA_ROLL_PITCH_YAW
00305
00306
00307 um6 = Um6Drv(3, dataMask, data_cb)
00308
00309
00310 um6.sendCommand(Um6Drv.CMD_ZERO_GYROS, cmd_cb)
00311 um6.sendCommand(Um6Drv.CMD_SET_MAG_REF, cmd_cb)
00312 um6.sendCommand(Um6Drv.CMD_SET_ACCEL_REF, cmd_cb)
00313
00314
00315 while(True):
00316 um6.update()