// NMEA 9dof sensor https://wokwi.com/projects/474932890527678465
// Intended to translate a 9DOF into NMEA sentences for
// the gpsd server https://gpsd.io/
// DaveX 2026-09-12
// Based on the HMC5883L chip from https://wokwi.com/projects/472374946341413889
// adafruit MPU6050
// See other custom Wokwi chips at
// https://forum.arduino.cc/t/wokwi-simulator-custom-chips/1292759
// See https://github.com/bareboat-necessities/ocean-imu for a much more capable system
/***************************************************************************
This is a library example for the HMC5883 magnentometer/compass
Designed specifically to work with the Adafruit HMC5883 Breakout
http://www.adafruit.com/products/1746
*** You will also need to install the Adafruit_Sensor library! ***
These displays use I2C to communicate, 2 pins are required to interface.
Adafruit invests time and resources providing this open source code,
please support Adafruit andopen-source hardware by purchasing products
from Adafruit!
Written by Kevin Townsend for Adafruit Industries with some heading example from
Love Electronics (loveelectronics.co.uk)
This program is free software: you can redistribute it and/or modify
it under the terms of the version 3 GNU General Public License as
published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***************************************************************************/
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h> // https://github.com/adafruit/Adafruit_HMC5883_Unified
#include <Adafruit_MPU6050.h> // https://github.com/adafruit/Adafruit_MPU6050/tree/master
// assume MPU6050 orientation aligns with the board and Invesense chip:
// +X,+y,+z is Right,Up the page, Out of the page
// per
// https://docs.wokwi.com/parts/wokwi-mpu6050
// and https://github.com/wokwi/wokwi-features/issues/1138
// HMC 5883 orientation from
// https://cdn.sparkfun.com/datasheets/Components/General%20IC/20100524052222_HMC5883.pdf
// the compass chip, with the pin1 corner oriented in the NorthWest/upper left corner,
// has +m_x east/right, +m_y south/downpage, +m_z up/out of the page
//
// using https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml#igrfwmm to find the magnetic field in VA, USA
// I get +N,+E,+D values of 21.7, -4.09, 43.8 uT,
// and with the Y axis of the chip aimed north, X to the east that makes atan2(21.7,-4.09)=100.67 deg,
// or Azimuth=90-100.67=-10.67 or 10.67 deg west declination, consistent with the local declination
// Az = 90-atan2(Y,X)
// Note that with +D =43.9 is the opposite direction of a x=east=-4.09, y=north=21.7, Z=up=-(D=43.8) orientation,
// so a level chip with Y pointing north should read about (x,y,z()=(-4.09,21.7,-43.8)uT
// E,N,Up is the other system from NEDown.
Adafruit_MPU6050 mpu;
/* Assign a unique ID to this sensor at the same time */
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
const bool printNMEA = true;
const bool printHCHDG = true;
const bool printPASHR = true; // https://help.fieldsystems.trimble.com/r10/nmea0183-messages-pashr.htm
const bool printGYOACC = true; // https://en.unicore.com/uploads/file/UFB2%20Protocol/UFBII%20Protocol%20Specification%20EN/Unicore%20Messages/Sensor%20Fusion%20Messages/03-4-01%20GYOACC.html
const bool printText = true;
const uint32_t IntervalMs = 1000;
void displaySensorDetails(void)
{
sensor_t magsensor;
mag.getSensor(&magsensor);
Serial.println("------------------------------------");
Serial.print ("Sensor: "); Serial.println(magsensor.name);
Serial.print ("Driver Ver: "); Serial.println(magsensor.version);
Serial.print ("Unique ID: "); Serial.println(magsensor.sensor_id);
Serial.print ("Max Value: "); Serial.print(magsensor.max_value); Serial.println(" uT");
Serial.print ("Min Value: "); Serial.print(magsensor.min_value); Serial.println(" uT");
Serial.print ("Resolution: "); Serial.print(magsensor.resolution); Serial.println(" uT");
Serial.println("------------------------------------");
Serial.println("");
delay(500);
}
struct Nmea {
// Acc in m/s
float a_x;
float a_y;
float a_z;
// gyro in deg/sec
float g_x;
float g_y;
float g_z;
// magnetometer in ??
float m_x;
float m_y;
float m_z;
float pitch;
float roll;
float heading;
static constexpr double DEG = M_PI/180.0;
void update(){
pitch = atan2(a_y, sqrt(a_z * a_z + a_x * a_x)) / DEG;
roll = atan2(a_x, sqrt(a_z * a_z + a_y * a_y)) / DEG;
heading = headingDegrees();
}
byte calcNmeaChecksum(char str[], size_t len) {
byte checksum = 0;
int ii = 0;
while (ii < len) {
if (str[ii++] == '$') break;
}
while (ii < len) {
if (str[ii] == '*') break;
checksum ^= str[ii];
++ii;
}
return checksum;
}
float headingDegrees() {
float heading = atan2(m_y, m_x) * 180 / M_PI;
return ( heading + heading > 0 ? 0 : 360);
}
void printHCHDG() {
// HeadingCompass mag,dev,var
// http://shantek.net/gps/NMEA_SentenceDecoding.html#hchdg
// $HCHDG,101.1,,,7.1,W*3C
const size_t BUFSZ = 128;
char buf[BUFSZ];
byte checksum = 0;
snprintf(buf, BUFSZ, "$HCHDG,%.2f,,,,*%02X", headingDegrees(), checksum);
checksum = calcNmeaChecksum(buf, 20);
snprintf(buf, BUFSZ, "$HCHDG,%.2f,,,,*%02X", headingDegrees(), checksum);
Serial.println(buf);
}
void printPASHR() {
}
} nmea;
void setup(void)
{
Serial.begin(9600);
Serial.println("HMC5883 Magnetometer Test"); Serial.println("");
if (!mpu.begin()) {
Serial.println("Failed to find MPU6050 chip");
while (1) {
delay(10);
}
}
/* Initialise the sensor */
if (!mag.begin())
{
/* There was a problem detecting the HMC5883 ... check your connections */
Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
while (1);
}
mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
mpu.setGyroRange(MPU6050_RANGE_250_DEG);
mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
/* Display some basic information on this sensor */
displaySensorDetails();
}
void loop(void)
{
static uint32_t lastReport = -IntervalMs;
uint32_t now = millis();
if (now - lastReport >= IntervalMs) {
lastReport += IntervalMs;
/* Get a new sensor event */
sensors_event_t a, g, temp, event;
mag.getEvent(&event);
mpu.getEvent(&a, &g, &temp);
// Hold the module so that Z is pointing 'up' and you can measure the heading with x&y
// Calculate heading when the magnetometer is level, then correct for signs of axis.
//
// See https://youtu.be/nwHKobJ8ndk?t=294 demo the behavior:
// X+ y small at x=north
// X=y at 045
// X~0 Y+ at 090
// ...
float heading = atan2(event.magnetic.y, event.magnetic.x);
// Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
// Find yours here: http://www.magnetic-declination.com/
// Mine is: -13* 2' W, which is ~13 Degrees, or (which we need) 0.22 radians
// If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
const float DEG = M_PI / 180; // radians in a degree
float declinationAngle = 0.00 * DEG;
heading += declinationAngle;
// Correct for when signs are reversed.
if (heading < 0)
heading += 2 * PI;
// Check for wrap due to addition of declination.
if (heading > 2 * PI)
heading -= 2 * PI;
// Convert radians to degrees for readability.
float headingDegrees = heading / DEG;
// populate nmea with 9dof converted to ??, m/s, deg/sec
nmea.m_x = event.magnetic.x;
nmea.m_y = event.magnetic.y;
nmea.m_z = event.magnetic.z;
nmea.a_x = a.acceleration.x;
nmea.a_y = a.acceleration.y;
nmea.a_z = a.acceleration.z;
nmea.g_x = g.gyro.x/DEG;
nmea.g_y = g.gyro.y/DEG;
nmea.g_z = g.gyro.z/DEG;
nmea.update(); // calculate magnetic heading, pitch, roll, etc
if (printText) {
Serial.print("Mx:"); Serial.print(event.magnetic.x);
Serial.print(" My:"); Serial.print(event.magnetic.y);
Serial.print(" Mz:"); Serial.print(event.magnetic.z);
Serial.print(" Ax:");
Serial.print(a.acceleration.x);
Serial.print(" Ay:");
Serial.print(a.acceleration.y);
Serial.print(" Az:");
Serial.print(a.acceleration.z);
Serial.print(" Rx:");
Serial.print(g.gyro.x);
Serial.print(" Ry:");
Serial.print(g.gyro.y);
Serial.print(" Rz:");
Serial.print(g.gyro.z);
Serial.print(" T:");
Serial.print(temp.temperature);
Serial.print(" Heading:"); Serial.print(headingDegrees);
Serial.println();
}
if (printNMEA) {
// NMEA0183 HeadingCompass HeaDinG Dev,EW,Var,EW output
// per https://www.simrad.online/fisherysounder/help/online_help_en/_interface_specifications/GUID-510497DA-8885-4E15-81EA-D46D01FCCB42.html
const size_t BUFSZ = 256;
char buf[BUFSZ];
byte checksum = 0;
if (printHCHDG) { // HeadingCompass mag,dev,var
// http://shantek.net/gps/NMEA_SentenceDecoding.html#hchdg
// $HCHDG,101.1,,,7.1,W*3C
snprintf(buf, BUFSZ, "$HCHDG,%.2f,,,,*%02X", headingDegrees, checksum);
checksum = nmea.calcNmeaChecksum(buf, 20);
snprintf(buf, BUFSZ, "$HCHDG,%.2f,,,,*%02X", headingDegrees, checksum);
Serial.println(buf);
}
if (printPASHR) {
// NMEA compass heading string to be compatible with gpsd
// https://docs.novatel.com/OEM7/Content/SPAN_Logs/PASHR.htm
// $PASHR,163029.000,158.09,T,-0.30,+0.31,+0.01,0.029,0.029,0.059,1,1*3B
// hhmmss.sss,HHH.HH,M,-R.OL,+P.IT,H.EAV,r.sdv,p.sdv,hsd,,,*CK
float pitch = atan2(a.acceleration.y, sqrt(a.acceleration.z * a.acceleration.z + a.acceleration.x * a.acceleration.x)) / DEG;
float roll = atan2(a.acceleration.x, sqrt(a.acceleration.z * a.acceleration.z + a.acceleration.y * a.acceleration.y)) / DEG;
snprintf(buf, BUFSZ, "$PASHR,,%.2f,M,%+.2f,%+.2f,,,,,,*%02X",
headingDegrees, roll, pitch, checksum);
checksum = nmea.calcNmeaChecksum(buf, BUFSZ);
snprintf(buf, BUFSZ, "$PASHR,,%.2f,M,%+.2f,%+.2f,,,,,,*%02X",
headingDegrees, roll, pitch, checksum);
Serial.println(buf);
}
if (printGYOACC) {
// NMEA gyro accelerometer string to be compatible with gpsd
// https://en.unicore.com/uploads/file/UFB2%20Protocol/UFBII%20Protocol%20Specification%20EN/Unicore%20Messages/Sensor%20Fusion%20Messages/03-4-01%20GYOACC.html
//
// $GYOACC,050624,002133.10,0.004634,0.000273,0.004348,100,-4.666065,
// -3.466573,7.960348,100,31,0,100,0*02
// YYMMDD,hhmmss.ss,Gx,Gy,Gz,*CK
snprintf(buf, BUFSZ, "$GYOACC,,,%.6f,%.6f,%.6f,,%.6f,%.6f,%.6f,,%d,,,*%02X",
g.gyro.x / DEG, g.gyro.y / DEG, g.gyro.z / DEG, a.acceleration.x, a.acceleration.y, a.acceleration.z, int(temp.temperature + 0.5), checksum);
checksum = nmea.calcNmeaChecksum(buf, BUFSZ);
snprintf(buf, BUFSZ, "$GYOACC,,,%.6f,%.6f,%.6f,,%.6f,%.6f,%.6f,,%d,,,*%02X",
g.gyro.x / DEG, g.gyro.y / DEG, g.gyro.z / DEG, a.acceleration.x, a.acceleration.y, a.acceleration.z, int(temp.temperature + 0.5), checksum);
Serial.println(buf);
}
}
}
}
Assume VCC aligns with forward,
MCU6050 text reads left to right
HMC5883L