Nicla Sense ME Orientation Problems

[DNez2001]

I have mounted a Nicla Sense ME on a simple platform, with USB on the underside of the board. I have setup a simple 15 sec logging program for the rotation (SENSOR_ID_RV). I have performed simple pitch, roll and heading directional movements in accordance with the X-Y-Z graphic on the top of the board, to try and orientate myself to the Nicla coordinate system. After logging the data at 100 Hz for 15 sec and porting the data to Excel using Putty, I normalized and converted the data to Euler angles in degrees using

// Normalize quaternion
double norm = sqrt(rot_W* rot_W+ rot_X* rot_X+ rot_Y* rot_Y+ rot_Z* rot_Z);
double invNorm = 1.0 / norm;
rot_W*= invNorm;
rot_X*= invNorm;
rot_Y*= invNorm;
rot_Z*= invNorm;

// Compute Euler angles in radians
roll = atan2(2 * (rot_W* rot_X+ rot_Y* rot_Z), 1 - 2 * (rot_X* rot_X+ rot_Y* rot_Y));
pitch = asin(2 * (rot_W* rot_Y- rot_Z* rot_X));
heading = atan2(2 * (rot_W* rot_Z+ rot_X* rot_Y), 1 - 2 * (rot_Y* rot_Y+ rot_Z* rot_Z));

// Convert to degrees
roll = roll * 180.0 / M_PI;
pitch = pitch * 180.0 / M_PI;
heading = heading * 180.0 / M_PI;

When I've reviewed the data in Excel, I find the following conclusions:

• the X and Y axis as printed on the board are backwards. Pitch (normally rot_x) is rot_y, Roll (normally rot_y) is rot_x.
• Pitch* (corrected) is 0° when USB is pointed downward, and +90° when laying flat on the table with USB on the underside
• Roll* (corrected) is 0° when the board is laying flat on the table and the USB is on the underside
• when USB is pointed north, heading is 90° (instead of 0°); when USB is pointed south, heading is -90° (instead of 180°); clockwise rotation moves the direction positively (+°)

Can you see any problems with what I am concluding? I'll admit I'm new at this. My sketch is below

#include <Arduino_BHY2.h> // BHY2 sensor library
#include <Nicla_System.h> //Nicla system library
#include <LibPrintf.h> // Printf function library

// ///////////////////////////////////////////////////////////////////////////

#define FREQUENCY_HZ    (100) // 10 Hz
#define INTERVAL_MS     (1000 / FREQUENCY_HZ)
#define SAMPLE_NO       (1500)

#define ACCEL_RANGE     (4) // up to 4g
#define GYRO_RANGE      (2000) // up to 2000 dps

// ///////////////////////////////////////////////////////////////////////////
// Local variables

// SensorXYZ accel(SENSOR_ID_ACC); //returns xyz data
// SensorXYZ gyro(SENSOR_ID_GYRO); //returns xyz data
// SensorXYZ mag(SENSOR_ID_MAG); //returns xyz data
SensorQuaternion rot(SENSOR_ID_RV); //returns xyz data
SensorQuaternion orient(SENSOR_ID_ORI); //returns xyz data

static unsigned long printTime;
static unsigned int sampleCount = 0; // number of samples taken
bool dataCol = false; // New variable to track data collection status

// ///////////////////////////////////////////////////////////////////////////

void setup()
{
  // Initialize Serial Port for debug info
  Serial.begin(115200);
  while(!Serial); //waits until serial com established before proceeding

  // Initialize Sensor Modules
  Serial.print("Initializing the sensors...");
    nicla::begin();
    nicla::leds.begin();
    BHY2.begin(NICLA_I2C);
    // accel.begin();
    // gyro.begin();
    // mag.begin();
    rot.begin();
    orient.begin();
  Serial.println("done");

  printTime = millis();

  // Print CSV header 
  printf("timestamp, rot_X, rot_Y, rot_Z, rot_W, orient_x, orient_y, orient_z, orient_w, sampleCount\n");

  // Turn on LED to blinking yellow for 5 sec delay
  for (int i = 0; i < 10; i++) {
    nicla::leds.setColor(red);
    delay(500); // Yellow on for 0.5 seconds
    nicla::leds.setColor(off);
    delay(500); // Yellow off for 0.5 seconds
  }

  // Change LED color to green and indicate data collection has started
  nicla::leds.setColor(green);
  dataCol = true;
}

// ///////////////////////////////////////////////////////////////////////////

void loop()
{
  auto currentTime  = millis();

  // Update function should be continuously polled
  BHY2.update();

  // Set Interval Time and Check sensor values
  if (sampleCount < SAMPLE_NO && currentTime - printTime >= INTERVAL_MS) {
    printTime = currentTime;
    sampleCount++;    

    printf("%lu,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%d\n",
      printTime,
      rot.x(), rot.y(), rot.z(), rot.w(),
      orient.x(), orient.y, orient.z, orient.w(),
      sampleCount
      );
  }

  if (sampleCount >= SAMPLE_NO) {
    nicla::leds.setColor(off);
    dataCol = false;
  }

}