// 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);
      }
    }
  }
}