def _mag_callback(self):
'''
Update magnetometer Vector3d object (if data available)
'''
try: # If read fails, returns last valid data and
self._read(self.buf1, 0x02,
self._mag_addr) # increments mag_stale_count
if self.buf1[0] & 1 == 0:
return self._mag # Data not ready: return last value
self._read(self.buf6, 0x03, self._mag_addr)
self._read(self.buf1, 0x09, self._mag_addr)
except OSError:
raise MPUException(self._I2Cerror)
if self.buf1[0] & 0x08 > 0: # An overflow has occurred
self._mag_stale_count += 1 # Error conditions retain last good value
return # user should check for ever increasing stale_counts
self._mag._ivector[1] = bytes_toint(
self.buf6[1],
self.buf6[0]) # Note axis twiddling and little endian
self._mag._ivector[0] = bytes_toint(self.buf6[3], self.buf6[2])
self._mag._ivector[2] = -bytes_toint(self.buf6[5], self.buf6[4])
scale = 0.15 # scale is 0.15uT/LSB
self._mag._vector[
0] = self._mag._ivector[0] * self.mag_correction[0] * scale
self._mag._vector[
1] = self._mag._ivector[1] * self.mag_correction[1] * scale
self._mag._vector[
2] = self._mag._ivector[2] * self.mag_correction[2] * scale
self._mag_stale_count = 0
def _mag_callback(self):
'''
Update magnetometer Vector3d object (if data available)
'''
try: # If read fails, returns last valid data
if self.mag_ready: # Starts mag if necessary
self._read(self.buf6, 0x03, self._mag_addr)
self.mag_triggered = False
else:
self._mag_stale_count += 1 # Data not ready: retain last value
return # but increment stale count
self._read(self.buf6, 0x03, self._mag_addr) # Mag was ready
self._read(self.buf1, 0x09, self._mag_addr) # Read ST2
except OSError:
self.mag_triggered = False
raise MPUException(self._I2Cerror)
if self.buf1[0] & 0x0C > 0: # An overflow or data error has occurred
self._mag_stale_count += 1 # transitory condition? User checks stale count.
return
self._mag._ivector[1] = bytes_toint(
self.buf6[1],
self.buf6[0]) # Note axis twiddling and little endian
self._mag._ivector[0] = bytes_toint(self.buf6[3], self.buf6[2])
self._mag._ivector[2] = -bytes_toint(self.buf6[5], self.buf6[4])
scale = 0.3 # 0.3uT/LSB
self._mag._vector[
0] = self._mag._ivector[0] * self.mag_correction[0] * scale
self._mag._vector[
1] = self._mag._ivector[1] * self.mag_correction[1] * scale
self._mag._vector[
2] = self._mag._ivector[2] * self.mag_correction[2] * scale
self._mag_stale_count = 0
def mag_ready(
self): # Initiates a reading if necessary. Returns ready state.
self.mag_trigger()
try:
self._read(self.buf1, 0x02, self._mag_addr)
except OSError:
raise MPUException(self._I2Cerror)
return bool(self.buf1[0] & 1)
def mag_trigger(self): # Initiate a mag reading. Can be called repeatedly.
if not self.mag_triggered:
try:
self._write(0x01, 0x0A,
self._mag_addr) # single measurement mode
except OSError:
raise MPUException(self._I2Cerror)
self.mag_triggered = True
def temperature(self):
'''
Returns the temperature in degree C.
'''
try:
self._read(self.buf2, 0x41, self.mpu_addr)
except OSError:
raise MPUException(self._I2Cerror)
return bytes_toint(self.buf2[0], self.buf2[1]) / 340 + 35 # I think
def filter_range(self, filt):
# set range
if filt in range(7):
try:
self._write(filt, 0x1A, self.mpu_addr)
except OSError:
raise MPUException(self._I2Cerror)
else:
raise ValueError('Filter coefficient must be between 0 and 6')
def filter_range(self):
'''
Returns the gyro and temperature sensor low pass filter cutoff frequency
Pass: 0 1 2 3 4 5 6
Cutoff (Hz): 250 184 92 41 20 10 5
Sample rate (KHz): 8 1 1 1 1 1 1
'''
try:
self._read(self.buf1, 0x1A, self.mpu_addr)
res = self.buf1[0] & 7
except OSError:
raise MPUException(self._I2Cerror)
return res
def accel_filter_range(self):
'''
Returns the accel low pass filter cutoff frequency
Pass: 0 1 2 3 4 5 6 7 BEWARE 7 doesn't work on device I tried.
Cutoff (Hz): 460 184 92 41 20 10 5 460
Sample rate (KHz): 1 1 1 1 1 1 1 1
'''
try:
self._read(self.buf1, 0x1D, self.mpu_addr)
res = self.buf1[0] & 7
except OSError:
raise MPUException(self._I2Cerror)
return res
def gyro_filter_range(self, filt):
'''
Sets the gyro and temperature sensor low pass filter cutoff frequency
Pass: 0 1 2 3 4 5 6 7
Cutoff (Hz): 250 184 92 41 20 10 5 3600
Sample rate (KHz): 8 1 1 1 1 1 1 8
'''
if filt in range(8):
try:
self._write(filt, 0x1A, self.mpu_addr)
except OSError:
raise MPUException(self._I2Cerror)
else:
raise ValueError('Filter coefficient must be between 0 and 7')
def accel_filter_range(self, filt):
'''
Sets the accel low pass filter cutoff frequency
Pass: 0 1 2 3 4 5 6 7 BEWARE 7 doesn't work on device I tried.
Cutoff (Hz): 460 184 92 41 20 10 5 460
Sample rate (KHz): 1 1 1 1 1 1 1 1
'''
if filt in range(8):
try:
self._write(filt, 0x1D, self.mpu_addr)
except OSError:
raise MPUException(self._I2Cerror)
else:
raise ValueError('Filter coefficient must be between 0 and 7')
def _magsetup(self):
'''
Read magnetometer correction values from ROM. Perform the maths as decribed
on page 59 of register map and store the results.
'''
try:
self._write(0x0F, 0x0A, self._mag_addr)
self._read(self.buf3, 0x10, self._mag_addr)
self._write(0, 0x0A, self._mag_addr) # Power down mode
except OSError:
raise MPUException(self._I2Cerror)
mag_x = (0.5 * (self.buf3[0] - 128)) / 128 + 1
mag_y = (0.5 * (self.buf3[1] - 128)) / 128 + 1
mag_z = (0.5 * (self.buf3[2] - 128)) / 128 + 1
return (mag_x, mag_y, mag_z)
def _magsetup(self): # mode 2 100Hz continuous reads, 16 bit
'''
Magnetometer initialisation: use 16 bit continuous mode.
Mode 1 is 8Hz mode 2 is 100Hz repetition
returns correction values
'''
try:
self._write(0x0F, 0x0A, self._mag_addr) # fuse ROM access mode
self._read(self.buf3, 0x10, self._mag_addr) # Correction values
self._write(0, 0x0A, self._mag_addr) # Power down mode (AK8963 manual 6.4.6)
self._write(0x16, 0x0A, self._mag_addr) # 16 bit (0.15uT/LSB not 0.015), mode 2
except OSError:
raise MPUException(self._I2Cerror)
mag_x = (0.5*(self.buf3[0] - 128))/128 + 1
mag_y = (0.5*(self.buf3[1] - 128))/128 + 1
mag_z = (0.5*(self.buf3[2] - 128))/128 + 1
return (mag_x, mag_y, mag_z)
