I have 6 IMUs (FXAS21002 & FXOS87000) that I'm reading data from, so I use a multiplexer (TCA9548a) to switch between them. I put the readings of the IMUs in a dataframe and the send it to the serial port. The code can be seen below:
Code: Select all
#include <Adafruit_FXOS8700.h>
#include <Adafruit_FXAS21002C.h>
#include <Adafruit_Sensor.h>
#include "Wire.h"
#define TCAADDR 0x70
#define FRAMESIZE (45) //(1 + 4 + 4*3 + 4*3 + 4 + 4*3)
// accmag timestamp uint long + 3 floats acc + 3 floats mag + gyro timestamp uint long
#define NUMSENSORS 6
/* Assign a unique ID to this sensor at the same time */
Adafruit_FXOS8700 accelmag0 = Adafruit_FXOS8700(0x8700A, 0x8700B);
Adafruit_FXAS21002C gyro0 = Adafruit_FXAS21002C(0x0021002C);
byte dataframe[FRAMESIZE] ={0x00};
unsigned long timestamp;
void tcaselect(uint8_t i) {
if (i > 7) return;
Wire.beginTransmission(TCAADDR);
Wire.write(1 << i);
Wire.endTransmission();
}
void setup(void) {
/* Wait for the Serial Monitor */
while (!Serial) {
delay(1000);
}
// Wire.begin();
Serial.begin(115200);
for (uint8_t i=0; i<NUMSENSORS; i++) {
Serial.println("NXPsensor Test Accel+Gyro Test");
Serial.println("");
tcaselect(i);
/* Initialise the sensor */
if (!gyro0.begin()) {
/* There was a problem detecting the FXAS21002C ... check your connections
*/
Serial.println("Ooops, no FXAS21002C detected ... Check your wiring!");
while (1);
}
/* Initialise the sensor */
if (!accelmag0.begin(ACCEL_RANGE_4G)) {
/* There was a problem detecting the FXOS8700 ... check your connections */
Serial.println("Ooops, no FXOS8700 detected ... Check your wiring!");
while (1)
;
}
}
timestamp = micros();
}
// https://arduino.stackexchange.com/questions/60863/print-byte-array-in-serial-monitor-screen-of-arduino-ide
//void printHex(uint8_t num) {
// char hexCar[2];
//
// sprintf(hexCar, "%02X", num);
// Serial.print(hexCar);
//}
void loop(void) {
// // Set sr to 100 Hz.
if ( micros() - timestamp <10000 - 10) {
// test micros()-timestamp>10000 ? delay( (micros()- timestamp-1000)/1000 : ; // delay for something less than time remaining only if there is a lot of time left
return;
}
timestamp = micros();
for (uint8_t i=0; i<NUMSENSORS; i++) {
tcaselect(i); //Change sensor
sensors_event_t aevent, mevent, gevent; //Nikos
/* Get a new sensor event */
accelmag0.getEvent(&aevent, &mevent);
gyro0.getEvent(&gevent);
/* dataframe:
* |UINT8_T| ULONG | FLOAT | FLOAT | FLOAT | FLOAT | FLOAT | FLOAT | ULONG | FLOAT | FLOAT | FLOAT |
* | - |- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|
* |sensor | accts | acc.x | acc.y | acc.z | mag.x | mag.y | mag.z | gyrts | gyr.x | gyr.y | gyr.z |
* | # | μs | m/s^2 | m/s^2 | m/s^2 | uT | uT | uT | μs | rad/s | rad/s | rad/s |
*
*/
dataframe[0] = i;
memcpy(&dataframe[1], &aevent.timestamp, 4);
memcpy(&dataframe[5], &aevent.acceleration, 12);
memcpy(&dataframe[17], &mevent.magnetic, 12);
memcpy(&dataframe[29], &gevent.timestamp, 4);
memcpy(&dataframe[33], &gevent.gyro, 12);
Serial.println("");
Serial.write(dataframe, FRAMESIZE);
}
// delay(10);
}
Code: Select all
#include <Adafruit_FXOS8700.h>
#include <Adafruit_FXAS21002C.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <EEPROM.h>
#define TCAADDR 0x70
#define FRAMESIZE (45) //(1 + 4 + 4*3 + 4*3 + 4 + 4*3)
// accmag timestamp uint long + 3 floats acc + 3 floats mag + gyro timestamp uint long
#define NUMSENSORS 6
/* Assign a unique ID to this sensor at the same time */
Adafruit_FXOS8700 accelmag0 = Adafruit_FXOS8700(0x8700A, 0x8700B);
Adafruit_FXAS21002C gyro0 = Adafruit_FXAS21002C(0x0021002C);
byte dataframe[FRAMESIZE] ={0x00};
unsigned long timestamp;
template <class T> int EEPROM_readAnything(int ee, T& value)
{
byte* p = (byte*)(void*)&value;
int i;
for (i = 0; i < sizeof(value); i++)
*p++ = EEPROM.read(ee++);
return i;
}
void getCalibration(int noIMU, double calibration[]){
unsigned int baseAddrTest = noIMU*12*4;
for (int i=0; i <= 11; i++){
double val;
int addr = (i*4)+baseAddrTest;
EEPROM_readAnything( addr, val);
calibration[i];
}
return;
}
bool calibrate_mag(sensors_event_t &mag_event, int noIMU){
double calibration[12];
getCalibration(noIMU, calibration);
float mx = mag_event.magnetic.x - calibration[0];
float my = mag_event.magnetic.y - calibration[1];
float mz = mag_event.magnetic.z - calibration[2];
mag_event.magnetic.x = mx * calibration[3] + my * calibration[6] + mz * calibration[7];
mag_event.magnetic.y = mx * calibration[6] + my * calibration[4] + mz * calibration[8];
mag_event.magnetic.z = mx * calibration[7] + my * calibration[8] + + mz * calibration[5];
}
bool calibrate_gyro(sensors_event_t &gyro_event, int noIMU){
double calibration[12];
getCalibration(noIMU, calibration);
gyro_event.gyro.x -= calibration[9];
gyro_event.gyro.y -= calibration[10];
gyro_event.gyro.z -= calibration[11];
return true;
}
void tcaselect(uint8_t i) {
if (i > 7) return;
Wire.beginTransmission(TCAADDR);
Wire.write(1 << i);
Wire.endTransmission();
}
void setup(void) {
Serial.begin(115200);
/* Wait for the Serial Monitor */
while (!Serial) {
delay(1000);
}
Wire.begin();
for (uint8_t i=0; i<NUMSENSORS; i++) {
delay(100);
Serial.println("NXPsensor Test Accel+Gyro Test");
Serial.println("");
tcaselect(i);
/* Initialise the sensor */
if (!gyro0.begin()) {
/* There was a problem detecting the FXAS21002C ... check your connections
*/
Serial.println("Ooops, no FXAS21002C detected ... Check your wiring!");
while (1)
;
}
/* Initialise the sensor */
if (!accelmag0.begin(ACCEL_RANGE_4G)) {
/* There was a problem detecting the FXOS8700 ... check your connections */
Serial.println("Ooops, no FXOS8700 detected ... Check your wiring!");
while (1)
;
}
}
timestamp = micros();
}
// https://arduino.stackexchange.com/questions/60863/print-byte-array-in-serial-monitor-screen-of-arduino-ide
//void printHex(uint8_t num) {
// char hexCar[2];
//
// sprintf(hexCar, "%02X", num);
// Serial.print(hexCar);
//}
void loop(void) {
// // Set sr to 100 Hz.
if ( micros() - timestamp <10000 - 10) {
// test micros()-timestamp>10000 ? delay( (micros()- timestamp-1000)/1000 : ; // delay for something less than time remaining only if there is a lot of time left
return;
}
timestamp = micros();
for (uint8_t i=0; i<NUMSENSORS; i++) {
tcaselect(i); //Change sensor
sensors_event_t aevent, mevent, gevent; //Nikos
/* Get a new sensor event */
accelmag0.getEvent(&aevent, &mevent);
gyro0.getEvent(&gevent);
calibrate_gyro(gevent, i);
calibrate_mag(mevent, i);
/* dataframe:
* |UINT8_T| ULONG | FLOAT | FLOAT | FLOAT | FLOAT | FLOAT | FLOAT | ULONG | FLOAT | FLOAT | FLOAT |
* | - |- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|- - - -|
* |sensor | accts | acc.x | acc.y | acc.z | mag.x | mag.y | mag.z | gyrts | gyr.x | gyr.y | gyr.z |
* | # | μs | m/s^2 | m/s^2 | m/s^2 | uT | uT | uT | μs | rad/s | rad/s | rad/s |
*
*/
dataframe[0] = i;
memcpy(&dataframe[1], &aevent.timestamp, 4);
memcpy(&dataframe[5], &aevent.acceleration, 12);
memcpy(&dataframe[17], &mevent.magnetic, 12);
memcpy(&dataframe[29], &gevent.timestamp, 4);
memcpy(&dataframe[33], &gevent.gyro, 12);
Serial.println("");
Serial.write(dataframe, FRAMESIZE);
}
// delay(10);
}
I checked if the IMUs are visible to the Arduino by using the Adafruit's TCA scanner function and it can detect the sensors. The scanner script and the results are:
Code: Select all
/**
* TCA9548 I2CScanner.ino -- I2C bus scanner for Arduino
*
* Based on https://playground.arduino.cc/Main/I2cScanner/
*
*/
#include "Wire.h"
#define TCAADDR 0x70
void tcaselect(uint8_t i) {
if (i > 7) return;
Wire.beginTransmission(TCAADDR);
Wire.write(1 << i);
Wire.endTransmission();
}
// standard Arduino setup()
void setup()
{
while (!Serial);
delay(1000);
Wire.begin();
Serial.begin(115200);
Serial.println("\nTCAScanner ready!");
for (uint8_t t=0; t<8; t++) {
tcaselect(t);
Serial.print("TCA Port #"); Serial.println(t);
for (uint8_t addr = 0; addr<=127; addr++) {
if (addr == TCAADDR) continue;
Wire.beginTransmission(addr);
if (!Wire.endTransmission()) {
Serial.print("Found I2C 0x"); Serial.println(addr,HEX);
}
}
}
Serial.println("\ndone");
}
void loop()
{
TCA Port #0
Found I2C 0x1F
Found I2C 0x21
TCA Port #1
Found I2C 0x1F
Found I2C 0x21
TCA Port #2
Found I2C 0x1F
Found I2C 0x21
TCA Port #3
Found I2C 0x1F
Found I2C 0x21
TCA Port #4
Found I2C 0x1F
Found I2C 0x21
TCA Port #5
Found I2C 0x1F
Found I2C 0x21
TCA Port #6
TCA Port #7
*
Any help would be really appreciated.