recording data from a BNO055 with a pyboard

General discussions and questions abound development of code with MicroPython that is not hardware specific.
Target audience: MicroPython Users.
Post Reply
lucas952
Posts: 3
Joined: Fri Nov 01, 2019 11:42 am
Location: france

recording data from a BNO055 with a pyboard

Post by lucas952 » Fri Nov 01, 2019 12:06 pm

Hello, i'm trying to record data from a adafruit bno055. Only I am novice and do not understand all the subtleties of micropython, the following code borrowed here "viewtopic.php?t=6045"

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'
works very well to record the data of a PIN but when it comes to take the data of the BNO055 it does not works more and returns "uncaught exception in Timer (4) interrupt handler
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

User avatar
pythoncoder
Posts: 3724
Joined: Fri Jul 18, 2014 8:01 am
Location: UK
Contact:

Re: recording data from a BNO055 with a pyboard

Post by pythoncoder » Sat Nov 02, 2019 8:12 am

There are restrictions on what you can do in an interrupt handler (such as a timer callback). These are documented here.

There are various solutions but in your case I would abandon the timer. As far as I can see you can abandon the adc which you don't seem to be using. Simply write a loop which periodically reads the Euler angles and puts them in the buffer. Between readings it pauses for the correct interval.

I'm not sure what driver you're using but there is one which is written for MicroPython here. You could adapt my usage example in that doc. If you read further you'll see that reading Euler angles at 1KHz is pointless as that far exceeds the rate at which the BNO055 can compute them.
Peter Hinch

lucas952
Posts: 3
Joined: Fri Nov 01, 2019 11:42 am
Location: france

Re: recording data from a BNO055 with a pyboard

Post by lucas952 » Sat Nov 02, 2019 2:15 pm

thank you I will try this

lucas952
Posts: 3
Joined: Fri Nov 01, 2019 11:42 am
Location: france

Re: recording data from a BNO055 with a pyboard

Post by lucas952 » Sat Nov 02, 2019 2:41 pm

I've tried this more simple code

Code: Select all

import pyb
import utime

from BNO055 import BNO055

b=BNO055()

buf_index = 0
buf_0 = [[i, 0] for i in range(2000)]
t1=utime.ticks_us()

f = open('/sd/log_1.csv', 'w')

for i in range(len(buf_0)):
    t0=utime.ticks_us()
    a=b.read_euler()
    buf_0[i][0]=a[2]
    buf_0[i][1]=0
    value_string = '%i, %i;' %(buf_0[i][0], buf_0[i][1])
    f.write(value_string)
    buf_index += 1
    t1=utime.ticks_us()
    pyb.udelay(20000-(t1-t0))

f.close()
but I receive the following error message...

Traceback (most recent call last):
File "BNO055.py", line 522, in read_euler
File "BNO055.py", line 511, in _read_vector
MemoryError:

I don't understand where is the mistake

User avatar
jimmo
Posts: 892
Joined: Tue Aug 08, 2017 1:57 am
Location: Sydney, Australia

Re: recording data from a BNO055 with a pyboard

Post by jimmo » Mon Nov 04, 2019 12:45 am

This line...

Code: Select all

buf_0 = [[i, 0] for i in range(2000)]
...allocates a lot of memory. I think this ends up using about 72kB of RAM, based on the way that objects work in MicroPython. (It's a 2000-element list, each element is a 4-byte pointer to a list, which is itself 16 bytes of metadata, and 8 bytes of data (two ints), but the 8 bytes is rounded to 16 bytes to fit in a GC block).

I would suggest using...

Code: Select all

buf_0_0 = [i for i in range(2000)]                                                                                                
buf_0_1 = [0 for i in range(2000)]  
...which uses very close to 16kB of RAM instead. (You'll just have to change the way you access it).

Or perhaps take a look at ulab (numpy for micropython) -- viewtopic.php?f=3&t=7005

Post Reply