Nicla Sense Me - Sensor Fusion quaternion orientation for THREE.js

[MichaelBell]

Hello,

I recently acquired a Nicla Sense Me. I have created an ardunio sketch that sends quaternions via Bluetooth to a browser. I am then applying the quaternion's to a mesh in ThreeJS.

Everything seems to work nicely but I think the orientation is off which I suspect may be the coordinate system? But If there are any suggestions I would be happy to hear them.

I can provide a github repo if needed. (Also added an image with the coordinate system for the bosch component)
Below are the Ardunio and basic web setup

// ARDUNIO EXAMPLE
#include "Arduino.h"
#include "Arduino_BHY2.h"
#include <ArduinoBLE.h>

SensorQuaternion quaternion(SENSOR_ID_RV);

// BLE Service
BLEService imuService("e905de3e-0000-44de-92c4-bb6e04fb0212"); // Custom UUID

// BLE Characteristic
BLECharacteristic imuCharacteristic("917649A1-D98E-11E5-9EEC-0002A5D5C51B", BLERead | BLENotify, 16);


void setup()
{
  Serial.begin(115200);
  while(!Serial); // Remove when powered by battery
  BHY2.begin();
  quaternion.begin();

// Setup bluetooth
  if (!BLE.begin()) {
    Serial.println("starting BLE failed!");
    while (1);
  }
  BLE.setAdvertisedService(imuService);
  imuService.addCharacteristic(imuCharacteristic);
  BLE.addService(imuService);

  // start advertising
  BLE.advertise();
  Serial.println("Bluetooth device active, waiting for connections...");
}

void loop()
{
  // wait for a BLE central
  BLEDevice central = BLE.central();

  // if a BLE central is connected to the peripheral:
  if (central) {
    Serial.print("Connected to central: ");
    // print the central's BT address:
    Serial.println(central.address());

    // while the central is connected:
    while (central.connected()) {
      // Update function should be continuously polled
      BHY2.update();
  
        Serial.println(String("quaternion: ") + quaternion.toString());
        // quaternion
        float data[4];
        data[0] = quaternion.x();
        data[1] = quaternion.y();
        data[2] = quaternion.z();
        data[3] = quaternion.w();
        imuCharacteristic.setValue((byte *) &data, sizeof(data));
    }

    Serial.print("Disconnected from central: ");
    Serial.println(central.address());
   }
}

// WEB EXAMPLE
// Create an empty scene
    var scene = new THREE.Scene();

    // Create a basic perspective camera
    var camera = new THREE.PerspectiveCamera(75, window.innerWidth / window.innerHeight, 0.1, 1000);
    camera.position.z = 4;

    // Create a renderer with Antialiasing
    var renderer = new THREE.WebGLRenderer({ antialias: true });

    // Configure renderer clear color
    renderer.setClearColor("#000000");

    // Configure renderer size
    renderer.setSize(window.innerWidth, window.innerHeight);

    // Append Renderer to DOM
    document.body.appendChild(renderer.domElement);

    // Create a Cube Mesh with basic material
    var geometry = new THREE.BoxGeometry(1, 1, 1);
    var material = new THREE.MeshBasicMaterial({ color: "#433F81" });
    var cube = new THREE.Mesh(geometry, material);

    // Add cube to Scene
    scene.add(cube);

    // Render Loop
    var render = function () {
        requestAnimationFrame(render);

        cube.quaternion.set( quaternion[0], quaternion[1], quaternion[2], quaternion[3] );

        // Render the scene
        renderer.render(scene, camera);
    };

    render();

[MichaelBell]

I ended up figuring it out by using a utility to convert the threejs scene to the same coordinate system as the board

threejs-sandbox/basis-transform at master · gkjohnson/threejs-sandbox

// Setup matrix transform for nicla
    const matrix = new THREE.Matrix4();
    getBasisTransform('-Y+Z-X', '+X+Y+Z', matrix);

    const group = new THREE.Group();
    matrix.decompose(group.position, group.quaternion, group.scale);

    // Create an empty scene
    var scene = new THREE.Scene();

    // Create a basic perspective camera
    var camera = new THREE.PerspectiveCamera(75, window.innerWidth / window.innerHeight, 0.1, 1000);
    camera.position.z = 4;

    // Create a renderer with Antialiasing
    var renderer = new THREE.WebGLRenderer({ antialias: true });

    // Configure renderer clear color
    renderer.setClearColor("#000000");

    // Configure renderer size
    renderer.setSize(window.innerWidth, window.innerHeight);

    // Append Renderer to DOM
    document.body.appendChild(renderer.domElement);

    // Create a Cube Mesh with basic material
    var geometry = new THREE.BoxGeometry(1, 1, 1);
    for (var i = 0; i < geometry.faces.length; i += 2) {
        var color = {
            h: (1 / (geometry.faces.length)) * i,
            s: 0.5,
            l: 0.5
        };
        geometry.faces[i].color.setHSL(color.h, color.s, color.l);
        geometry.faces[i + 1].color.setHSL(color.h, color.s, color.l);
    }
    var material = new THREE.MeshBasicMaterial({
        vertexColors: THREE.FaceColors,
        overdraw: 0.5
    });
    var cube = new THREE.Mesh(geometry, material);
    const axisHelper = new THREE.AxisHelper(2);
    cube.add(axisHelper)
    // Add cube to Scene
    scene.add(cube);
    scene.applyMatrix(matrix)

    // Render Loop
    var render = function () {
        requestAnimationFrame(render);
        cube.quaternion.fromArray(quaternion);

        // Render the scene
        renderer.render(scene, camera);
    };

    render();

[iridiu]

Hi Michael, did you succed to have a working Three.js sketch with Arduino Nicla?