Code: Select all
from BNO055 import BNO055
b=BNO055()
import pyb
import micropython
micropython.alloc_emergency_exception_buf(200)
# Turn on LED to indicate logging
pyb.LED(2).on()
class T_4(object):
def __init__(self):
# Initialise timer 4 and ADC
self.status = 'Sample'
self.tim = pyb.Timer(4)
self.tim.init(freq=1000)
self.set_start()
self.adc = pyb.ADC('X19')
self.buf_index = 0
self.buf_0 = [[0, 0] for i in range(2000)]
self.tim.callback(self.t_4_callback)
def set_start(self):
# Remember time of start
self.t_start = pyb.micros()
def store_data(self):
# Open log file on SD-card
f = open('/flash/log_1.csv', 'w')
# Store buffer data to log file
for i in range(len(self.buf_0)):
t = self.buf_0[i][0]
d = self.buf_0[i][1]
value_string = '%i, %i;' %(t, d)
f.write(value_string)
# Close file and blink LED
f.close()
pyb.LED(2).off()
pyb.LED(3).on()
pyb.LED(3).off()
def t_4_callback(self, tim):
# Read ADC value and current time
a=b.read_euler()
t = pyb.micros()
# Add value and time to buffer
self.buf_0[self.buf_index][0] = t
self.buf_0[self.buf_index][1] = a[2]
# Increment buffer index until buffer is filled,
# then disable interrupt and change status
self.buf_index += 1
if (self.buf_index >= len(self.buf_0)):
self.tim.callback(None)
self.status = 'Store'Traceback (most recent call last):
File "BNO055.py", line 510, in _read_vector
File "BNO055.py", line 266, in _read_bytes
MemoryError: memory allocation failed, heap is locked "
here the BNO055 code that works
Code: Select all
from pyb import I2C
import pyb
class BNO055(object):
def __init__(self, I2Cport=2):
self.tim = pyb.Timer(4)
self.tim.init(freq=1000)
# If reset pin is provided save it and a reference to provided GPIO
# bus (or the default system GPIO bus if none is provided).
if I2Cport != 1 and I2Cport != 2:
print('Erreur: port I2C ', I2Cport, ' unknown')
return
self.i2c = I2C(I2Cport,I2C.MASTER)
# Look for BNO on I2C bus
i2cAddrList = self.i2c.scan() # I2C address list for that bus
if 0x28 in i2cAddrList:
self.addr = 0x28
print('BNO055 found at address: ',0x28)
elif 0x29 in i2cAddrList:
self.addr = 0x29
print('BNO055 found at address: ',0x29)
else:
print('Error: No BNO055 found')
return
# Save the desired normal operation mode.
self._mode = OPERATION_MODE_NDOF
# First send a thow-away command and ignore any response or I2C errors
# just to make sure the BNO is in a good state and ready to accept
# commands (this seems to be necessary after a hard power down).
try:
self._write_byte(BNO055_PAGE_ID_ADDR, 0)
except IOError:
# Swallow an IOError that might be raised by an I2C issue. Only do
# this for this very first command to help get the BNO and board's
# I2C into a clear state ready to accept the next commands.
pass
# Make sure we're in config mode and on page 0.
self._config_mode()
self._write_byte(BNO055_PAGE_ID_ADDR, 0)
# Check the chip ID
bno_id = self._read_byte(BNO055_CHIP_ID_ADDR)
# logger.debug('Read chip ID: 0x{0:02X}'.format(bno_id))
if bno_id != BNO055_ID:
return False
# Reset the device.
# Use the reset command.
self._write_byte(BNO055_SYS_TRIGGER_ADDR, 0x20)
# Wait 650 ms after reset for chip to be ready (as suggested
# in datasheet).
pyb.delay(650)
# Set to normal power mode.
self._write_byte(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL)
# Default to internal oscillator.
self._write_byte(BNO055_SYS_TRIGGER_ADDR, 0x0)
# Enter normal operation mode.
self._operation_mode()
print(True)
def _write_bytes(self, address, data):
# Write a list of 8-bit values starting at the provided register address.
#self._i2c_device.writeList(address, data)
self.i2c.mem_write(data, self.addr, address)
def _write_byte(self, address, value):
# Write an 8-bit value to the provided register address.
#self._i2c_device.write8(address, value)
a = bytearray(1)
a[0] = value & 0xff
self.i2c.mem_write(a, self.addr, address)
def _read_bytes(self, address, length):
# Read a number of unsigned byte values starting from the provided address.
#return bytearray(self._i2c_device.readList(address, length))
return self.i2c.mem_read(length, self.addr, address)
def _read_byte(self, address):
# Read an 8-bit unsigned value from the provided register address.
#return self._i2c_device.readU8(address)
res = bytearray(1)
self.i2c.mem_read(res,self.addr,address)
return res[0]
def _read_signed_byte(self, address):
# Read an 8-bit signed value from the provided register address.
#data = self._read_byte(address)
res = bytearray(1)
self.i2c.mem_read(res,self.addr,address)
if res[0] > 127:
return res[0] - 256
else:
return res[0]
def _config_mode(self):
# Enter configuration mode.
self.set_mode(OPERATION_MODE_CONFIG)
def _operation_mode(self):
# Enter operation mode to read sensor data.
self.set_mode(self._mode)
def set_mode(self, mode):
"""Set operation mode for BNO055 sensor. Mode should be a value from
table 3-3 and 3-5 of the datasheet:
http://www.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf
"""
self._write_byte(BNO055_OPR_MODE_ADDR, mode & 0xFF)
# Delay for 30 milliseconds (datsheet recommends 19ms, but a little more
# can't hurt and the kernel is going to spend some unknown amount of time
# too).
pyb.delay(30)
def get_revision(self):
"""Return a tuple with revision information about the BNO055 chip. Will
return 5 values:
- Software revision
- Bootloader version
- Accelerometer ID
- Magnetometer ID
- Gyro ID
"""
# Read revision values.
accel = self._read_byte(BNO055_ACCEL_REV_ID_ADDR)
mag = self._read_byte(BNO055_MAG_REV_ID_ADDR)
gyro = self._read_byte(BNO055_GYRO_REV_ID_ADDR)
bl = self._read_byte(BNO055_BL_REV_ID_ADDR)
sw_lsb = self._read_byte(BNO055_SW_REV_ID_LSB_ADDR)
sw_msb = self._read_byte(BNO055_SW_REV_ID_MSB_ADDR)
sw = ((sw_msb << 8) | sw_lsb) & 0xFFFF
# Return the results as a tuple of all 5 values.
return (sw, bl, accel, mag, gyro)
def set_external_crystal(self, external_crystal):
"""Set if an external crystal is being used by passing True, otherwise
use the internal oscillator by passing False (the default behavior).
"""
# Switch to configuration mode.
self._config_mode()
# Set the clock bit appropriately in the SYS_TRIGGER register.
if external_crystal:
self._write_byte(BNO055_SYS_TRIGGER_ADDR, 0x80)
else:
self._write_byte(BNO055_SYS_TRIGGER_ADDR, 0x00)
# Go back to normal operation mode.
self._operation_mode()
def get_system_status(self, run_self_test=True):
"""Return a tuple with status information. Three values will be returned:
- System status register value with the following meaning:
0 = Idle
1 = System Error
2 = Initializing Peripherals
3 = System Initialization
4 = Executing Self-Test
5 = Sensor fusion algorithm running
6 = System running without fusion algorithms
- Self test result register value with the following meaning:
Bit value: 1 = test passed, 0 = test failed
Bit 0 = Accelerometer self test
Bit 1 = Magnetometer self test
Bit 2 = Gyroscope self test
Bit 3 = MCU self test
Value of 0x0F = all good!
- System error register value with the following meaning:
0 = No error
1 = Peripheral initialization error
2 = System initialization error
3 = Self test result failed
4 = Register map value out of range
5 = Register map address out of range
6 = Register map write error
7 = BNO low power mode not available for selected operation mode
8 = Accelerometer power mode not available
9 = Fusion algorithm configuration error
10 = Sensor configuration error
If run_self_test is passed in as False then no self test is performed and
None will be returned for the self test result. Note that running a
self test requires going into config mode which will stop the fusion
engine from running.
"""
self_test = None
if run_self_test:
# Switch to configuration mode if running self test.
self._config_mode()
# Perform a self test.
sys_trigger = self._read_byte(BNO055_SYS_TRIGGER_ADDR)
self._write_byte(BNO055_SYS_TRIGGER_ADDR, sys_trigger | 0x1)
# Wait for self test to finish.
pyb.delay(1000)
# Read test result.
self_test = self._read_byte(BNO055_SELFTEST_RESULT_ADDR)
# Go back to operation mode.
self._operation_mode()
# Now read status and error registers.
status = self._read_byte(BNO055_SYS_STAT_ADDR)
error = self._read_byte(BNO055_SYS_ERR_ADDR)
# Return the results as a tuple of all 3 values.
return (status, self_test, error)
def get_calibration_status(self):
"""Read the calibration status of the sensors and return a 4 tuple with
calibration status as follows:
- System, 3=fully calibrated, 0=not calibrated
- Gyroscope, 3=fully calibrated, 0=not calibrated
- Accelerometer, 3=fully calibrated, 0=not calibrated
- Magnetometer, 3=fully calibrated, 0=not calibrated
"""
# Return the calibration status register value.
cal_status = self._read_byte(BNO055_CALIB_STAT_ADDR)
sys = (cal_status >> 6) & 0x03
gyro = (cal_status >> 4) & 0x03
accel = (cal_status >> 2) & 0x03
mag = cal_status & 0x03
# Return the results as a tuple of all 3 values.
return (sys, gyro, accel, mag)
def get_calibration(self):
"""Return the sensor's calibration data and return it as an array of
22 bytes. Can be saved and then reloaded with the set_calibration function
to quickly calibrate from a previously calculated set of calibration data.
"""
# Switch to configuration mode, as mentioned in section 3.10.4 of datasheet.
self._config_mode()
# Read the 22 bytes of calibration data and convert it to a list (from
# a bytearray) so it's more easily serialized should the caller want to
# store it.
cal_data = list(self._read_bytes(ACCEL_OFFSET_X_LSB_ADDR, 22))
# Go back to normal operation mode.
self._operation_mode()
return cal_data
def set_calibration(self, data):
"""Set the sensor's calibration data using a list of 22 bytes that
represent the sensor offsets and calibration data. This data should be
a value that was previously retrieved with get_calibration (and then
perhaps persisted to disk or other location until needed again).
"""
# Check that 22 bytes were passed in with calibration data.
if data is None or len(data) != 22:
raise ValueError('Expected a list of 22 bytes for calibration data.')
# Switch to configuration mode, as mentioned in section 3.10.4 of datasheet.
self._config_mode()
# Set the 22 bytes of calibration data.
self._write_bytes(ACCEL_OFFSET_X_LSB_ADDR, data)
# Go back to normal operation mode.
self._operation_mode()
def get_axis_remap(self):
"""Return a tuple with the axis remap register values. This will return
6 values with the following meaning:
- X axis remap (a value of AXIS_REMAP_X, AXIS_REMAP_Y, or AXIS_REMAP_Z.
which indicates that the physical X axis of the chip
is remapped to a different axis)
- Y axis remap (see above)
- Z axis remap (see above)
- X axis sign (a value of AXIS_REMAP_POSITIVE or AXIS_REMAP_NEGATIVE
which indicates if the X axis values should be positive/
normal or negative/inverted. The default is positive.)
- Y axis sign (see above)
- Z axis sign (see above)
Note that by default the axis orientation of the BNO chip looks like
the following (taken from section 3.4, page 24 of the datasheet). Notice
the dot in the corner that corresponds to the dot on the BNO chip:
| Z axis
|
| / X axis
____|__/____
Y axis / * | / /|
_________ /______|/ //
/___________ //
|____________|/
"""
# Get the axis remap register value.
map_config = self._read_byte(BNO055_AXIS_MAP_CONFIG_ADDR)
z = (map_config >> 4) & 0x03
y = (map_config >> 2) & 0x03
x = map_config & 0x03
# Get the axis remap sign register value.
sign_config = self._read_byte(BNO055_AXIS_MAP_SIGN_ADDR)
x_sign = (sign_config >> 2) & 0x01
y_sign = (sign_config >> 1) & 0x01
z_sign = sign_config & 0x01
# Return the results as a tuple of all 3 values.
return (x, y, z, x_sign, y_sign, z_sign)
def set_axis_remap(self, x, y, z,
x_sign=AXIS_REMAP_POSITIVE, y_sign=AXIS_REMAP_POSITIVE,
z_sign=AXIS_REMAP_POSITIVE):
"""Set axis remap for each axis. The x, y, z parameter values should
be set to one of AXIS_REMAP_X, AXIS_REMAP_Y, or AXIS_REMAP_Z and will
change the BNO's axis to represent another axis. Note that two axises
cannot be mapped to the same axis, so the x, y, z params should be a
unique combination of AXIS_REMAP_X, AXIS_REMAP_Y, AXIS_REMAP_Z values.
The x_sign, y_sign, z_sign values represent if the axis should be positive
or negative (inverted).
See the get_axis_remap documentation for information on the orientation
of the axises on the chip, and consult section 3.4 of the datasheet.
"""
# Switch to configuration mode.
self._config_mode()
# Set the axis remap register value.
map_config = 0x00
map_config |= (z & 0x03) << 4
map_config |= (y & 0x03) << 2
map_config |= x & 0x03
self._write_byte(BNO055_AXIS_MAP_CONFIG_ADDR, map_config)
# Set the axis remap sign register value.
sign_config = 0x00
sign_config |= (x_sign & 0x01) << 2
sign_config |= (y_sign & 0x01) << 1
sign_config |= z_sign & 0x01
self._write_byte(BNO055_AXIS_MAP_SIGN_ADDR, sign_config)
# Go back to normal operation mode.
self._operation_mode()
def _read_vector(self, address, count=3):
# Read count number of 16-bit signed values starting from the provided
# address. Returns a tuple of the values that were read.
data = self._read_bytes(address, count*2)
result = [0]*count
for i in range(count):
result[i] = ((data[i*2+1] << 8) | data[i*2]) & 0xFFFF
if result[i] > 32767:
result[i] -= 65536
return result
def read_euler(self):
"""Return the current absolute orientation as a tuple of heading, roll,
and pitch euler angles in degrees.
"""
heading, roll, pitch = self._read_vector(BNO055_EULER_H_LSB_ADDR)
return [heading/16.0, roll/16.0, pitch/16.0]
def read_magnetometer(self):
"""Return the current magnetometer reading as a tuple of X, Y, Z values
in micro-Teslas.
"""
x, y, z = self._read_vector(BNO055_MAG_DATA_X_LSB_ADDR)
return (x/16.0, y/16.0, z/16.0)
def read_gyroscope(self):
"""Return the current gyroscope (angular velocity) reading as a tuple of
X, Y, Z values in degrees per second.
"""
x, y, z = self._read_vector(BNO055_GYRO_DATA_X_LSB_ADDR)
return (x/900.0, y/900.0, z/900.0)
def read_accelerometer(self):
"""Return the current accelerometer reading as a tuple of X, Y, Z values
in meters/second^2.
"""
x, y, z = self._read_vector(BNO055_ACCEL_DATA_X_LSB_ADDR)
return (x/100.0, y/100.0, z/100.0)
def read_linear_acceleration(self):
"""Return the current linear acceleration (acceleration from movement,
not from gravity) reading as a tuple of X, Y, Z values in meters/second^2.
"""
x, y, z = self._read_vector(BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR)
return (x/100.0, y/100.0, z/100.0)
def read_gravity(self):
"""Return the current gravity acceleration reading as a tuple of X, Y, Z
values in meters/second^2.
"""
x, y, z = self._read_vector(BNO055_GRAVITY_DATA_X_LSB_ADDR)
return (x/100.0, y/100.0, z/100.0)
def read_quaternion(self):
"""Return the current orientation as a tuple of X, Y, Z, W quaternion
values.
"""
w, x, y, z = self._read_vector(BNO055_QUATERNION_DATA_W_LSB_ADDR, 4)
# Scale values, see 3.6.5.5 in the datasheet.
scale = (1.0 / (1<<14))
return (x*scale, y*scale, z*scale, w*scale)
def read_temp(self):
"""Return the current temperature in Celsius."""
return self._read_signed_byte(BNO055_TEMP_ADDR)if anyone could try to explain to me just how to debug the code because I would need this data to draw graphs.
thanks a lot
