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
/*
* This sketch is a stand-alone Nicla Sense ME app to
*/
#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;
}
}
// ///////////////////////////////////////////////////////////////////////////