def start(self): self.curtime = robtimer() opsStats = { 'totalLoopTime': 0, 'numLoops': 0, 'totalWaitTime': 0, 'numWaits': 0, 'successfulReadings': 0, 'badReadings': 0 } lastWaitStart = 0 gyroReading = gyro(0, 0, 0, self.curtime) while True: loopStartTime = robtimer() opsStats['numLoops'] += 1 mpuReading = self.mpu.read() gyroReading = mpuReading.gyro #logging.debug("%s",(str(mpuReading),)) if gyroReading.z != None: opsStats['successfulReadings'] += 1 if robtimer( ) - self.lastMpuReportTime > self.mpuReportInterval: self.broadcastQ.put(mpuReading) self.lastMpuReportTime = robtimer() else: opsStats['badReadings'] += 1 if not self.commandQ.empty(): task = self.commandQ.get_nowait() logging.debug("%s: mpops task is: %s" % (time.asctime(), str(task))) self.commandQ.task_done() self.execTask(task) if task == 'Shutdown': self.processStats(opsStats) break elapsedTime = robtimer() - loopStartTime waitTime = self.minLoopTime - elapsedTime if waitTime > 0: opsStats['totalWaitTime'] += waitTime opsStats['numWaits'] += 1 else: waitTime = 0 time.sleep(waitTime) opsStats['totalLoopTime'] += robtimer() - loopStartTime self.end()
def calibrateGyro(self): """ Simple software calibration method previously used with ITG3200 eval board. A more official approach is probably available at this point. The main purpose of this approach is to cancel noise to avoid reporting small angular movement when the device is stationary. """ calx = 0.0 caly = 0.0 calz = 0.0 recordCount = 0 for i in range(100): su = self.read() if su is not None: calx += su.gyro.x caly += su.gyro.y calz += su.gyro.z recordCount += 1 # Read at ~50Hz instead of 200Hz full speed. # Just being conservative for calibration. time.sleep(0.020) # print("Gyro Calibration: z total %f/ %d readings = %f degrees/sec" % ( # calz, recordCount, calz/recordCount/Z_Convention)) if recordCount != 0: result = gyro( float(calx) / recordCount / X_Convention, float(caly) / recordCount / Y_Convention, float(calz) / recordCount / Z_Convention, 0.0) # timestamp 0.0 for calibration by convention else: result = self.zeroGyroResult # todo: Likely should raise an exception here.. # print "gyro calibration %.2f,%.2f,%.2f" % (result.x,result.y,result.z) return result
def getTestData(self): if not self.sampleFile: try: self.sampleFile = open(self.sampleFileName, 'r') except: print('error opening test data file', self.sampleFileName) raise self.sampleData = self.sampleFile.readlines( ) # assumes 'small' test data self.sampleFile.close() self.numSamples = len(self.sampleData) self.curSample = 0 if self.curSample < self.numSamples: line = self.sampleData[self.curSample] readings = line[29:].split( ',') #unforgiving - based on current log format sample = float(readings[2]) self.curSample += 1 else: sample = 30.0 logging.debug("test sample: z=%f" % (sample, )) return gyro(0, 0, sample)
def __init__( self, commandQ=None, broadcastQ=None, # todo avoid calling with positional args rangecq=None, rangebq=None, mpucq=None, mpubq=None): self.commandQ = commandQ self.broadcastQ = broadcastQ self.mpucq = mpucq self.mpubq = mpubq #self.mpucq = None #self.mpubq = None self.rangecq = rangecq self.rangebq = rangebq self.initializeDevices() self.bat = robdrivers.agmbat.Agmbat() self.mover = movepa.Movepa(self.devices['motorController']) self.startTime = robtimer() self.lastLogTime = 0 self.lastForwardRange = -1 self.forwardRange = -1 self.voltageMonitorThreshold = 10.6 self.alarmInterval = 60 self.rangeInterval = 0.5 self.lastRangeTime = 0 self.lastPowerTime = 0. self.stopPower = power(0, 0) self.lastPower = self.stopPower self.rangeNoise = 1 self.lastVoltageAlarm = robtimer() - self.alarmInterval self.lastVoltage = voltages(0., 0., 0., 0.) #self.voltageInterval = 180 self.voltageInterval = 15 #self.voltageInterval = 2 self.voltageNoise = 0.1 #self.voltageNoise = 0 self.lastVoltageTime = robtimer() - self.voltageInterval self.lastAmpsTime = 0 self.ampsInterval = 1 self.lastAmps = amperages(0, 0, "") self.rangeRules = opsrules.RangeRules() self.adjustedTask = power(0., 0.) self.lastPower = power(0., 0.) self.autoAdjust = True # False means don't adjust for range self.lastRanges = { 'Ranges': { 'Forward': 0, 'Left': 0, 'Right': 0, 'Bottom': 0, 'Back': 0, 'Deltat': 0 }, 'Timestamp': 0.0 } self.lastMpu = mpuData(gyro(0, 0, 0, 0), accel(0, 0, 0), mag(0, 0, 0), 0, 0, 0) # todo convert to json self.curHeading = 0 self.lastMpuTime = 0 self.mpuInterval = 2 logging.info("Instantiated Robot Operations") if self.commandQ: self.start()
class MPU9150_A: bus = smbus.SMBus(1) #timeout = 0 #non-blocking mode zeroGyroResult = gyro(0.0, 0.0, 0.0, 0.0) zeroAccelResult = accel(0.0, 0.0, 0.0) zeroMagResult = mag(0.0, 0.0, 0.0) # todo - validate these settings, see page 30 of register map gyroRanges = {250: 0x0, 500: 0x08, 1000: 0x10, 2000: 0x18} # def __init__(self, port=MPU9150_ADDRESS, hix=-17.9244, hiy=-15.01645): def __init__(self, port=MPU9150_ADDRESS): self.port = port # hard iron offsets in uT measured on 2019-01-28 for lbr2a # for an untested device, set the default values to # hix = 0, hiy = 0 # self.hix = hix # self.hiy = hiy self.hix = 0 self.hiy = 0 self.alpha_setting = None self.beta_setting = None self.mpu_enabled = False self.read_errors = 0 self.error_limit = 3 self.mpuPub = publisher.Publisher("MPU Message Publisher") self.gyroPub = publisher.Publisher("Gyro Publisher") self.doPublish = False # doPublish means publish even if the result 0 self.onRecord = False self.gyroRange = 250 # dps self.gyroFullScaleRange = 2000 # dps self.gyroSquelch = 0.09 self.gyroCalibration = self.zeroGyroResult self.accelRange = 0 # selects range of +/- 2g #self.magDecl = 13+55./60. # todo dynamically look up declination self.magDecl = 0. self.magResolution = 0.3 # AK8975C for MPU9150, +4095 to -4096 : +-1229uT self.asa = [0., 0., 0.] # sensitivy adjustments self.rawAngleL = [] self.lastg = self.zeroGyroResult self.lasta = self.zeroAccelResult self.lastm = self.zeroMagResult self.lastsu = mpuData(self.zeroGyroResult, self.zeroAccelResult, self.zeroMagResult, 0., 0., 0.) self.mpusu_zero = mpuData(self.zeroGyroResult, self.zeroAccelResult, self.zeroMagResult, 0., 0., 0.) self.lastRawAngle = -1 self.unitConTime = 0 self.numReads = 0 self.setup() def setup(self): try: # initialize the device here self.mpu = self.port # legacy from when it was a serial device self.mag = MAG_ADDRESS # power up the device by selecting the gyro oscilator self.bus.write_byte_data(self.mpu, POWER_MGMT_1, 0x01) # respect the 30ms startup time for the gyroscope time.sleep(0.030) # enable slave i2c mode #self.bus.write_byte_data(self.mpu, USER_CTRL, 0x20) # enable direct access mode self.bus.write_byte_data(self.mpu, USER_CTRL, 0x00) # setup the magnetometer # for slave mode: set mode to single read and do reading #self.bus.write_byte_data(self.mpu, WRITE_TO_MAG, 0x01) #self.sampleMagnetometer() # set mode to bypass for direct access to magnetometer from the host self.bus.write_byte_data(self.mpu, INT_PIN_CFG, 0x02) # get magnetometer sensitivity adjustments self.bus.write_byte_data(self.mag, MAG_CNTL, 0x0f) time.sleep(0.010) self.asa = self.bus.read_i2c_block_data(self.mag, MAG_SENS_ADJ, 3) # get hard iron calibration adjustments self.get_mag_calibration() # print(f"mag calibrations: hix={self.hix}, hiy={self.hiy}") # queue up the first sensor run self.bus.write_byte_data(self.mag, MAG_CNTL, 0x01) except: print("Unexpected error initializing MPU:", sys.exc_info()[0]) raise self.mpu_enabled = True if not self.setGyroRange(250): print("Failed to set gyro range") #todo set accel range def reset(self): self.close() self.read_errors = 0 time.sleep(2) # todo - determine a good amount of time for the reset self.setup() def sampleMagnetometer(self): """sample magnetometer in slave mode. Data is placed in EXT registers""" self.bus.write_byte_data(self.mpu, MAG, MAG_ADDRESS) self.bus.write_byte_data(self.mpu, MAG_REGISTER, MAG_CNTL) self.bus.write_byte_data(self.mpu, MAG_CTRL, 0x81) self.bus.write_byte_data(self.mpu, MAG, MAG_READ_ADDRESS) self.bus.write_byte_data(self.mpu, MAG_REGISTER, MAG_STATUS1) self.bus.write_byte_data(self.mpu, MAG_CTRL, 0x88) return def get_mag_calibration(self): cal = Calibration() self.hix, self.alpha_setting = cal.find_setting('MAG_ALPHA') self.hiy, self.beta_setting = cal.find_setting('MAG_BETA') return def adjustSensitivity(self, h): """ Adjustment for factory measured magnetometer sensitivity see https://www.akm.com/akm/en/file/datasheet/AK8975.pdf, sec 8.3.11 """ hadj = list( map(lambda x, a: x * (((a - 128.) * 0.5) / 128. + 1.), h, self.asa)) return hadj def adjustIron(self, h): """ Make hard and soft iron adjustments here. As of 2019-01-28, lbr2a required hard iron adjustments but not soft iron. """ hadj = [h[0] - self.hix, h[1] - self.hiy, h[2]] return hadj def readMagnetometer(self): """ returns new sensitivity adjusted mag reading in uT if it's ready or most recent previous reading Note that mag sensor normally operates at 8-10Hz (read cycle is 9ms), while accel and gyro can run at 200Hz """ drdy = self.bus.read_byte_data(self.mag, MAG_STATUS1) if drdy == 1: lsbraw = self.bus.read_i2c_block_data(self.mag, MAG_DATA, 7) status = lsbraw[6] if status == 0: # normal lsbrawi = [ lsbraw[i + 1] << 8 | lsbraw[i] for i in range(0, 6, 2) ] lsbraw2 = list(map(self.twos_comp, lsbrawi)) lsbadj = self.adjustSensitivity(lsbraw2) msu = [m * self.magResolution for m in lsbadj] msuadji = self.adjustIron(msu) self.lastm = mag(round(msuadji[0], 4), round(msuadji[1], 4), round(msuadji[2], 4)) else: if status & 0x04: print("Magnetic data read error") if status & 0x08: print("Magnetic sensor overflow occured") # when |X|+|Y|+|Z| > 2400uT # order some for next time if self.bus.read_byte_data(self.mag, MAG_CNTL) == 0: self.bus.write_byte_data(self.mag, MAG_CNTL, 0x01) return self.lastm def setGyroRange(self, gyroRange=250, calibrate=True): """ The gyro full scale range is controlled by bits 3 and 4 of register 1B. It is ok to write to the whole register, as the other bits are normally 0 unless a test is enabled. """ if gyroRange in self.gyroRanges: rangeCode = self.gyroRanges[gyroRange] else: return False #todo: raise an exception try: self.bus.write_byte_data(self.mpu, GYRO_CONFIG, rangeCode) self.gyroRange = gyroRange if calibrate: self.gyroCalibration = self.calibrateGyro() except: print("Unexpected error:", sys.exc_info()[0]) raise return True def read(self): """ Ask for accelerometer, temperature, gyro, as a burst. While the accel, temp and gyro registers are in big endian order, the mag data is in little endian. Magnetometer reading has been delegated to separate function since the data isn't ready as often as the others. (The MPU EXT registers are still read as they would have been in slave mode, but the data will not be valid. Instead, the mag data is collected directly from the magnetometer. Leaving this in makes changing back to slave mode easier, should the need arise.) """ if not self.mpu_enabled: return self.mpusu_zero if self.read_errors > self.error_limit: print("Resetting MPU due to excessive read errors") self.reset() return self.mpusu_zero try: # self.sampleMagnetometer() # only in slave mode regs = self.bus.read_i2c_block_data(self.mpu, READINGS_START_REG, READINGS_LEN) except IOError: self.read_errors += 1 print("Unexpected MPU read error:", sys.exc_info()[0]) print("\treturning 0 result") return self.mpusu_zero # raise t = robtimer() lsb = tuple() # accelerometer for i in range(0, 6, 2): lsb += (self.twos_comp(regs[i] << 8 | regs[i + 1]), ) # temperature lsb += (self.twos_comp(regs[6] << 8 | regs[7]), ) # gyro for i in range(8, 14, 2): lsb += (self.twos_comp(regs[i] << 8 | regs[i + 1]), ) lsb += (t, ) mpusu = self.lsb2su(lsb) self.mpuPub.publish(mpusu) self.gyroPub.publish( mpusu.gyro) # publish separately for legacy reasons.. self.lastsu = mpusu self.numReads += 1 return mpusu def twos_comp(self, val, bits=16): """compute the 2's complement of int value val""" if (val & (1 << (bits - 1))) != 0: # if sign bit is set e.g., 8bit: 128-255 val = val - (1 << bits) # compute negative value return val def lsb2su(self, lsb): """ local sensor bus (LSB) to standard units (SI) lsb format: (accelx,accely,accelz,temp,gyrox,gyroy,gyroz,compx,compy,compz) conversion formulas from InvenSense UI Data Logger App Note, MPU9150 Register Map and Product Spec, and for magnetometer: https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial """ #print lsb t0 = robtimer() t = lsb[-1] # accelerometer in g aR = self.accelRange aR = 1.0 # print("accel lsb: x=%d, y=%d, z=%d" % (lsb[0], lsb[1], lsb[2])) a = accel(round(float(lsb[0] / 16384.) * aR, 4), round(float(lsb[1] / 16384.) * aR, 4), round(float(lsb[2] / 16384.) * aR, 4)) # temperature in C temperature = round((lsb[3] / 340. + 35), 1) # gyro in deg/sec gR = self.gyroRange gR = 1 calx = self.gyroCalibration.x caly = self.gyroCalibration.y calz = self.gyroCalibration.z gL = [(float(lsb[4] / 131.) * gR - calx) * X_Convention, (float(lsb[5] / 131.) * gR - caly) * Y_Convention, (float(lsb[6] / 131.) * gR - calz) * Z_Convention] # further squelch noise for i in range(len(gL)): if abs(gL[i]) < self.gyroSquelch: gL[i] = 0.0 g = gyro(round(gL[0], 3), round(gL[1], 3), round(gL[2], 3), round(t, 4)) # merge in the magentometer results # magnetometer function already returns mag uT m = self.zeroMagResult try: m = self.readMagnetometer() except Exception: self.read_errors += 1 print("Magnetometer exception", sys.exc_info()[0]) ''' # this has to be re-worked since readMagnetometer() #todo: better error handling for a.y, a.x domains if a.y <= 1.0: rollRadians = asin(a.y) else: rollRadians = asin(1.0) if a.x <= 1.0: pitchRadians = asin(a.x) else: pitchRadians = asin(1.0) # algorithm good for tilt <= 40 degrees, ignore tilt otherwise for now if abs(rollRadians) > 0.78 or abs(pitchRadians) > 0.78: m.x = self.magResolution * lsb[8] # appears x-y swapped m.y = self.magResolution * lsb[7] else: cosRoll = cos(rollRadians) sinRoll = sin(rollRadians) cosPitch = cos(pitchRadians) sinPitch = sin(pitchRadians) magx = self.magResolution * lsb[7] # may need to swap x-y, investigate magy = self.magResolution * lsb[8] magz = self.magResolution * lsb[9] * -1 # to align with accelerometer cx = magx * cosPitch + magz * sinPitch cy = magx * sinRoll * sinPitch + magy * cosRoll - \ magz * sinRoll * cosPitch if cy > 0: heading = 90. - atan(cy/cx) * 180./pi elif cy < 0: heading = 270. - atan(cy/cx) * 180./pi elif cy == 0: if cx < 0: heading = 180. else: heading = 0. else: heading = -1 ''' # This version of the algorithm does not tilt compensate heading = -1 if m.y == 0: if m.x <= 0: heading = 0. elif m.x > 0: heading = 180. else: rawAngle = atan(m.x / m.y) * 180. / pi #print rawAngle if m.y <= 0: heading = round((270. + rawAngle), 0) else: heading = round((90. + rawAngle), 0) #print 'heading: %.0f' % heading su = mpuData(g, a, m, round(heading, 2), temperature, round(t, 4)) deltat = robtimer() - t0 self.unitConTime += deltat return su def calibrateGyro(self): """ Simple software calibration method previously used with ITG3200 eval board. A more official approach is probably available at this point. The main purpose of this approach is to cancel noise to avoid reporting small angular movement when the device is stationary. """ calx = 0.0 caly = 0.0 calz = 0.0 recordCount = 0 for i in range(100): su = self.read() if su is not None: calx += su.gyro.x caly += su.gyro.y calz += su.gyro.z recordCount += 1 # Read at ~50Hz instead of 200Hz full speed. # Just being conservative for calibration. time.sleep(0.020) # print("Gyro Calibration: z total %f/ %d readings = %f degrees/sec" % ( # calz, recordCount, calz/recordCount/Z_Convention)) if recordCount != 0: result = gyro( float(calx) / recordCount / X_Convention, float(caly) / recordCount / Y_Convention, float(calz) / recordCount / Z_Convention, 0.0) # timestamp 0.0 for calibration by convention else: result = self.zeroGyroResult # todo: Likely should raise an exception here.. # print "gyro calibration %.2f,%.2f,%.2f" % (result.x,result.y,result.z) return result def calibrateMag(self, samples=500, source=None): """ Execute calibration procedure to calculate the hard (and eventually soft) iron adjustments. Note that the robot must be in a safe location for spinning around it's Z axis in order to collect the data. :param: samples - the number of magnetometer readings to collect :param: source - Collect new samples if None, otherwise read samples from path given by source. :return: todo - simplify using numpy arrays """ if samples > 1000: print(f"Samples limited to 1000 instead of {samples}") samples = 1000 cal_data = [] x = None y = None if source is None or source == '-': self.hix = 0. self.hiy = 0. print( "Collecting magnetometer calibration data. Robot should be spinning about its z axis." ) for r in range(samples): cal_data.append(self.read()) time.sleep(0.150) print("Calibration data collected.") print(f"Example: {str(cal_data[-1])}") x = [r.mag.x for r in cal_data] y = [r.mag.y for r in cal_data] try: self.save_cal_data(cal_data) except Exception as e: print(f"Error saving magnetometer samples:\n{str(e)}") return try: if source is not None and source != '-': source_path = os.path.join(LOG_DIR, source) else: source_path = None alpha, beta, x_corrected, y_corrected = calc_mag_correction( x, y, source_path) self.hix = alpha self.hiy = beta if self.alpha_setting is None: self.alpha_setting = CalibrationSetting( robot_id, 'MAG_ALPHA', alpha) else: self.alpha_setting.value = alpha self.alpha_setting.save() if self.beta_setting is None: self.beta_setting = CalibrationSetting(robot_id, 'MAG_BETA', beta) else: self.beta_setting.value = beta self.beta_setting.save() self.save_corrected(x_corrected, y_corrected) print( f"Completed hard iron calibration with alpha {alpha}, beta {beta}" ) except Exception as calexception: print(f"Error calibrating magnetometer:\n{str(calexception)}") return def save_cal_data(self, cal_data): with open(magCalibrationLogFile, 'w') as f: print("X,Y,Z,Heading", file=f) for r in cal_data: print(f"{r.mag.x},{r.mag.y},{r.mag.z},{r.heading}", file=f) def save_corrected(self, x, y): with open(magCalibrationLogFile + '-corrected', 'w') as f: print("X-Corrected,Y-Corrected", file=f) for i in range(len(x)): print(f"{x[i]},{y[i]}", file=f) def close(self): self.mpu_enabled = False try: # self.mpu.close() # for the i2c version of the driver, interpret close as reset mpu self.bus.write_byte_data(self.mpu, POWER_MGMT_1, 0x80) self.mpuPub.publish(time.asctime() + " InvenSense MPU Closed") except: msg = time.asctime() + " Error Closing Controller" self.mpuPub.publish(msg) print(msg)
def lsb2su(self, lsb): """ local sensor bus (LSB) to standard units (SI) lsb format: (accelx,accely,accelz,temp,gyrox,gyroy,gyroz,compx,compy,compz) conversion formulas from InvenSense UI Data Logger App Note, MPU9150 Register Map and Product Spec, and for magnetometer: https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial """ #print lsb t0 = robtimer() t = lsb[-1] # accelerometer in g aR = self.accelRange aR = 1.0 # print("accel lsb: x=%d, y=%d, z=%d" % (lsb[0], lsb[1], lsb[2])) a = accel(round(float(lsb[0] / 16384.) * aR, 4), round(float(lsb[1] / 16384.) * aR, 4), round(float(lsb[2] / 16384.) * aR, 4)) # temperature in C temperature = round((lsb[3] / 340. + 35), 1) # gyro in deg/sec gR = self.gyroRange gR = 1 calx = self.gyroCalibration.x caly = self.gyroCalibration.y calz = self.gyroCalibration.z gL = [(float(lsb[4] / 131.) * gR - calx) * X_Convention, (float(lsb[5] / 131.) * gR - caly) * Y_Convention, (float(lsb[6] / 131.) * gR - calz) * Z_Convention] # further squelch noise for i in range(len(gL)): if abs(gL[i]) < self.gyroSquelch: gL[i] = 0.0 g = gyro(round(gL[0], 3), round(gL[1], 3), round(gL[2], 3), round(t, 4)) # merge in the magentometer results # magnetometer function already returns mag uT m = self.zeroMagResult try: m = self.readMagnetometer() except Exception: self.read_errors += 1 print("Magnetometer exception", sys.exc_info()[0]) ''' # this has to be re-worked since readMagnetometer() #todo: better error handling for a.y, a.x domains if a.y <= 1.0: rollRadians = asin(a.y) else: rollRadians = asin(1.0) if a.x <= 1.0: pitchRadians = asin(a.x) else: pitchRadians = asin(1.0) # algorithm good for tilt <= 40 degrees, ignore tilt otherwise for now if abs(rollRadians) > 0.78 or abs(pitchRadians) > 0.78: m.x = self.magResolution * lsb[8] # appears x-y swapped m.y = self.magResolution * lsb[7] else: cosRoll = cos(rollRadians) sinRoll = sin(rollRadians) cosPitch = cos(pitchRadians) sinPitch = sin(pitchRadians) magx = self.magResolution * lsb[7] # may need to swap x-y, investigate magy = self.magResolution * lsb[8] magz = self.magResolution * lsb[9] * -1 # to align with accelerometer cx = magx * cosPitch + magz * sinPitch cy = magx * sinRoll * sinPitch + magy * cosRoll - \ magz * sinRoll * cosPitch if cy > 0: heading = 90. - atan(cy/cx) * 180./pi elif cy < 0: heading = 270. - atan(cy/cx) * 180./pi elif cy == 0: if cx < 0: heading = 180. else: heading = 0. else: heading = -1 ''' # This version of the algorithm does not tilt compensate heading = -1 if m.y == 0: if m.x <= 0: heading = 0. elif m.x > 0: heading = 180. else: rawAngle = atan(m.x / m.y) * 180. / pi #print rawAngle if m.y <= 0: heading = round((270. + rawAngle), 0) else: heading = round((90. + rawAngle), 0) #print 'heading: %.0f' % heading su = mpuData(g, a, m, round(heading, 2), temperature, round(t, 4)) deltat = robtimer() - t0 self.unitConTime += deltat return su