A little Toit experiment to validate correctness and of an AHRS algorithm, as well as responsiveness. The orientation is broadcasted on the network using UDP.

The code is running on an ESP32 using the ICM20948 9-axis chip.

I made a small Unity3D viewer. It allowed me to, in real time, change orientation and validate that the orientation is as expected:

The main loop, running at 100Hz, is simply reading the latest accel/gyro and updating the AHRS algorithm.

  every UPDATE_RATE:
    accel := to_vector3 driver.read_accel
    gyro := to_vector3 driver.read_gyro

    madgwick.update_imu
      gyro * RAD_PER_DEG
      accel
      UPDATE_RATE

The network part is running in a different task, sending the 4 Quaternion parts on the UDP broadcast socket. Here the update rate is 20Hz.

  task::
    every BROADCAST_RATE:
      rotation := madgwick.rotation
      data := json.encode {
        "x": rotation.x,
        "y": rotation.y,
        "z": rotation.z,
        "w": rotation.w,
      }
      socket.send
        udp.Datagram data BROADCAST_ADDRESS

The MonoBehavior for Unity3D is available here.