2 BNO085 via I2C

Other Hardware Sensors
1

I'm using the following code. Works great for 1 BNO085 from Adafruit but I can't get it to work with 2 BNO085 (having the second one changed to 0x4B I2C Adress). Both SCL and SDA are wired to the A4 and A5 of the Arduino Connect RP2040:

#include <Arduino.h>
// This demo explores two reports (SH2_ARVR_STABILIZED_RV and SH2_GYRO_INTEGRATED_RV) both can be used to give 
// quartenion and euler (yaw, pitch roll) angles.  Toggle the FAST_MODE define to see other report.  
// Note sensorValue.status gives calibration accuracy (which improves over time)
//https://github.com/adafruit/Adafruit_BNO08x/blob/master/examples/quaternion_yaw_pitch_roll/quaternion_yaw_pitch_roll.ino
//_____________________________________________________________________________________________________________________________
//#include <Adafruit_BNO08xM.h>
#include "sh2.h"
#include "sh2_SensorValue.h"
#include "sh2_err.h"
#include <Adafruit_BusIO_Register.h>
#include <Adafruit_I2CDevice.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

#define BNO08x_I2CADDR_DEFAULT 0x4A ///< The default I2C address
//EL SEGON ES 0x4B
#define BNO08x_I2CADDR_DEFAULT2 0x4B

/* Additional Activities not listed in SH-2 lib */
#define PAC_ON_STAIRS 8 ///< Activity code for being on stairs
#define PAC_OPTION_COUNT                                                       \
  9 ///< The number of current options for the activity classifier

/*!
 *    @brief  Class that stores state and functions for interacting with
 *            the BNO08x 9-DOF Orientation IMU Fusion Breakout
 */
class Adafruit_BNO08x {
public:
  Adafruit_BNO08x(int8_t reset_pin = -1);
  ~Adafruit_BNO08x();

  bool begin_I2C(uint8_t i2c_addr = BNO08x_I2CADDR_DEFAULT,
                 TwoWire *wire = &Wire, int32_t sensor_id = 0);
  bool begin_UART(HardwareSerial *serial, int32_t sensor_id = 0);

  bool begin_SPI(uint8_t cs_pin, uint8_t int_pin, SPIClass *theSPI = &SPI,
                 int32_t sensor_id = 0);

  void hardwareReset(void);
  bool wasReset(void);

  bool enableReport(sh2_SensorId_t sensor, uint32_t interval_us = 10000);
  bool getSensorEvent(sh2_SensorValue_t *value);

  sh2_ProductIds_t prodIds; ///< The product IDs returned by the sensor

protected:
  virtual bool _init(int32_t sensor_id);

  sh2_Hal_t
      _HAL; ///< The struct representing the SH2 Hardware Abstraction Layer
};
class Adafruit_BNO08x_2 {
public:
  Adafruit_BNO08x_2(int8_t reset_pin = -1);
  ~Adafruit_BNO08x_2();

  bool begin_I2C(uint8_t i2c_addr = BNO08x_I2CADDR_DEFAULT2,
                 TwoWire *wire = &Wire, int32_t sensor_id2 = 2);
  bool begin_UART(HardwareSerial *serial, int32_t sensor_id2 = 2);

  bool begin_SPI(uint8_t cs_pin, uint8_t int_pin, SPIClass *theSPI = &SPI,
                 int32_t sensor_id2 = 2);

  void hardwareReset(void);
  bool wasReset(void);

  bool enableReport(sh2_SensorId_t sensor2, uint32_t interval_us = 10000);
  bool getSensorEvent(sh2_SensorValue_t *value2);

  sh2_ProductIds_t prodIds; ///< The product IDs returned by the sensor

protected:
  virtual bool _init(int32_t sensor_id);

  sh2_Hal_t
      _HAL; ///< The struct representing the SH2 Hardware Abstraction Layer
};

//_______________________________________________________________________________________________________________________________
//per checkejar la llibreria https://github.com/adafruit/Adafruit_BNO08x/blob/master/src/Adafruit_BNO08x.h

// For SPI mode, we need a CS pin
// EL FEM SERVIR??SÍ QUE CAL. Però jo realment  estic  possant el SDA i l'SCL a A4 i A5 de la Nano RP2040, respectivament.
#define BNO08X_CS 10
#define BNO08X_INT 9


// #define FAST_MODE

// For SPI mode, we also need a RESET 
//#define BNO08X_RESET 5
// but not for I2C or UART
#define BNO08X_RESET -1

struct euler_t {
  float yaw;
  float pitch;
  float roll;
} ypr;

struct euler2_t {
  float yaw2;
  float pitch2;
  float roll2;
} ypr2;

Adafruit_BNO08x  bno08x(BNO08X_RESET);
sh2_SensorValue_t sensorValue;
sh2_SensorValue_t sensorValue2;

#ifdef FAST_MODE
  // Top frequency is reported to be 1000Hz (but freq is somewhat variable)
  sh2_SensorId_t reportType = SH2_GYRO_INTEGRATED_RV;
  long reportIntervalUs = 2000;
#else
  // Top frequency is about 250Hz but this report is more accurate
  sh2_SensorId_t reportType = SH2_ARVR_STABILIZED_RV;
  long reportIntervalUs = 5000;
#endif
void setReports(sh2_SensorId_t reportType, long report_interval) {
  Serial.println("Setting desired reports");
  if (! bno08x.enableReport(reportType, report_interval)) {
    Serial.println("Could not enable stabilized remote vector");
  }
}

void setup(void) {

  Serial.begin(115200);
  while (!Serial) delay(10);     // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit BNO08x test!");

  // Try to initialize!
  if (!bno08x.begin_I2C()) {
  //if (!bno08x.begin_UART(&Serial1)) {  // Requires a device with > 300 byte UART buffer!
  //if (!bno08x.begin_SPI(BNO08X_CS, BNO08X_INT)) {
    Serial.println("Failed to find BNO08x chip");
    while (1) { delay(10); }
  }
  Serial.println("BNO08x Found!");


  setReports(reportType, reportIntervalUs);

  Serial.println("Reading events");
  delay(100);
}

void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr, bool degrees = false) {

    float sqr = sq(qr);
    float sqi = sq(qi);
    float sqj = sq(qj);
    float sqk = sq(qk);

    ypr->yaw = atan2(2.0 * (qi * qj + qk * qr), (sqi - sqj - sqk + sqr));
    ypr->pitch = asin(-2.0 * (qi * qk - qj * qr) / (sqi + sqj + sqk + sqr));
    ypr->roll = atan2(2.0 * (qj * qk + qi * qr), (-sqi - sqj + sqk + sqr));

    if (degrees) {
      ypr->yaw *= RAD_TO_DEG;
      ypr->pitch *= RAD_TO_DEG;
      ypr->roll *= RAD_TO_DEG;
    }
}

void quaternionToEuler2(float qr2, float qi2, float qj2, float qk2, euler2_t* ypr2, bool degrees2 = false) {

    float sqr2 = sq(qr2);
    float sqi2 = sq(qi2);
    float sqj2 = sq(qj2);
    float sqk2 = sq(qk2);

    ypr2->yaw2 = atan2(2.0 * (qi2 * qj2 + qk2 * qr2), (sqi2 - sqj2 - sqk2 + sqr2));
    ypr2->pitch2 = asin(-2.0 * (qi2 * qk2 - qj2 * qr2) / (sqi2 + sqj2 + sqk2 + sqr2));
    ypr2->roll2 = atan2(2.0 * (qj2 * qk2 + qi2 * qr2), (-sqi2 - sqj2 + sqk2 + sqr2));

    if (degrees2) {
      ypr2->yaw2 *= RAD_TO_DEG;
      ypr2->pitch2 *= RAD_TO_DEG;
      ypr2->roll2 *= RAD_TO_DEG;
    }
}

void quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, euler_t* ypr, bool degrees = false) {
    quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}

void quaternionToEulerGI(sh2_GyroIntegratedRV_t* rotational_vector, euler_t* ypr, bool degrees = false) {
    quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}

void quaternionToEulerRV2(sh2_RotationVectorWAcc_t* rotational_vector2, euler2_t* ypr2, bool degrees2 = false) {
    quaternionToEuler2(rotational_vector2->real, rotational_vector2->i, rotational_vector2->j, rotational_vector2->k, ypr2, degrees2);
}

void quaternionToEulerGI2(sh2_GyroIntegratedRV_t* rotational_vector2, euler2_t* ypr2, bool degrees2 = false) {
    quaternionToEuler2(rotational_vector2->real, rotational_vector2->i, rotational_vector2->j, rotational_vector2->k, ypr2, degrees2);
}
void loop() {

  if (bno08x.wasReset()) {
    Serial.print("sensor was reset ");
    setReports(reportType, reportIntervalUs);
  }
  
  if (bno08x.getSensorEvent(&sensorValue)) {
    // in this demo only one report type will be received depending on FAST_MODE define (above)
    switch (sensorValue.sensorId) {
      case SH2_ARVR_STABILIZED_RV:
        quaternionToEulerRV(&sensorValue.un.arvrStabilizedRV, &ypr, true);
      case SH2_GYRO_INTEGRATED_RV:
        // faster (more noise?)
        quaternionToEulerGI(&sensorValue.un.gyroIntegratedRV, &ypr, true);
        break;
    }
    switch (sensorValue.sensorId) {
      case SH2_ARVR_STABILIZED_RV:
        quaternionToEulerRV2(&sensorValue.un.arvrStabilizedRV, &ypr2, true);
      case SH2_GYRO_INTEGRATED_RV:
        // faster (more noise?)
        quaternionToEulerGI2(&sensorValue.un.gyroIntegratedRV, &ypr2, true);
        break;
    }
    static long last = 0;
    long now = micros();
    Serial.print(now - last);             Serial.print("\t");
    last = now;
    Serial.print(sensorValue.status);     Serial.print("\t");  // This is accuracy in the range of 0 to 3
    Serial.print(ypr.yaw);                Serial.print("\t");
    Serial.print(ypr.pitch);              Serial.print("\t");
    Serial.print(ypr.roll);               Serial.print("\t");
    //Serial.print(sensorValue.status);     Serial.print("\t");  // This is accuracy in the range of 0 to 3
    Serial.print(ypr2.yaw2);                Serial.print("\t");
    Serial.print(ypr2.pitch2);              Serial.print("\t");
    Serial.println(ypr2.roll2);
  }

}

Don't really know what I have to do now. Thank you in advance.

2

A class declaration is something that is not specific, but something that can be used in different ways. Other classes can use it. One or more objects can be created from it.

Please remove the two classes from your sketch. Use what is in the Arduino library.

Create two objects:

Adafruit_BNO08x  bno08x_1();
Adafruit_BNO08x  bno08x_2();

Initialize them both:

if (!bno08x_1.begin_I2C(0x4A)) 
  Serial.println("Failed to find BNO08x_1 chip");
if (!bno08x_2.begin_I2C(0x4B)) 
  Serial.println("Failed to find BNO08x_2 chip");

I see that you use the quaternion_yaw_pitch_roll.ino example. I have not looked into the use of sh2_SensorValue_t. I assume that you need two of those as well.

3

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.