0

Mulitple BNO080 using multiplexer
Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.

Re: Mulitple BNO080 using multiplexer

by adafruit_support_carter on Fri Feb 26, 2021 10:43 pm

Neat! Looks like your code comments ref sections from the SH-2 Reference Manual (Document Number: 1000-3625)?

@joakimolsson Maybe you can take that and adapt and use for your use case?

For the library code, it will be much more work, since it has to be more general and allow user access to all these knobs, etc. The SHTP is much more involved than basic register read/writes.

adafruit_support_carter
 
Posts: 19806
Joined: Tue Nov 29, 2016 2:45 pm

Re: Mulitple BNO080 using multiplexer

by Jorgen3 on Sat Feb 27, 2021 6:18 am

Hi Gammaburst
Did you add the opposite on the pullup or did you remove existing pollups on the card, which values of resistance did you use?

Best regards / Jörgen

Jorgen3
 
Posts: 2
Joined: Thu Jun 11, 2020 4:10 pm

Re: Mulitple BNO080 using multiplexer

by gammaburst on Sat Feb 27, 2021 7:24 am

Yes, my example code references the SH-2 Reference Manual 1000-3625 rev 1.5:
https://www.ceva-dsp.com/wp-content/upl ... Manual.pdf

That code (or a mini-library having only essential features) fits easily into an Arduino Uno.

I don't like having to bit-bang the I2C. Requires using an oscilloscope and fiddling with software timing delays to achieve fast I2C timing. On the other hand, it easily does clock stretching. I will study the Wire library some more, maybe I can use it.

Hi Jorgen3, your question "add the opposite on the pullup" is unclear. I usually add 2 or 3 K ohms to SDA, and add 5 or 6 K ohms to SCL when talking to a BNO0xx. In my photo you see 2.4K and 6.2K because I had those values sitting on my desk (sorry the camera angle obscures the connections). 2.7K and 4.7K would work too. I didn't touch any of the existing pullups located on the breakout boards.

Here's my highly sophisticated test for I2C stability: Add a short bare wire to SDA and grab it with two fingers. The project should continue working. Move the wire to SCL and grab it with two fingers. The project should continue working. Your fingers add capacitance that slows the signal risetime. The I2C bus should tolerate the slowdown and continue working.

gammaburst
 
Posts: 550
Joined: Thu Dec 31, 2015 12:06 pm

Re: Mulitple BNO080 using multiplexer

by Jorgen3 on Sat Feb 27, 2021 7:57 am

Hi Gammaburst
Thanks for a quick reply, I will feedback when the test is done.

Best regards / Jörgen

Jorgen3
 
Posts: 2
Joined: Thu Jun 11, 2020 4:10 pm

Re: Mulitple BNO080 using multiplexer

by jps2000 on Sat Feb 27, 2021 8:03 am

@ gammaburst
yep
I like these stability tests very much. Also a hair dryer or a freezing spray can provide eye-opening results.
In the BNO the 32kHz xtal pins - in particular the input pin- is very critical.
Startup time of the oscillator varies strongly with xtal q factor and any external loads of the pin. Oscillator starts once VCC is applied. No influence of reset pin. Time to accept commands is influenced by this. Moisture / Flux below the BNO is very critical in that respect.

jps2000
 
Posts: 614
Joined: Fri Jun 02, 2017 4:12 pm

Re: Mulitple BNO080 using multiplexer

by gammaburst on Sat Feb 27, 2021 8:48 pm

Long ago I developed the "ouch" test for troubleshooting newly assembled TTL logic boards. Gently place the palm of my hand onto the board ... Ouch! Found the backwards part.

Tiny low-power low-freq crystal oscillators tend to have very high impedance. Ten megohms to VDD or GND could stop it cold.

Which BNO+crystal were you testing? The BNO055 crystal seems to have absolutely no effect on BNO operation. Like a placebo. I can short it out, inject a different frequency, etc, and see no change in BNO behavior or speed. Time to accept commands ... hmmm ... I wouldn't have noticed that because I don't send commands during normal operation. I haven't examined the crystal on the BNO070/080/085, maybe it does more.

gammaburst
 
Posts: 550
Joined: Thu Dec 31, 2015 12:06 pm

Re: Mulitple BNO080 using multiplexer

by jps2000 on Sun Feb 28, 2021 2:33 am

Indeed there is no effect of the xtal on a BNO055. It always starts with internal oscillator. There is a command to switch to external with homeopathic effect.

By contrast for the BNO080 the xtal is a must!!!!. It does not even start without. The controller ist not running without. -> Try with your magic finger. With a scope on xout you can measure how fast the oscillation starts. You see a fade-in.

I recommended xtals with lowest ballast capactiors. ( 7 pF). The higher they are the lower is the loop gain of the oscillator.

Also look at the Q in the datasheet. The smaller they are the worse can be Q
Attached an application note on that
Attachments
AVR4100_ Atmel.pdf
(902.2 KiB) Downloaded 1 time

jps2000
 
Posts: 614
Joined: Fri Jun 02, 2017 4:12 pm

Re: Mulitple BNO080 using multiplexer

by gammaburst on Mon Mar 01, 2021 3:37 pm

I converted my experimental multi-BNO085 example to use the Arduino Wire I2C library. The I2C controller now provides accurate I2C clock timing, however the project doesn't run as fast as bit-banging because the Wire library inserts STOP/START cycles where we don't want them, so I had to use clumsy data transfer workarounds to get the BNO data. The Adafruit and Sparkfun libraries had to use clumsy workarounds too. Hey Arduino, fix your library!

I tested this with a Feather M4 Express and a Metro Mini, but remember this is rough experimental code. See code comments.

Code: Select all | TOGGLE FULL SIZE
// Talks to multiple BNO085 sensors through a TCA9548 multiplexer.
//
// Uses NED/aerospace coordinates, so my axes definitions may differ from your other projects.
// Outputs data in compact Base64 format.
//
// Uses frustrating deficient Wire I2C library. Must do time-consuming SHTP header rereads, hurting overall performance.
// Need a better I2C library that allows reading an unlimited length I2C message one byte at a time without forcing any STOP/START cycles midway, because BNO085 message length is determined by bytes within the message.
//
// To improve SDA-to-SCL setup time during clock-stretching cycles, try adding a 2.7K pullup to SDA and a 4.7K pullup to SCL (or similar values), and use short I2C wires (a few cm).
//
// I measure 700us I2C clock stretching cycles. Ouch! Hillcrest, please try harder to reduce or eliminate clock stretching.
//
// To program Adafruit Feather M4 Express 3857:  select "Adafruit Feather M4 Express (SAMD51)", COMxx, "AVRISP mkII". Beware COM port may annoyingly change itself.
// To program Adafruit Metro Mini 2590:  select "Adafruit Metro", COMxx, "AVRISP mkII". This CPU is slower try fewer reports, or increase SENSOR_US, or reduce BNOs.

#include <Wire.h>

#define BNOs        3           // number of BNO08x breakouts connected via TCA9548 mux
#define pinRST      A3          // output pin to BNO RST

#define MUX_ADDR    0x70        // I2C address of TCA9548 multiplexer
#define BNO_ADDR    0x4A        // I2C address of BNO085 sensor (0x4A if SA0=0, 0x4B if SA0=1)
#define I2C_CLOCK   400000L     // I2C clock rate
#define SERIAL_BAUD 230400L     // serial port baud rate
#define SENSOR_US   10000L      // time between sensor reports, microseconds, 10000L is 100 Hz, 20000L is 50 Hz

#define DEBUG       0           // output extra info: 0 off, 1 on. Beware this causes too much output data at low baud rates and/or high sensor rates.

// *******************************
// **  Request desired reports  **
// *******************************

#define ACC_REPORT   0x01   // accel report, see 6.5.9
#define GYRO_REPORT  0x02   // gyro report, see 6.5.13
#define MAG_REPORT   0x03   // magneto report, see 6.5.16
#define LAC_REPORT   0x04   // linear accel report, see 6.5.10
#define QUAT_REPORT  0x05   // quaternion report, see 6.5.18
#define TIME_REPORT  0xFB   // time report, see 7.2.1

static void request_reports(uint8_t bno)
{
  Wire.beginTransmission(MUX_ADDR);     // select BNO
  Wire.write(1 << bno);
  Wire.endTransmission();

  // request acc reports, see 6.5.4
  static const uint8_t cmd_acc[]  = {21, 0, 2, 0, 0xFD, ACC_REPORT,  0, 0, 0, (SENSOR_US>>0)&255, (SENSOR_US>>8)&255, (SENSOR_US>>16)&255, (SENSOR_US>>24)&255, 0, 0, 0, 0, 0, 0, 0, 0};
  Wire.beginTransmission(BNO_ADDR);  Wire.write(cmd_acc, sizeof(cmd_acc));  Wire.endTransmission();

  // request gyro reports, see 6.5.4
  static const uint8_t cmd_gyro[] = {21, 0, 2, 0, 0xFD, GYRO_REPORT, 0, 0, 0, (SENSOR_US>>0)&255, (SENSOR_US>>8)&255, (SENSOR_US>>16)&255, (SENSOR_US>>24)&255, 0, 0, 0, 0, 0, 0, 0, 0};
  Wire.beginTransmission(BNO_ADDR);  Wire.write(cmd_gyro, sizeof(cmd_gyro));  Wire.endTransmission();

  // request magneto reports, see 6.5.4
  static const uint8_t cmd_mag[]  = {21, 0, 2, 0, 0xFD, MAG_REPORT,  0, 0, 0, (SENSOR_US>>0)&255, (SENSOR_US>>8)&255, (SENSOR_US>>16)&255, (SENSOR_US>>24)&255, 0, 0, 0, 0, 0, 0, 0, 0};
  Wire.beginTransmission(BNO_ADDR);  Wire.write(cmd_mag, sizeof(cmd_mag));  Wire.endTransmission();

  // request linear acc reports, see 6.5.4
  static const uint8_t cmd_lac[]  = {21, 0, 2, 0, 0xFD, LAC_REPORT,  0, 0, 0, (SENSOR_US>>0)&255, (SENSOR_US>>8)&255, (SENSOR_US>>16)&255, (SENSOR_US>>24)&255, 0, 0, 0, 0, 0, 0, 0, 0};
  Wire.beginTransmission(BNO_ADDR);  Wire.write(cmd_lac, sizeof(cmd_lac));  Wire.endTransmission();

  // request quaternion reports, see 6.5.4
  static const uint8_t cmd_quat[] = {21, 0, 2, 0, 0xFD, QUAT_REPORT, 0, 0, 0, (SENSOR_US>>0)&255, (SENSOR_US>>8)&255, (SENSOR_US>>16)&255, (SENSOR_US>>24)&255, 0, 0, 0, 0, 0, 0, 0, 0};
  Wire.beginTransmission(BNO_ADDR);  Wire.write(cmd_quat, sizeof(cmd_quat));  Wire.endTransmission();

  // At 10ms rate, BNO08x outputs most reports in one burst, Gyro-Quat-Lac-Mag, however Acc is asynchronous and a few percent faster. Situation may vary with SENSOR_US and maximum sensor rates.
}

// *******************
// **  Output data  **
// *******************

int16_t iax[BNOs], iay[BNOs], iaz[BNOs];             // accel, integer
int16_t igx[BNOs], igy[BNOs], igz[BNOs];             // gyro, integer
int16_t imx[BNOs], imy[BNOs], imz[BNOs];             // magneto, integer
int16_t ilx[BNOs], ily[BNOs], ilz[BNOs];             // linear accel, integer
int16_t iqw[BNOs], iqx[BNOs], iqy[BNOs], iqz[BNOs];  // quaternion, integer

char obuf[70], *pbuf;           // ensure this output buffer is big enough for your output string!

void uart_b64(int32_t i)        // output 18-bit integer as compact 3-digit base64
{
  for (int n=12; n >= 0; n-=6)
  {
    uint8_t c = (i >> n) & 63;
    *pbuf++ = (char)(c<26 ? 'A'+c : c<52 ? 'a'-26+c : c<62 ? '0'-52+c : c==62 ? '+' : '/');
  }
}

static void output_data(uint8_t bno)
{
  float kACC = 1/9.80665/256 * 131072/10.0;     // scale units for my project
  float kGYR =  180/M_PI/512 * 131072/4000.0;
  float kMAG =       0.01/16 * 131072/1.0;
  float kLAC = 1/9.80665/256 * 131072/10.0;

  pbuf = obuf;                        // pointer into output buffer
  *pbuf++ = 'k';  *pbuf++ = 'q'+bno;  // string header "kq" is BNO0, "kr" is BNO1, "ks" is BNO2, etc
  uart_b64(kACC*iax[bno]);  uart_b64(-kACC*iay[bno]);  uart_b64(-kACC*iaz[bno]);  // accel,   convert from m/sec/sec*256 to       g*131072/10.0
  uart_b64(kGYR*igx[bno]);  uart_b64(-kGYR*igy[bno]);  uart_b64(-kGYR*igz[bno]);  // gyro,    convert from   rad/sec*512 to deg/sec*131072/4000.0
  uart_b64(kMAG*imx[bno]);  uart_b64(-kMAG*imy[bno]);  uart_b64(-kMAG*imz[bno]);  // magneto, convert from        uT*16  to   gauss*131072/1.0
  uart_b64(kLAC*ilx[bno]);  uart_b64(-kLAC*ily[bno]);  uart_b64(-kLAC*ilz[bno]);  // linacc,  convert from m/sec/sec*256 to       g*131072/10.0
  // my BNO08x orientation dot is towards left rear, rotate BNO08x quaternion to NED conventions
  uart_b64(iqw[bno]+iqz[bno]); uart_b64(iqx[bno]+iqy[bno]); uart_b64(iqx[bno]-iqy[bno]); uart_b64(iqw[bno]-iqz[bno]);  // quat,    no conversion required becasue it'll be normalized
  uart_b64(0);          // temp,    convert from    degC*128     to  degC*131072/100.0
  uart_b64(0);          // baro,    convert from hectoPa*1048576 to  mbar*131072/2000.0
  uart_b64(0xFF);       // status,  four 2-bit codes {sys,gyr,acc,mag}
  *pbuf++ = 13;         // CR LF
  *pbuf++ = 10;
  *pbuf++ = 0;          // terminate string
  Serial.write(obuf);   // writing one long string is *much* faster than printing individual values
}

// ******************************************
// **  Check for and parse sensor reports  **
// ******************************************

#if DEBUG
  static uint8_t printbyte(uint8_t b)
  {
    Serial.print(" ");  Serial.print(b,HEX);
    return b;
  }
#else
  #define printbyte(b) (b)
#endif

static void ensure_read_available(int16_t length)  // ensure a read byte is available, if necessary reread and discard 4-byte SHTP header, then read as much length as possible
{
  if (!Wire.available())
    Wire.requestFrom(BNO_ADDR,4+length), Wire.read(), Wire.read(), Wire.read(), Wire.read();
}

static void check_report(uint8_t bno)
{
  int16_t length;
  uint8_t channel __attribute__((unused));
  uint8_t seqnum  __attribute__((unused));

  Wire.beginTransmission(MUX_ADDR);     // select BNO
  Wire.write(1 << bno);
  Wire.endTransmission();

  Wire.requestFrom(BNO_ADDR,4+1);       // read 4-byte SHTP header and first byte of cargo
  if (DEBUG) {Serial.print("SHTP");}
  length  = printbyte(Wire.read());     // length LSB
  length |= printbyte((Wire.read() & 0x7F) << 8);  // length MSB (ignore continuation flag)
  channel = printbyte(Wire.read());     // channel number
  seqnum  = printbyte(Wire.read());     // sequence number (ignore)
  length -= 4;                          // done reading SHTP Header
  if (length <= 0 || length > 1000)     // if null/bad/degenerate SHTP header
  {
    if (DEBUG) {Serial.println(" What?");}
    return;
  }
  if (DEBUG) {Serial.print(" L=");  Serial.print(length,HEX);}
  if (DEBUG) {Serial.print(" C=");  Serial.println(channel,HEX);}

  while (length)                        // while more reports in cargo
  {
    uint8_t buf[20];                    // report buffer, big enough for longest interesting report (uninteresting reports will be ignored)
    uint16_t n = 0;                     // index into report buffer

    ensure_read_available(length);
    buf[n++] = printbyte(Wire.read());  // first byte of report
    length--;

    // known reports
    if (channel==3 && buf[0]==TIME_REPORT && length >= 5-1)
    {
      for (uint8_t n=1; n<5; n++)       // read remainder of report
      {
        ensure_read_available(length);
        buf[n] = printbyte(Wire.read());
        length--;
      }
      if (DEBUG) {Serial.println(" Time");}
      continue;
    }
    if (channel==3 && buf[0]==ACC_REPORT && length >= 10-1)
    {
      for (uint8_t n=1; n<10; n++)      // read remainder of report
      {
        ensure_read_available(length);
        buf[n] = printbyte(Wire.read());
        length--;
      }
      iax[bno] = *(int16_t*)&buf[4];
      iay[bno] = *(int16_t*)&buf[6];
      iaz[bno] = *(int16_t*)&buf[8];
      if (DEBUG) {Serial.println(" Acc");}
      continue;
    }
    if (channel==3 && buf[0]==GYRO_REPORT && length >= 10-1)
    {
      for (uint8_t n=1; n<10; n++)      // read remainder of report
      {
        ensure_read_available(length);
        buf[n] = printbyte(Wire.read());
        length--;
      }
      igx[bno] = *(int16_t*)&buf[4];
      igy[bno] = *(int16_t*)&buf[6];
      igz[bno] = *(int16_t*)&buf[8];
      if (DEBUG) {Serial.println(" Gyro");}
      continue;
    }
    if (channel==3 && buf[0]==MAG_REPORT && length >= 10-1)
    {
      for (uint8_t n=1; n<10; n++)      // read remainder of report
      {
        ensure_read_available(length);
        buf[n] = printbyte(Wire.read());
        length--;
      }
      imx[bno] = *(int16_t*)&buf[4];
      imy[bno] = *(int16_t*)&buf[6];
      imz[bno] = *(int16_t*)&buf[8];
      if (DEBUG) {Serial.println(" Mag");}
      output_data(bno);                 // magneto seems to be last report of burst, so use it to trigger data output
      continue;
    }
    if (channel==3 && buf[0]==LAC_REPORT && length >= 10-1)
    {
      for (uint8_t n=1; n<10; n++)      // read remainder of report
      {
        ensure_read_available(length);
        buf[n] = printbyte(Wire.read());
        length--;
      }
      ilx[bno] = *(int16_t*)&buf[4];
      ily[bno] = *(int16_t*)&buf[6];
      ilz[bno] = *(int16_t*)&buf[8];
      if (DEBUG) {Serial.println(" Lac");}
      continue;
    }
    if (channel==3 && buf[0]==QUAT_REPORT && length >= 14-1)
    {
      for (uint8_t n=1; n<14; n++)      // read remainder of report
      {
        ensure_read_available(length);
        buf[n] = printbyte(Wire.read());
        length--;
      }
      iqw[bno] = *(int16_t*)&buf[10];
      iqx[bno] = *(int16_t*)&buf[4];
      iqy[bno] = *(int16_t*)&buf[6];
      iqz[bno] = *(int16_t*)&buf[8];
      if (DEBUG) {Serial.println(" Quat");}
      continue;
    }

    // unknown reports
    while (length)                      // discard remainder of cargo (shouldn't happen very often)
    {
      ensure_read_available(length);
      printbyte(Wire.read());
      length--;
    }
    if (DEBUG) {Serial.println(" Unknown");}
    continue;
  }
  return;
}

// **********************
// **  Setup and Loop  **
// **********************

void setup()
{
  Serial.begin(SERIAL_BAUD);            // initialize serial
  Serial.println("\nRunning...");

  pinMode(pinRST,OUTPUT);               // reset all BNOs
  digitalWrite(pinRST,LOW);
  delay(1);
  digitalWrite(pinRST,HIGH);
  delay(300);

  Wire.begin();                         // initialize I2C
  Wire.setClock(I2C_CLOCK);

  for (uint8_t bno=0; bno<BNOs; bno++)  // request desired reports
    request_reports(bno);

  for (uint8_t bno=0; bno<BNOs; bno++)  // wait until all BNOs output non-zero quaternions
    do
      check_report(bno);
    while (!iqw[bno] && !iqx[bno] && !iqy[bno] && !iqz[bno]);
}

void loop()
{
  for (uint8_t bno=0; bno<BNOs; bno++)  // check for reports from all BNOs
    check_report(bno);
}
Attachments
IMG_2494a.jpg
Feather M4 Express reads three BNO085s using TCA9548 mux and Wire library
IMG_2494a.jpg (346.78 KiB) Viewed 97 times

gammaburst
 
Posts: 550
Joined: Thu Dec 31, 2015 12:06 pm

Re: Mulitple BNO080 using multiplexer

by jps2000 on Tue Mar 02, 2021 1:58 am

Chapeau!
The data transfer via I2C is somehow slow. May be a BNO is hosted better via SPI (never tried that however)

Hence I2C reporting should be limited to quats, linacc (or acc ) and timestamp. In my view it makes little sense - despite for debugging- to use a BNO for raw data like acc, gyro and mag. Its strenght is embedded fusion calc.

However, there is also a fast gyro + quat output mode (SH2 chapter 6.5.44) in the sh2 protocol may be for small balancing robot control with less bus load.

jps2000
 
Posts: 614
Joined: Fri Jun 02, 2017 4:12 pm

Re: Mulitple BNO080 using multiplexer

by gammaburst on Wed Mar 03, 2021 6:00 pm

Good grief, the I2C clock is running slow.
Wire.setClock() gives me incorrect clock rates for the Feather M4 Express.
I changed my 400000L to 480000L, and now the I2C clock is much closer to 400 kHz.
Also, to get 100 kHz I had to specify 104000L.
I tried to read the SAMD51 datasheet and debug the Arduino source code, but the rabbit hole goes deep.

I can disable accel/gyro/magneto reports and read almost 400 Hz quaternions from all three BNO devices, but a few reports are getting lost. Occasionally it takes about 1.6ms to read one quaternion from one BNO (occasional 700us clock stretching + header reread clumsiness + unwanted time report + quaternion report). So much wasted time trying to read 8 bytes of information. Let's hope they implemented SPI better.

gammaburst
 
Posts: 550
Joined: Thu Dec 31, 2015 12:06 pm

Re: Mulitple BNO080 using multiplexer

by jps2000 on Fri Mar 05, 2021 3:38 am

I tried the BNO I2C running with almost 800kHz (set clock 1000000L) on an M4 board and it is still working fine. Did not look for the upper limit. So there may be some headroom to speed up the bus. (Spec says 400kHz however).
By the way:
The use of Adafruit SSD1306 oled driver is a problem because it switches the bus speed to 100kHz despite other definition in setup.
I did not want to dig into details and used another less bloated library instead.

We can conclude that it is always recommended to check with a scope what is really going on.

jps2000
 
Posts: 614
Joined: Fri Jun 02, 2017 4:12 pm

Re: Mulitple BNO080 using multiplexer

by gammaburst on Mon Mar 08, 2021 11:29 pm

Ha! You are an overclocking beast.

I managed to get three BNO085s working with a Feather M4 over SPI, and was preparing to share my experimental code when Adafruit's forum went dead for the weekend. Ok, forum is working again.

The BNO085's overly complex communication protocol is further complicated by full-duplex SPI. Apparently, to send a command to the BNO, we must wait for the BNO to send us any report (signalled by an INT interrupt), then we send one command byte for each received report byte while extending the shorter message with zero bytes. It's madness!

I thought life would improve because BNO SPI runs up to 3 MHz, but noooo. The BNO seems to need lots of time to process those incoming and outgoing messages. If I enable only one output report (such as quaternion, acceleration, gyro), then it outputs 400 Hz fine, but if I enable two or more reports, then the out rate decreases significantly, sometimes dramatically, even though the SPI bus is only slightly utilized. It never outputs more than about 600 to 900 reports per second over Channel 3. Disappointing! But it's a BNO, so unsurprising.

I had to connect individual Chip Select signals to the BNO CS pins, and individual Interrupt signals from the BNO INT pins, and one Reset signal to all the BNO RST pins. All that plus the three SPI signals is a lot of wiring for such pokey performance.

Beware this code is experimental, merely a demonstration example. Could be buggy. Please read the comments.

Code: Select all | TOGGLE FULL SIZE
// Talks to multiple BNO085 sensors via SPI.
// This is experimental demonstration code, so you may discover problems.
// I've run this with up to three BNO devices (that's all I have).
//
// Uses NED/aerospace coordinates, so my axes definitions may differ from your other projects.
// My BNO08x orientation dot is towards left rear, so code flips various sensor axes and quaternions accordingly.
// Outputs data in compact Base64 format.
//
// To program Adafruit Feather M4 Express 3857:  select "Adafruit Feather M4 Express (SAMD51)", COMxx, "AVRISP mkII". Beware COM port may annoyingly change itself.
//
// Connect Feather "MI"  to "SDA" of all BNOs
// Connect Feather "MO"  to "DI"  of all BNOs
// Connect Feather "SCK" to "SCL" of all BNOs
//
// Bugs:
// Occasionally, a random BNO seems to not receive one of the report requests, causing it to not output that report, but the problem has mysteriously vanished.

#include <SPI.h>

#define BNOs        3           // number of BNO08x breakouts (you must also comment-out unused "isr" definitions, see below)
#define pinRST      A3          // connect this  Feather pin  to "RST" of all BNOs
#define pinsINT     A0,A1,A2    // connect these Feather pins to "INT" of BNO0,BNO1,BNO2
#define pinsCS      5,6,9       // connect these Feather pins to "CS"  of BNO0,BNO1,BNO2

#define ALL_REPORTS 1           // 0 requrest only quaternions, 1 request all reports
#define REPORT_TIME 2500L       // time between sensor reports, microseconds  (2500L 400 Hz, 10000L 100 Hz, 100000L 10 Hz)
#define SPI_CLOCK   3000000L    // SPI clock rate, hertz
#define SERIAL_BAUD 230400L     // serial port baud rate, hertz

#define DATAOUT     1           // 1 enables data output messages
#define STATS       0           // 1 enables report counting statistics
#define DEBUG       0           // 1 enables verbose (overwhelming) info messages, suggest also greatly increasing REPORT_TIME

#define SPI_SETTINGS SPISettings(SPI_CLOCK, MSBFIRST, SPI_MODE3)

#define numberof(x)  (sizeof(x) / sizeof(*(x)))

// *******************
// **  Output data  **
// *******************

int16_t iax[BNOs], iay[BNOs], iaz[BNOs];             // accel, integer
int16_t igx[BNOs], igy[BNOs], igz[BNOs];             // gyro, integer
int16_t imx[BNOs], imy[BNOs], imz[BNOs];             // magneto, integer
int16_t ilx[BNOs], ily[BNOs], ilz[BNOs];             // linear accel, integer
int16_t iqw[BNOs], iqx[BNOs], iqy[BNOs], iqz[BNOs];  // quaternion, integer

char obuf[70], *pbuf;               // ensure this output buffer is big enough for your output string!

static void uart_b64(int32_t i)     // output 18-bit integer as compact 3-digit base64
{
  for (int n=12; n >= 0; n-=6)
  {
    uint8_t c = (i >> n) & 63;
    *pbuf++ = (char)(c<26 ? 'A'+c : c<52 ? 'a'-26+c : c<62 ? '0'-52+c : c==62 ? '+' : '/');
  }
}

#if ALL_REPORTS
  static void output_data(uint8_t bno)
  {
    float kACC = 1/9.80665/256 * 131072/10.0;   // scale units for my project
    float kGYR =  180/M_PI/512 * 131072/4000.0;
    float kMAG =       0.01/16 * 131072/1.0;
    float kLAC = 1/9.80665/256 * 131072/10.0;

    pbuf = obuf;                        // pointer into output buffer
    *pbuf++ = 'k';  *pbuf++ = 'q'+bno;  // string header "kq" is BNO0, "kr" is BNO1, "ks" is BNO2, etc
    uart_b64(kACC*iax[bno]);  uart_b64(-kACC*iay[bno]);  uart_b64(-kACC*iaz[bno]);  // accel,   convert from m/sec/sec*256 to       g*131072/10.0
    uart_b64(kGYR*igx[bno]);  uart_b64(-kGYR*igy[bno]);  uart_b64(-kGYR*igz[bno]);  // gyro,    convert from   rad/sec*512 to deg/sec*131072/4000.0
    uart_b64(kMAG*imx[bno]);  uart_b64(-kMAG*imy[bno]);  uart_b64(-kMAG*imz[bno]);  // magneto, convert from        uT*16  to   gauss*131072/1.0
    uart_b64(kLAC*ilx[bno]);  uart_b64(-kLAC*ily[bno]);  uart_b64(-kLAC*ilz[bno]);  // linacc,  convert from m/sec/sec*256 to       g*131072/10.0
    uart_b64(iqw[bno]+iqz[bno]); uart_b64(iqx[bno]+iqy[bno]); uart_b64(iqx[bno]-iqy[bno]); uart_b64(iqw[bno]-iqz[bno]);  // quat,    rotate quat to my reference frame, do no conversion because it'll be normalized later
    uart_b64(0);            // temp,    convert from    degC*128     to  degC*131072/100.0
    uart_b64(0);            // baro,    convert from hectoPa*1048576 to  mbar*131072/2000.0
    uart_b64(0xFF);         // status,  four 2-bit codes {sys,gyr,acc,mag}
    *pbuf++ = 13;           // CR LF
    *pbuf++ = 10;
    *pbuf++ = 0;            // terminate string
    Serial.write(obuf);     // writing one long string is *much* faster than printing individual values
  }
#else
  static void output_data(uint8_t bno)
  {
    pbuf = obuf;                        // pointer into output buffer
    *pbuf++ = 'k';  *pbuf++ = 'q'+bno;  // string header "kq" is BNO0, "kr" is BNO1, "ks" is BNO2, etc
    // my BNO08x orientation dot is towards left rear, rotate BNO08x quaternion to NED conventions
    uart_b64(iqw[bno]+iqz[bno]); uart_b64(iqx[bno]+iqy[bno]); uart_b64(iqx[bno]-iqy[bno]); uart_b64(iqw[bno]-iqz[bno]);  // quat,    rotate quat to my reference frame, do no conversion because it'll be normalized later
    *pbuf++ = 13;           // CR LF
    *pbuf++ = 10;
    *pbuf++ = 0;            // terminate string
    Serial.write(obuf);     // writing one long string is *much* faster than printing individual values
  }
#endif

// ******************************************************************************************************
// **  Receive one SPI byte while simultaneously sending one byte from a queue of pending tx messages  **
// ******************************************************************************************************

#define ACC_REPORT   0x01   // accel report, see 6.5.9
#define GYRO_REPORT  0x02   // gyro report, see 6.5.13
#define MAG_REPORT   0x03   // magneto report, see 6.5.16
#define LAC_REPORT   0x04   // linear accel report, see 6.5.10
#define QUAT_REPORT  0x05   // quaternion report, see 6.5.18
#define TIME_REPORT  0xFB   // time report, see 7.2.1

static const uint8_t req_acc[]  = {21, 0, 2, 0, 0xFD, ACC_REPORT,  0, 0, 0, (REPORT_TIME>>0)&255, (REPORT_TIME>>8)&255, (REPORT_TIME>>16)&255, (REPORT_TIME>>24)&255, 0, 0, 0, 0, 0, 0, 0, 0};
static const uint8_t req_gyro[] = {21, 0, 2, 0, 0xFD, GYRO_REPORT, 0, 0, 0, (REPORT_TIME>>0)&255, (REPORT_TIME>>8)&255, (REPORT_TIME>>16)&255, (REPORT_TIME>>24)&255, 0, 0, 0, 0, 0, 0, 0, 0};
static const uint8_t req_mag[]  = {21, 0, 2, 0, 0xFD, MAG_REPORT,  0, 0, 0, (REPORT_TIME>>0)&255, (REPORT_TIME>>8)&255, (REPORT_TIME>>16)&255, (REPORT_TIME>>24)&255, 0, 0, 0, 0, 0, 0, 0, 0};
static const uint8_t req_lac[]  = {21, 0, 2, 0, 0xFD, LAC_REPORT,  0, 0, 0, (REPORT_TIME>>0)&255, (REPORT_TIME>>8)&255, (REPORT_TIME>>16)&255, (REPORT_TIME>>24)&255, 0, 0, 0, 0, 0, 0, 0, 0};
static const uint8_t req_quat[] = {21, 0, 2, 0, 0xFD, QUAT_REPORT, 0, 0, 0, (REPORT_TIME>>0)&255, (REPORT_TIME>>8)&255, (REPORT_TIME>>16)&255, (REPORT_TIME>>24)&255, 0, 0, 0, 0, 0, 0, 0, 0};

static const struct TxQueue                 // list of pending tx messages
{
  const uint8_t length;
  const uint8_t *message;
} txqueue[] =
{
 #if ALL_REPORTS
  {sizeof(req_acc),  req_acc},
  {sizeof(req_gyro), req_gyro},
  {sizeof(req_mag),  req_mag},
  {sizeof(req_lac),  req_lac},
 #endif
  {sizeof(req_quat), req_quat},
};

uint8_t txqueue_msg[BNOs];                  // next message to send
uint8_t txqueue_pos[BNOs];                  // next byte to send

static uint8_t recv(uint8_t bno)            // receive one byte while simultaneously sending one byte from current tx message (or zero if no tx message)
{
  uint8_t tx = 0;
  if (txqueue_msg[bno] < numberof(txqueue)) // if more tx messages to send
  {
    if (txqueue_pos[bno] < txqueue[txqueue_msg[bno]].length)  // if current tx message is incomplete
    {
      tx = txqueue[txqueue_msg[bno]].message[txqueue_pos[bno]++];  // byte to send
      if (DEBUG) {Serial.print(" +");  Serial.print(tx,HEX);}
    }
  }
  uint8_t rx = SPI.transfer(tx);            // send tx, receive rx
  if (DEBUG) {Serial.print(" ");  Serial.print(rx,HEX);}
  return rx;
}

static void flush(uint8_t bno)              // send all pending tx message bytes, then advance to next tx message
{
  if (txqueue_msg[bno] >= numberof(txqueue)) // if no more tx messages to send
    return;
  if (txqueue_pos[bno])                     // if current tx message is underway
  {
    while (txqueue_pos[bno] < txqueue[txqueue_msg[bno]].length)  // while tx message is incomplete
    {
      uint8_t tx = txqueue[txqueue_msg[bno]].message[txqueue_pos[bno]++];
      SPI.transfer(tx);                     // send tx byte, discard rx byte
      if (DEBUG) {Serial.print(" +");  Serial.print(tx,HEX);}
    }
    txqueue_msg[bno]++;                     // prepare next tx message
    txqueue_pos[bno] = 0;
  }
}

static const decltype(pinRST) CS[]  = {pinsCS};   // array of CS pin numbers for all BNOs
static const decltype(pinRST) INT[] = {pinsINT};  // array of INT pin numbers for all BNOs

static void bno_select(uint8_t bno)         // select SPI port of desired BNO
{
  digitalWrite(CS[bno], LOW);
  pinMode(CS[bno], OUTPUT);
}

static void bno_deselect(uint8_t bno)       // deselect SPI port of desired BNO
{
  digitalWrite(CS[bno], HIGH);
  pinMode(CS[bno], OUTPUT);
}

// ******************************************
// **  Check for and parse sensor reports  **
// ******************************************

static uint16_t millis_last __attribute__((unused));  // one second timer

static void check_report(uint8_t bno)
{
  int16_t length;
  uint8_t channel __attribute__((unused));
  uint8_t seqnum[BNOs] __attribute__((unused));
  static uint8_t seqnum_next[BNOs] __attribute__((unused));
  static uint16_t seqnum_errors[BNOs] __attribute__((unused));
  static uint16_t acc_reports[BNOs] __attribute__((unused));
  static uint16_t gyro_reports[BNOs] __attribute__((unused));
  static uint16_t mag_reports[BNOs] __attribute__((unused));
  static uint16_t lac_reports[BNOs] __attribute__((unused));
  static uint16_t quat_reports[BNOs] __attribute__((unused));

  if (DEBUG) {Serial.print("SHTP");}
  bno_select(bno);
  length = recv(bno);                       // length LSB
  length |= (uint16_t)recv(bno) << 8;       // length MSB
  length &= 0x7FFF;                         // ignore continuation flag
  channel = recv(bno);                      // channel number
  seqnum[bno] = recv(bno);                  // sequence number (ignore)
  length -= 4;                              // done reading SHTP Header

  if (length <= 0 || length > 1000)         // if null/bad/degenerate SHTP header
  {
    if (DEBUG) {Serial.println(" What?");}
    flush(bno);
    bno_deselect(bno);
    return;
  }
  if (DEBUG) {Serial.print(" L=");  Serial.print(length,HEX);}
  if (DEBUG) {Serial.print(" C=");  Serial.println(channel,HEX);}

  #if STATS
    if (channel==3)
    {
      seqnum_errors[bno] += (seqnum[bno] != seqnum_next[bno]);
      seqnum_next[bno] = seqnum[bno] + 1;
    }
  #endif

  while (length)                            // while more reports in cargo
  {
    uint8_t buf[20];                        // report buffer, big enough for longest interesting report (uninteresting reports will be ignored)
    uint16_t n = 0;                         // index into report buffer

    buf[n++] = recv(bno);                   // first byte of report
    length--;

    // known reports
    if (channel==3 && buf[0]==TIME_REPORT && length >= 5-1)
    {
      for (uint8_t n=1; n<5; n++)           // read remainder of report
      {
        buf[n] = recv(bno);
        length--;
      }
      if (DEBUG) {Serial.println(" Time");}
      continue;
    }
    #if ALL_REPORTS
      if (channel==3 && buf[0]==ACC_REPORT && length >= 10-1)
      {
        for (uint8_t n=1; n<10; n++)        // read remainder of report
        {
          buf[n] = recv(bno);
          length--;
        }
        iax[bno] = *(int16_t*)&buf[4];
        iay[bno] = *(int16_t*)&buf[6];
        iaz[bno] = *(int16_t*)&buf[8];
        if (DEBUG) {Serial.println(" Acc");}
        if (STATS) {acc_reports[bno]++;}
        continue;
      }
      if (channel==3 && buf[0]==GYRO_REPORT && length >= 10-1)
      {
        for (uint8_t n=1; n<10; n++)        // read remainder of report
        {
          buf[n] = recv(bno);
          length--;
        }
        igx[bno] = *(int16_t*)&buf[4];
        igy[bno] = *(int16_t*)&buf[6];
        igz[bno] = *(int16_t*)&buf[8];
        if (DEBUG) {Serial.println(" Gyro");}
        if (STATS) {gyro_reports[bno]++;}
        continue;
      }
      if (channel==3 && buf[0]==MAG_REPORT && length >= 10-1)
      {
        for (uint8_t n=1; n<10; n++)        // read remainder of report
        {
          buf[n] = recv(bno);
          length--;
        }
        imx[bno] = *(int16_t*)&buf[4];
        imy[bno] = *(int16_t*)&buf[6];
        imz[bno] = *(int16_t*)&buf[8];
        if (DEBUG) {Serial.println(" Mag");}
        if (STATS) {mag_reports[bno]++;}
        continue;
      }
      if (channel==3 && buf[0]==LAC_REPORT && length >= 10-1)
      {
        for (uint8_t n=1; n<10; n++)        // read remainder of report
        {
          buf[n] = recv(bno);
          length--;
        }
        ilx[bno] = *(int16_t*)&buf[4];
        ily[bno] = *(int16_t*)&buf[6];
        ilz[bno] = *(int16_t*)&buf[8];
        if (DEBUG) {Serial.println(" Lac");}
        if (STATS) {lac_reports[bno]++;}
        continue;
      }
    #endif
    if (channel==3 && buf[0]==QUAT_REPORT && length >= 14-1)
    {
      for (uint8_t n=1; n<14; n++)          // read remainder of report
      {
        buf[n] = recv(bno);
        length--;
      }
      iqw[bno] = *(int16_t*)&buf[10];
      iqx[bno] = *(int16_t*)&buf[4];
      iqy[bno] = *(int16_t*)&buf[6];
      iqz[bno] = *(int16_t*)&buf[8];
      if (DEBUG) {Serial.println(" Quat");}
      if (STATS) {quat_reports[bno]++;}
      if (DATAOUT) {output_data(bno);}      // output data message
      continue;
    }

    // unwanted reports
    if (channel==0 && buf[0]==0)
    {
      while (length)                        // discard remainder of cargo
      {
        recv(bno);
        length--;
      }
      if (DEBUG) {Serial.println(" Advert");}
      continue;
    }
    if (channel==1 && buf[0]==1)
    {
      while (length)                        // discard remainder of cargo
      {
        recv(bno);
        length--;
      }
      if (DEBUG) {Serial.println(" ExeRst");}
      continue;
    }
    if (channel==2 && buf[0]==0xF1)
    {
      while (length)                        // discard remainder of cargo
      {
        recv(bno);
        length--;
      }
      if (DEBUG) {Serial.println(" CmdResp");}
      continue;
    }
    if (channel==2 && buf[0]==0xFC)
    {
      while (length)                        // discard remainder of cargo
      {
        recv(bno);
        length--;
      }
      if (DEBUG) {Serial.println(" GetFeatResp");}
      continue;
    }
    while (length)                          // discard remainder of cargo
    {
      recv(bno);
      length--;
    }
    if (DEBUG) {Serial.println(" Unk:");  Serial.print(channel,HEX);  Serial.print(":");  Serial.println(buf[0],HEX);}
    continue;
  }
  flush(bno);
  bno_deselect(bno);

  #if STATS
    if ((uint16_t)(millis() - millis_last) > 1000)
    {
      millis_last += 1000;                  // for next second

      for (uint8_t bno=0; bno<BNOs; bno++)  // for each BNO display its various reports per second
      {
        Serial.print("Bno");  Serial.print(bno);  Serial.print("rpts: ");
        Serial.print(acc_reports[bno]);  Serial.print(",");
        Serial.print(gyro_reports[bno]); Serial.print(",");
        Serial.print(mag_reports[bno]);  Serial.print(",");
        Serial.print(lac_reports[bno]);  Serial.print(",");
        Serial.print(quat_reports[bno]); Serial.print(" ");
        Serial.println();
        acc_reports[bno]  = 0;
        gyro_reports[bno] = 0;
        mag_reports[bno]  = 0;
        lac_reports[bno]  = 0;
        quat_reports[bno] = 0;
      }

      Serial.print("SeqErrs: ");
      for (uint8_t bno=0; bno<BNOs; bno++)  // display sequence errors per second
      {
        Serial.print(seqnum_errors[bno]);  Serial.print(" ");
        seqnum_errors[bno] = 0;
      }
      Serial.println();
    }
  #endif

  return;
}

// **********************
// **  Setup and Loop  **
// **********************

uint8_t volatile int_count[BNOs];
uint8_t          int_counted[BNOs];

// the number of isr functions defined here must equal BNOs (I'd like to automate this with macros but don't know how)
  static void isr0(void) { int_count[0]++; }
  static void isr1(void) { int_count[1]++; }
  static void isr2(void) { int_count[2]++; }
//static void isr3(void) { int_count[3]++; }
//static void isr4(void) { int_count[4]++; }
//static void isr5(void) { int_count[5]++; }
//static void isr6(void) { int_count[6]++; }
//static void isr7(void) { int_count[7]++; }
  static void (*isr[])() = {isr0,isr1,isr2/*,isr3,isr4,isr5,isr6,isr7*/};

void setup()
{
  Serial.begin(SERIAL_BAUD);                // initialize serial
  while (!Serial);
  Serial.println("\nRunning...");

  for (uint8_t bno=0; bno<BNOs; bno++)      // disable all BNOs
    bno_deselect(bno);

  SPI.begin();                              // initialize SPI, however it sets SCK and MOSI low which is wrong for SPI_MODE3
  SPI.beginTransaction(SPI_SETTINGS);       // preset SPI signals (this is SPI.begin()'s job, not something I should have to do)

  pinMode(pinRST,OUTPUT);                   // reset all BNOs
  digitalWrite(pinRST,LOW);
  delay(2);                                 // because 1 sometimes gets truncated
  digitalWrite(pinRST,HIGH);
  delay(300);                               // below 145 fails, above 150 works

  for (uint8_t bno=0; bno<BNOs; bno++)
  {
    pinMode(INT[bno], INPUT);
    attachInterrupt(digitalPinToInterrupt(INT[bno]), isr[bno], FALLING);
  }

  millis_last = millis();
}

void loop()
{
  for (uint8_t bno=0; bno<BNOs; bno++)      // check for reports from all BNOs
  {
    if (int_count[bno] != int_counted[bno]) // if transfer request
    {
      int_counted[bno] = int_count[bno];    // acknowledge all interrupt(s)
      if (DEBUG) {Serial.print(" BNO=");  Serial.println(bno);}
      check_report(bno);
    }
  }
}
Attachments
IMG_2497a.jpg
Three BNO085s connected via SPI to Feather M4 Express, experimental
IMG_2497a.jpg (273.16 KiB) Viewed 49 times

gammaburst
 
Posts: 550
Joined: Thu Dec 31, 2015 12:06 pm

Re: Mulitple BNO080 using multiplexer

by jps2000 on Tue Mar 09, 2021 2:09 am

Great work!
As always the cherries in the neighbours garden look bigger. You need to come closer (= really try out SPI) to face the truth.
So it seems that I2C is not so bad as it seemed.
Overclocking may do it when you go to extremes.
Just one recommendation:
The BNO080 allows to autostore calibration This is very beneficial I found.

Code: Select all | TOGGLE FULL SIZE
// Saves dynamic cal data periodically every 5 minutes (default) 

void save_periodic_DCD(){                                             
  uint8_t periodic_dcd[16] = {16,0,2,0,0xF2,0,0x09,0,0,0,0,0,0,0,0,0};
  Wire.beginTransmission(BNO_ADDRESS); 
  Wire.write(periodic_dcd, sizeof(periodic_dcd));
  Wire.endTransmission();   
}

jps2000
 
Posts: 614
Joined: Fri Jun 02, 2017 4:12 pm

Re: Mulitple BNO080 using multiplexer

by gammaburst on Tue Mar 09, 2021 6:21 am

The BNO085's I2C is still bad: 700 us clock stretching, and the same SDA-to-SCL timing bug (during clock stretch cycle) as the BNO055.

Thanks for the DCD autosave tip. It's easy to add that command to my SPI example. Find the two lines that contain "req_quat", add similar lines named "periodic_dcd", and use the message bytes you provided. That message is described in "SH-2 Reference Manual" section 6.4.7.1. Where is the 5 minute default described?

One advantage to the BNO085's low utilization of the SPI bus: there's plenty of time for several BNOs to share it.

gammaburst
 
Posts: 550
Joined: Thu Dec 31, 2015 12:06 pm

Re: Mulitple BNO080 using multiplexer

by jps2000 on Tue Mar 09, 2021 7:43 am

This is from personal communication with Hillcrest in 2017
>>>
Save DCD(refer to section 6.4.6 in SH-2 Reference Manual) can save the current calibration record into BNO080 internal FLASH, and make sure the next bootup has the same calibrated data. I would recommend to Save DCD after calibration and before every power off. In fact, BNO080 will also save it periodically(every 5 minutes).
>>>

jps2000
 
Posts: 614
Joined: Fri Jun 02, 2017 4:12 pm

Please be positive and constructive with your questions and comments.