Project in progress
The NMEAtor

The NMEAtor © GPL3+

If you can't fix it in software you'll have to fix it in hardware in this case. Hence the NMEAtor.

  • 4 views
  • 0 comments
  • 0 respects

Components and supplies

Necessary tools and machines

Apps and online services

Ide web
Arduino IDE
tinkercad
Great simulation tool for Arduino
circuitio app
Another helpful tool for breadboarding with Arduino

About this project

Introduction:

I've an old Robertson data network on board feeding the on board devices with NMEA0183 v1.5. Not only is this RS-232 related, but the NMEA sentences are not terminated with a checksum. Most navigation apps don't care and checksum checking can be overridden by the user. There is how ever a newer NMEA0183 v2.0 related data stream for GPS and AIS information that needs to be integrated with the old data stream, better known as multiplexing.

Fig. 1: Current situation on board

I initially went for the easy way out and burned some Euro's on a commercial multiplexer, which is a great product from YachtDevices; the YDWR02 (4 NMEA inputs and 4 NMEA outputs). It even offers the option to pass through the old V1.5 NMEA0183 data. Unfortunately with this option activated the data can not be filtered out and that is where the problem, well the inconvenience, started. Multiplexing the the 2 data steams results in a frozen HDG at 2 degrees in the navigation app on the iPad. Since this is a given fact, I need to work on the other end of the wire and manipulate the old NMEA0183 data before it is send to the iPad. Fortunately there is a workaround and I can use the equipment, but without the old data stream resulting in only GPS and AIS info ( or all data with a frozen HDG at 2 degrees North)

Since I don't have a sensor or valid data available telling the magnetic or true heading I decided to integrate an MPU-9250 - GY9250 unit; a 9 DOF thingy with a digital compass, accelerometer and gyroscope.

All this also gives me the opportunity to design a user interface for all the incoming data like a Multi Function Display. Hence the 3, 5" TFT touchscreen.

The Arduino multiplexer project on King Tide Sailing triggered me and here I am. Never soldered a pin on a PCB before and yet there are a lot of them waiting to be soldered like a pro.

The plan:

Well there are several options:

  • the easy way out: read the NMEA0183 data stream from the Robertson Databox into the Arduino. Filter the data that I 'm interested in, add a checksum if necessary (GPS and AIS data contain a checksum). Extend the data with the data coming from the MPU 9250 unit and pass it through to the multiplexer ( adding the AIS data) which sends it via WiFi to the iPad. And create some nice gauges to show on the TFT display. Meaning several grouped pages with related data and a datalogger for the NMEA data. Or even trend data for the average speed and wind etc...
  • the hard way: sell the YDWR02 and do all the multiplexing via the Arduino and add WiFi capabilities to it so it can act as a client in my boat network.

Either way I'll follow the MoSCoW principle; Must, Should, Could and Would haves. Manipulating the original NMEA0183 v1.5 datastream is must a have. Integrating the 9 DoF thingy is a Should have and adding the TF display features is a Could have. The replacement of the current multiplexer by the Arduino is a Would or Wish to have for now.

Since I've a strong OO-background and the Arduino is merely sequential or loop based I'm looking into a project from Kevin Gagnon (based on a project from Alan Burlison) to implement an event driven architecture as much as possible into this project.

Status:

Currently I'm waiting for all the ordered items to arrive; ETA is end of May.

As per Apr 30st, 2020 the pin headers and the 9 DoF thingy have been delivered.May 1, 2020; soldering tin has been delivered

May 6, 2020: The PCB solder buddy and RS-422/485 to TTL converter have been delivered

Intermediate challenge:

From the old Robertson Databox there are 5 wires available for communication; Red (+ 12V) Brown (Rx), White (Tx), Green (Gnd) and Black (-0V).

This looks suspiciously like the RS-232 wiring as used in NMEA0183 in the past. Voltages range between + 12V and -12V for reps. 0 and 1. I have always used it like this in the network and it has always worked like this without any problems.

However…. the Arduino works with voltage between 0 and 3.3 or 5 V (you can choose) and 12V makes a toast of my Arduino.

This problem can be solved by adding an RS-232 <-> TTL converter, but then you have to make sure that you really have RS-232. Because the newer version of NMEA0183 works with RS-422 and the voltage is between 0 and 5 V (formally even between 0 and 10V). So I took a voltmeter to measure the wires and see the voltage fluctuate between 0 and 3.5V. So no negative voltages, which in itself would mean that it is based on RS-422 protocol.

So I ordered a multimeter with an oscilloscope(see here for extensive review) function to measure and analyze the signal and then determine whether I need an RS-232 <-> TTL converter or an RS-422.485 <-> TTL converter….

May 7, 2020, Yeahhh the Mega has arrived including " Die inoffizielle Anleitung zum Arduino!" and the board is working. Still waiting for the Buck step down converter to drive the Arduino with 7V (coming from 12V), the solder station, the RS-232 <-> TTL converter and the 810 pcs component kit

May 9, 2020. The desoldering thread and the Buck step down converter has arrived.

While waiting on the deliveries I've started building the wireframe for the application.

May 10, 2020. The NMEA parser (the NMEAtor) is working fine. The code contains some testsoftware (that can be disabled by outcommenting #define TEST) that can send NMEAsentences. I've selected 10 sentences from my log to test against.

May 11, 2020. The multimeter with oscilloscope view has been delivered and I jumped into the boat to do some measurements on the NMEA signal. Based on what I saw I cam to the conclusion that the signal is between 0V and 3, 1V and that I'll use the RS 422/ 485 <-> TTL converter.

May 13, 2020. The header pins have arrived. And I started soldering the pins to the MPU9250 board with a gas heated soldering iron

Progress:

My first attempt to write object oriented software managed by a task scheduler resulted in a good design, but in lack of variable memory to store the variables. Limited space and an OO design is not a happy marriage. So I had to overhaul my design and I've the first version running, reading the MPU9250 heading filtered for tilt and pitch. And the old NMEA0183 V1.5 sentences with out checksum or invalid format have been successfully parsed...

May 17, 2020. During testing with the MPU I noticed that I got 0x73 back from the Who_AM_I call instead of the expected 0x71. The code works with a slight adjustment on the IF statement. But wondering why this happened I found datasheet for the MPU9255, which will give back the 0x73 value. The good news is that the library works without complaining. There are some rumours that the chips contains a bug in the calibration values as discussed on the blog of Sureshjoshi https://sureshjoshi.com/embedded/invensense-imus-what-to-know/. Need to dive into that...

May 18, 2020. The 830 pcs wiring kit has arrived; lots of leds, wires, resistors, capacitors, potentiometer, a 4n35, a 74HC595. Still waiting for the soldering iron, the RS232<->TTlL converters and the TFT display...

May 29, 2020. The RS-232<->TTL converters have arrived.

May 20, 2020. The 3, 5" TFT touchscreen has arrived.

May 22, 2020. Finally all parts are complete. In the meanwhile I've been working on the software to get the display working. So at this point I can start soldering the bits and pieces together and connect the prototype to the NMEA talker.

Well at least important to note is that I've switched the Arduino IDE for the Visual Studio Code with the PlatformIO plugin installed. So from now on the files will be.cpp files instead of.ino files.

Intermediate result:

May 23, 2020. The software is ready to get tested on board. The hardware needs to be soldered and then the whole prototype can be is a preliminary test. Whoohoo!

Progress:

Unfortunately the pre-FAT failed big time. Although I'd tested the principle of the the pieces of code, putting them to work in real life was a total different experience. A frustrating one at least.

The NMEA signal from the network couldn't be read from te port and neither did the signal coming from the output port breath some life in my network. I took me sometime to understand what I was doing wrong. And there here several....

One issue had to due with the inverting of the signal by the RS422<->TTL converter. I'm now using a digital pin (53) via Softserial, acting as a UART and inverting the signal.

As this being my first Arduino project, I had not fully understand how the Arduino communication is implemented. In short it a character by character principle. I also had made some quirky mindfucks resulting in bad code.

With the current version I can read NMEA data and display the data on screen of the TFT display. I'm using an NMEA Simulator that can send several NMEA sentences by WiFi or by RS-232 cable. At a certain point the communication stalls, but the Arduino is still working. Restarting the simulator fixes the problem. So I guess it's not my code causing the stall. Next thing to test is the communication with the network from the Arduino via Serial port 2.

2nd FAT has been successful on board. Lessens learned; you cannot simultaneously read and write to different softserial ports on different baud rates. So the software now reads on 4800Bd from Rx2 en write on 38400 Bd to port 50 via SoftSerial. The first important milestone has been accomplished.


Code

The multiplexer code for Arduino Meta 2560 R3Arduino
Work in progress, while waiting on the hardware deliveries
NMEAparser is working correctly as per May 10, 2020
Slidely Change in approach; HDM, HDG and VHW message are processed normally. Will be filtered out by multiplexer. MPU is now generating filtered heading data. Changed OO task based design to sequential design dur to lack of variable memory...
Software is ready for a pre-FAT on board as per May 23,2020.
pre-Fat failed big time. Software overhaul for listener and talking routines
June 9; 2nd FAT successful. Now reading NMEA sentences incoming on 4800 Bd while manipulating and writing NMEA sentences on 38400 Bd to multiplexer.
#include <Arduino.h>
/*
  Project:  Yazz_Multiplexer.ino, Copyright 2020, Roy Wassili
  Contact:  waps61 @gmail.com
  URL:      https://www.hackster.io/waps61
  Date:     30-04-2020
  Last
  Update:   06-06-2020
  Achieved: Successful 2nd FAT on board. This software now reads NMEA data  incomming on
            Rx2 and sends data to a external multiplexer via Pin 50 (and inverted) via SoftSerial
            
  Purpose:  Build an NMEA0183 manupulator and animator for on board of my sailing boat
            supporting following types of tasks:
            - Reading NMEA0183 v1.5 data without a checksum,
            - Getting new course and roll & pitch data from a MPU9250
            - Inject this new data into the datastream (aka multiplexing)

  NOTE:     NMEA encoding conventions in short
            An NMEA sentence consists of a start delimiter, followed by a comma-separated sequence
            of fields, followed by the character '*' (ASCII 42), the checksum and an end-of-line marker.
            i.e. <start delimiter><field 0>,<field 1>,,,<field n>*<checksum><end-of-linemarker>
            The start delimiter is either $ or !. <field 0> contains the tag and the remaining fields
            the values. The tag is normaly a 5 character wide identifier where the 1st 2 characters
            identify the talker ID and the last 3 identify the sentence ID.
            Maximum sentence length, including the $ and <CR><LF> is 82 bytes.

  Source: https://gpsd.gitlab.io/gpsd/NMEA.html#_nmea_0183_physical_protocol_layer


  Minimal Hardware setup MPU9255:
  MPU9255/6500 ------------- Arduino
  VCC ---------------------- 5V
  GND ---------------------- GND
  SDA ---------------------- SDA20
  SCL ---------------------- SCL21
  FSYNC--------------------- GND
  OPTIONAL:
  INT ---------------------- ?
 
  Serial is reserved for the MPU9250 communication
  Rx2 is reserved for the NMEA listener on 4800Bd
  Digital pin 50 (and 52) are reserved for NMEA talker via
  SoftSerial on 38400 Bd
  
  Hardware setup:

  Wiring Diagram (for RS-422 / RS-485 shifter)
  This is used in my case.
  NMEA-0183 | RS-422/485 Shifter | ARDUINO
    NMEA+   |     B              |
    NMEA-   |     A              |
            |     VCC            |  5V
            |     GND            |  Ground
            |     RE             |  Ground
            |     RO             |  Receive Pin

Wiring Diagram (for RS-232 to NMEA0183 device)
  Arduino   | NMEA device
     Pin 50 |  RX +   
     GND    |  RX - 
            |  GND (if isolated input available)

Set the pins to the correct ones for your development shield or breakout board.
This program use these 8bit data lines to the LCD,
pin usage as follow:
                  LCD_CS  LCD_CD  LCD_WR  LCD_RD  LCD_RST  SD_SS  SD_DI  SD_DO  SD_SCK 
Arduino Mega2560    A3      A2      A1      A0      A4      10     11     12      13                           

                  LCD_D0  LCD_D1  LCD_D2  LCD_D3  LCD_D4  LCD_D5  LCD_D6  LCD_D7  
Arduino Mega2560    8       9       2       3       4       5       6       7 

*Remember to set the pins to suit your display module!


---------------
Terms of use:
---------------
The software is provided "AS IS", without any warranty of any kind, express or implied,
including but not limited to the warranties of mechantability, fitness for a particular
purpose and noninfringement. In no event shall the authors or copyright holders be liable
for any claim, damages or other liability, whether in an action of contract, tort or
otherwise, arising from, out of or in connection with the software or the use or other
dealings in the software.

-----------
Warning:
-----------
Do NOT use this compass in situations involving safety to life
such as navigation at sea.  
        
TODO: 

Credit:   
*/

/*
    Include the necessary libraries
*/

#include <Wire.h>
//#include <EEPROM.h>
#include "quaternionFilters.h"
#include <MPU9250.h>  

#include <LCDWIKI_GUI.h>
#include <LCDWIKI_KBV.h>
#include <TouchScreen.h>

//*** Since the signal from the RS422-TTL converter is inverted
//*** a digital input is used as a software serial port because
//*** it can invert te signal back to its orignal pulse set
#include <SoftwareSerial.h>
/*
   Definitions go here
*/
#define VESSEL_NAME "YAZZ"
// *** Conditional Debug & Test Info to Serial Monitor
// *** by commenting out the line(s) below the debugger and or test statements will 
// *** be ommitted from the code
//#define DEBUG 1
//#define TEST 1
#define DISPLAY_ATTACHED 1
//#define MPU_ATTACHED 1

#define SAMPLERATE 115200

#define LISTENER_RATE 4800 // Baudrate for the listner
#define LISTENER_PORT 53   // SoftSerial port 
#define TALKER_RATE 38400  // Baudrate for the talker
#define TALKER_PORT 50     // SoftSerial port 2


//*** Some conversion factors
#define FTM  0.3048        // feet to meters
#define MTF  3.28084       // meters to feet
#define NTK  1.852         // nautical mile to km
#define KTN  0.5399569     // km to nautical mile

//*** The NMEA defines in totl 82 characters including the starting 
//*** characters $ or ! and the checksum character *, the checksum
//*** AND last but not least the <CR><LF> chacters.
//*** we define one more for the terminating '\0' character for char buffers
#define NMEA_BUFFER_SIZE 82 // According NEA0183 specs the max char is 82
#define NMEA_TERMINATOR "\r\n"

//*** The maximum number of fields in an NMEA string
//*** The number is based on the largest sentence MDA,
//***  the Meteorological Composite sentence
#define MAX_NMEA_FIELDS 21

#define STACKSIZE 10  // Size of the stack; adjust according use

#define TALKER_ID "AO"
#define VARIATION "1.57,E" //Varition in Lemmer on 12-05-2020, change 0.11 per year
//*** On my boat there is an ofsett of 0.2V between the battery monitor and what 
//*** is measured by the Robertson Databox
#define BATTERY_OFFSET 0.2 //Volts
//*** define NMEA tags to be used
//*** make sure you know your Talker ID used in the sentences
//*** In my case next to GP for navigation related sentences
//*** II is used for Integrated Instruments and
//*** PS is used for vendor specific tags like Stowe Marine
//*** AO is used for my Andruino generated sentences

/* for lab testing with an NMEA simulator tool
#define _DBK "$SDDBK"   // Depth below keel
#define _DBS "$SDDBS"   // Depth below surface
#define _DBT "$SDDBT"   // Depth below transducer
*/
#define _DBK "$IIDBK"   // Depth below keel
#define _DBS "$IIDBS"   // Depth below surface
#define _DBT "$IIDBT"   // Depth below transducer
#define _HDG "$IIHDG"   // Heading  Deviation & Variation
#define _HDM "$IIHDM"   // Heading Magnetic
#define _HDT "$IIHDT"  // Heading True
#define _MWD "$IIMWD"  // Wind Direction & Speed
#define _MTW "$IIMTW"  // Water Temperature
/* for lab testing with an NMEA simulator tool
#define _MWV "$WIMWV"  // Wind Speed and Angle
*/
#define _MWV "$IIMWV"  // Wind Speed and Angle
#define _ROT "$IIROT"  // Rate of Turn
#define _RPM "$IIRPM"  // Revolutions
#define _RSA "$IIRSA"  // Rudder sensor angle
#define _VDR "$IIVDR"  // Set and Drift
#define _VHW "$IIVHW"  // Water Speed and Heading
#define _VLW "$IIVLW"  //  Distance Traveled through Water
#define _VTG "$IIVTG"  //  Track Made Good and Ground Speed
#define _VWR "$IIVWR"  //  Relative Wind Speed and Angle
#define _XDR "$IIXDR"  //  Cross Track Error  Dead Reckoning
#define _XTE "$IIXTE"  //  Cross-Track Error  Measured
#define _XTR "$IIXTR"  //  Cross Track Error  Dead Reckoning
#define _ZDA "$IIZDA"  //  Time & Date - UTC, day, month, year and local time zone
//*** Some specific GPS sentences
#define _GLL "$GPGLL"   // Geographic Position  Latitude/Longitude
#define _GGA "$GPGGA"   // GPS Fix Data. Time, Position and fix related data for a GPS receiver
#define _GSA "$GPGSA"   // GPS DOP and active satellites
#define _GSV "$GPGSV"   // Satellites in view
#define _RMA "$GPRMA"  // Recommended Minimum Navigation Information
#define _RMB "$GPRMB"  // Recommended Minimum Navigation Information
#define _RMC "$GPRMC"  // Recommended Minimum Navigation Information

//*** Some specific Robertson / Stowe Marine tags below
#define _TON "$PSTON"  // Distance Nautical since reset
#define _TOE "$PSTOE"  // Engine hours
#define _TOB "$PSTOB"  // Battery voltage
#define _TOD "$PSTOD"  // depth transducer below waterline in feet
//*** Arduino generated TAGS
#define _xDR "$" TALKER_ID "" "XDR" // Arduino Transducer measurement
#define _dBK "$" TALKER_ID "" "DBK" // Arduino Transducer measurement
#define _hDG "$" TALKER_ID "" "HDG" // Arduino Transducer measurement
/* SPECIAL NOTE:
  XDR - Transducer Measurement
        1 2   3 4            n
        | |   | |            |
  $--XDR,a,x.x,a,c--c, ..... *hh<CR><LF>
  Field Number:   1:Transducer Type
                2:Measurement Data
                3:Units of measurement
                4:Name of transducer

  There may be any number of quadruplets like this, each describing a sensor. The last field will be a checksum as usual.
  Example:
  $HCXDR,A,171,D,PITCH,A,-37,D,ROLL,G,367,,MAGX,G,2420,,MAGY,G,-8984,,MAGZ*41
*/

/*
   If there is some special treatment needed for some NMEA sentences then
   add the their definitions to the NMEA_SPECIALTY definition
   The pre-compiler concatenates string literals by using "" in between
*/
#define NMEA_SPECIALTY "" _DBK "" _TOB

//*** A structure to hold the NMEA data
typedef struct {
  String fields[ MAX_NMEA_FIELDS ];
  byte nrOfFields=0;
  String sentence="";

}NMEAData ;

char nmeaBuffer[NMEA_BUFFER_SIZE+1]={0};

enum NMEAReceiveStatus { INVALID, VALID, RECEIVING, CHECKSUMMING, TERMINATING, NMEA_READY};
byte nmeaStatus = INVALID;
byte nmeaIndex=0;
bool nmeaDataReady = false;


SoftwareSerial nmeaSerialOut(52,TALKER_PORT,true); // signal need to be inverted for RS-232

/*
TFT screen specific definitions go here
*/
#define YP A3  // must be an analog pin, use "An" notation!
#define XM A2  // must be an analog pin, use "An" notation!
#define YM 9   // can be a digital pin
#define XP 8   // can be a digital pin

//param calibration from kbv
/*#define TS_MINX 298
#define TS_MAXX 814

#define TS_MINY 114 
#define TS_MAXY 867
*/
#define TS_MINX 116
#define TS_MAXX 906

#define TS_MINY 92 
#define TS_MAXY 952


// For better pressure precision, we need to know the resistance
// between X+ and X- Use any multimeter to read it
// For the one we're using, its 300 ohms across the X plate
//touch sensitivity for press
#define MINPRESSURE 10
#define MAXPRESSURE 1000

#define BLACK        0x0000  /*   0,   0,   0 */
#define BLUE         0x001F  /*   0,   0, 255 */
#define RED          0xF800  /* 255,   0,   0 */
#define GREEN        0x07E0  /*   0, 255,   0 */
#define CYAN         0x07FF  /*   0, 255, 255 */
#define MAGENTA      0xF81F  /* 255,   0, 255 */
#define YELLOW       0xFFE0  /* 255, 255,   0 */
#define WHITE        0xFFFF  /* 255, 255, 255 */
#define NAVY         0x000F  /*   0,   0, 128 */
#define DARKGREEN    0x03E0  /*   0, 128,   0 */
#define DARKCYAN     0x03EF  /*   0, 128, 128 */
#define MAROON       0x7800  /* 128,   0,   0 */
#define PURPLE       0x780F  /* 128,   0, 128 */
#define OLIVE        0x7BE0  /* 128, 128,   0 */
#define LIGHTGREY    0xC618  /* 192, 192, 192 */
#define DARKGREY     0x7BEF  /* 128, 128, 128 */
#define ORANGE       0xFD20  /* 255, 165,   0 */
#define GREENYELLOW  0xAFE5  /* 173, 255,  47 */
#define LOG_COLOR    0xFD20

#define BUTTON_H  60 //button height
#define BUTTON_W  110 //button wodth
#define BUTTON_X  5 // x position of button column
#define BUTTON_Y 260 // y position of button column

//if the IC model is known or the modules is unreadable,you can use this constructed function
LCDWIKI_KBV my_lcd(ILI9486,A3,A2,A1,A0,A4); //model,cs,cd,wr,rd,reset
//if the IC model is not known and the modules is readable,you can use this constructed function
//LCDWIKI_KBV my_lcd(320,480,A3,A2,A1,A0,A4);//width,height,cs,cd,wr,rd,reset

TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);


int16_t current_color,flag_colour;
boolean show_flag = true;
int16_t screen_row = 0;

//*** Define the units of measurement in a string pointer array 
//*** and use an enum to get the index for the specific label
const char *screen_units[]={"Kts","NM","Deg","M","C","V"};
enum units { SPEED, DIST, DEGR, MTRS, TEMP, VOLT};
//*** the screen is divided in 4 quadrants
//***   Q1      Q2
//***   Q3      Q4
enum screen_quadrant { Q1, Q2, Q3, Q4 };

//*** a structure to hold the button info
typedef struct
{
     char button_name[10];
     uint8_t button_name_size;    // the text size i.e. 1,2,...,n based on a 5x8 char
     uint16_t button_name_colour;
     uint16_t button_colour;
     uint16_t button_x;
     uint16_t button_y;     
 }button_info;

//*** define the buttons used with an enumerated tag
enum menu_buttons { SPD, CRS, ALL, LOG};
uint8_t active_menu_button = SPD; //holds the active menu button pressed
//*** the definition of buttons menu
button_info menu_button[4] = 
{
  "Speed",3,BLACK,LIGHTGREY,BUTTON_X,BUTTON_Y,
  "Crs",3,BLACK,LIGHTGREY,BUTTON_X+(1*(BUTTON_W+BUTTON_X)),BUTTON_Y,
  "All",3,BLACK,LIGHTGREY,BUTTON_X+(2*(BUTTON_W+BUTTON_X)),BUTTON_Y,
  "Log",3,BLACK,LIGHTGREY,BUTTON_X+(3*(BUTTON_W+BUTTON_X)),BUTTON_Y, 
};

/* 
  puts a string in a foreground/background color on position x,y
*/
void show_string(char *str,int16_t x,int16_t y,uint8_t csize,uint16_t fc, uint16_t bc,boolean mode)
{
    my_lcd.Set_Text_Mode(mode); // if true background of button is used
    my_lcd.Set_Text_Size(csize);
    my_lcd.Set_Text_colour(fc);
    my_lcd.Set_Text_Back_colour(bc);
    my_lcd.Print_String(str,x,y);
}
/* 
  clears the visible part of the screen above the buttons
*/
void wipe_screen(){
   my_lcd.Set_Draw_color(BLACK);
  my_lcd.Fill_Rectangle(0,0,480,BUTTON_Y);

}

/*
Prints a line on the TFT screen taking into account that there is
a button row from y>BUTTON_Y
*/
void screen_println(char *str,uint8_t csize,uint16_t fc, uint16_t bc,boolean mode)
{
  if(screen_row > (BUTTON_Y-8)){
    screen_row = 0;
    wipe_screen();
  }
    my_lcd.Set_Text_Mode(mode);
    my_lcd.Set_Text_Size(csize);
    my_lcd.Set_Text_colour(fc);
    my_lcd.Set_Text_Back_colour(bc);
    my_lcd.Print_String(str,0,screen_row);
    screen_row += 8*csize;
}

/*
Prints the measured value and it's units + tag combi in one of the quadrants
*/
void update_display(double val,const char *str, const char *tag,int8_t q){
  uint16_t x=0,y=0,s=6;
  // which quadrants needs an update
  switch( q ){
    case Q1:
     x = 10;
     y = 10;
    break;
    case Q2:
    x = 240;
     y = 10;
    break;
    case Q3:
    x = 10;
     y = 150;
    break;
    case Q4:
    x = 240;
     y = 150;
    break;
    default:
    break;
  }
    // adjust the fontsize for large numbers o fit the screen
    if( val > 999.9) s=4; 
    else s=6;
    // print the value
    my_lcd.Set_Text_Mode(false);
    my_lcd.Set_Text_Size(s);
    my_lcd.Set_Text_colour(YELLOW);
    my_lcd.Set_Text_Back_colour(BLACK);
    my_lcd.Print_Number_Float(val,1,x,y,'.',5,' ');
    // print the unit and tag
    my_lcd.Set_Text_Size(3);
    my_lcd.Set_Text_colour(WHITE);
    my_lcd.Print_String( str,x+50,y+50);
    my_lcd.Print_String( tag,x+120,y+50);
}

/*
Show the menu as a row of 4 buttons on the lower part of the display
*/
void show_menu(void)
{
    int i;
    uint16_t c=RED;
    wipe_screen();
   for(i = 0;i < (int) (sizeof(menu_button)/sizeof(button_info));i++)
   {
     if( i==active_menu_button) c=RED;
     else c=menu_button[i].button_name_colour;
      my_lcd.Set_Draw_color(menu_button[i].button_colour);
      my_lcd.Fill_Round_Rectangle(menu_button[i].button_x, 
                            menu_button[i].button_y, 
                            menu_button[i].button_x+BUTTON_W, 
                            menu_button[i].button_y+BUTTON_H,
                            3);
      show_string(menu_button[i].button_name,
                  menu_button[i].button_x+5,
                  menu_button[i].button_y+13,
                  menu_button[i].button_name_size,
                  c,
                  menu_button[i].button_colour,
                  true);
                  
   }
}




/* 
 *  MPU specific defenitions go here
 */
#define I2Cclock 400000                                 // I2C clock is 400 kilobits/s
#define I2Cport Wire                                    // I2C using Wire library
#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0             // MPU9250 address when ADO = 0 (0x68)  
#define True_North false                                // change this to "true" for True North                
float Declination = +1.57;                              // substitute your magnetic declination 

// ----- software timer
unsigned long Timer1 = 500000L;                         // 500mS loop ... used when sending data to to Processing
unsigned long Stop1=0;                                  // Has current micros() to check Timer1 difference

// ----- NZ Offsets & Scale-factors
float
/*
Mag_x_offset = -34.560013,
Mag_y_offset = 528.885,
Mag_z_offset = -125.259995,
Mag_x_scale = 1.0247924,
Mag_y_scale = 0.99078894,
Mag_z_scale = 0.9853226;
*/
//*** Below values as per calibration on June 2nd, 2020
Mag_x_offset = -268.86,
Mag_y_offset = 49.53,
Mag_z_offset = 224.98,
Mag_x_scale = 0.80,
Mag_y_scale = 0.77,
Mag_z_scale = 2.21;

 
/* EEPROM buffer to mag bias and scale factors */
uint8_t eeprom_buffer[24];
float calValue;

/* MPU 9250 object */
MPU9250 imu(MPU9250_ADDRESS, I2Cport, I2Cclock);      // Create imu instance using I2C at 400 kilobits/s


bool on = true;
byte pin = 22;

//*** flag data on the mpu port is ready
volatile bool mpuDataReady = false;

//*** ISR to set mpuDataReady flag
void mpuReady(){
  mpuDataReady = true;
}

bool mpuNeedsCalibration = true;
String mpuNMEAString = "";

/*
 * End MPU specific definitions
 */

 /*
  * Setting fro Serial interrup
  */
//*** flag data on the listener port is ready
volatile bool listenerDataReady = false;

//*** ISR to set listerDataReady flag
void listenerReady(){
  listenerDataReady = true;
}


/*
debugWrite() <--provides basic debug info from other tasks
takes a String as input parameter
*/
void debugWrite(String debugMsg)
{
  #ifdef DEBUG
  if(debugMsg.length()>1) Serial.println(debugMsg);
    else Serial.print(debugMsg);
  #else
  #ifdef DISPLAY_ATTACHED
  int strlen = debugMsg.length();
  char charMsg[strlen];
  for(int i; i<strlen; i++){
    charMsg[i] = debugMsg[i];
  }
    screen_println( charMsg,2,flag_colour,BLACK,false);
  
    
    #endif
    #endif
}

/*
   Class definitions go here
*/

/*
  Purpose:  Helper class stacking NMEA data as a part of the multiplexer application
            - Pushin and popping NMEAData structure on the stack for buffer purposes
 */
class NMEAStack
 {
  public:
  NMEAStack();  // Constructor with the size of the stack
  int push( NMEAData _nmea );   // put an NMEAData struct on the stack and returns the lastIndex or -1
  NMEAData pop();               // get an NMEAData struct from the stack and decreases the lastIndex
  int getIndex();               // returns the position of the next free postion in the stack

  private:
  NMEAData stack[STACKSIZE]; // the array containg the structs
  int lastIndex=0;    // an index pointng to the first free psotiion in the stack
 };
 
  NMEAStack::NMEAStack()
  {
    this->lastIndex = 0;
    for(int i=0; i< STACKSIZE; i++ )
    {
      for(int j=0; j<MAX_NMEA_FIELDS; j++ ){
        stack[i].fields[j]="";
      }
      stack[i].nrOfFields = 0;
      stack[i].sentence = "";
    }
  }
  
  int NMEAStack::push( NMEAData _nmea )
  {
    #ifdef DEBUG
    debugWrite( "Pushing on index:"+ String(this->lastIndex ));
    #endif
    if( this->lastIndex < STACKSIZE )
    {
      stack[ this->lastIndex++ ] = _nmea;
      return this->lastIndex;
    } else
    {
      this->lastIndex = STACKSIZE;
      return -1;    // of stack is full
    }
  }

  NMEAData NMEAStack::pop()
  {
    NMEAData nmeaOut;
    nmeaOut.sentence = "";
    if( this->lastIndex>0)
    {
      this->lastIndex--;
      nmeaOut=stack[ this->lastIndex ];
    }
    #ifdef DEBUG
    debugWrite("Popped from index: "+String(lastIndex ));
    #endif
    
    return nmeaOut;   
  }

  int NMEAStack::getIndex()
  {
    return this->lastIndex;
  }

 /*
    Purpose:  An NMEA0183 parser to convert old to new version NMEA sentences
            - Reading NMEA0183 v1.5 data without a checksum,
            - Filtering out current heading data causing incorrect course in fo in navigation app
              i.e. HDG, HDM and VHW messages
            
          
  NOTE:     NMEA encoding conventions in short
            An NMEA sentence consists of a start delimiter, followed by a comma-separated sequence
            of fields, followed by the character '*' (ASCII 42), the checksum and an end-of-line marker.
            i.e. <start delimiter><field 0>,<field 1>,,,<field n>*<checksum><end-of-linemarker>
            The start delimiter is either $ or !. <field 0> contains the tag and the remaining fields
            the values. The tag is normaly a 5 character wide identifier where the 1st 2 characters
            identify the talker ID and the last 3 identify the sentence ID.
            Maximum sentence length, including the $ and <CR><LF> is 82 bytes.

  Source: https://gpsd.gitlab.io/gpsd/NMEA.html#_nmea_0183_physical_protocol_layer
  */
class NMEAParser 
{
 public:
    NMEAParser(NMEAStack *_ptrNMEAStack);
    void setSentence( String _nmeaSentence);
    void parseNMEASentence(String nmeaIn ); // parse an NMEA sentence with each part stored in the array
    NMEAData getNMEAData(); // getter function to get the NMEADAta struct
    
  private:
    NMEAStack *ptrNMEAStack;
    String NMEAFilter = NMEA_SPECIALTY;
    NMEAData nmeaData;  // self explaining
    String nmeaSentence = "";
    void reset(); // clears the nmeaData struct;
    String checksum( String str ); //calculate the checksum for str
    NMEAData nmeaSpecialty( NMEAData nmeaIn ); // special treatment function
};

// ***
// *** NMEAParser Constructor
// *** input parameters:
// *** reference to the debugger object
NMEAParser::NMEAParser(NMEAStack *_ptrNMEAStack)
  : ptrNMEAStack(_ptrNMEAStack)
{
  //*** initialize the NMEAData struct.
reset();

}

/*
 * Clear the nmeaData attribute
 */
void NMEAParser::reset(){
  nmeaData.nrOfFields = 0;
  nmeaData.sentence = "";
  for(int i=0; i< MAX_NMEA_FIELDS; i++){
    nmeaData.fields[i]="";
  }
  current_color=WHITE;
}

void NMEAParser::setSentence( String _nmeaSentence)
{
  #ifdef DEBUG
  debugWrite( "NMEA received "+String( _nmeaSentence) );
  #endif
  nmeaSentence = _nmeaSentence;
}

/*

*/
NMEAData NMEAParser::nmeaSpecialty( NMEAData nmeaIn )
{
  String filter = NMEA_SPECIALTY;
  String sentence = nmeaIn.sentence;
  String newSentence ="";
  NMEAData nmeaOut ;//= nmeaIn;
  #ifdef DEBUG
  debugWrite( " Specialty found... for filter"+filter);
  #endif
  if ( filter.indexOf( nmeaIn.fields[0]) > -1 )
  {
    /* In my on-board Robertson data network some sentences
       are not NMEA0183 compliant. So these sentences need
       to be converted to compliant sentences
    */
    //*** $IIDBK is not NMEA0183 compliant and needs conversion
    if ( nmeaIn.fields[0] == _DBK ) {
      #ifdef DEBUG
      debugWrite("Found "+String(_DBK));
      #endif
      // a typical non standard DBK message I receive is
      // $IIDBK,A,0017.6,f,,,,
      // Char A can also be a V if invalid and shoul be removed
      // All fields after the tag shift 1 position to the left
      // Since we modify the sentence we'll also put our talker ID in place
      nmeaOut.fields[0]="$AODBK";
      for( int i=1; i<nmeaIn.nrOfFields-2; i++ )
      {
        nmeaOut.fields[i] = nmeaIn.fields[i+1];
      }
      // We need the the feet  to convert to meters and add to string
      float ft = nmeaOut.fields[1].toFloat();
      nmeaOut.fields[3] = String( ft * FTM, 1);
      nmeaOut.fields[4] = "M";
      nmeaOut.fields[5] = "";
      nmeaOut.fields[6] = "";
      nmeaOut.nrOfFields = 7;
      for( int i=0; i< nmeaOut.nrOfFields ; i++)
      {
        #ifdef DEBUG
        debugWrite("Field["+String(i)+"] = "+nmeaOut.fields[i]);
        #endif
        if(i>0) nmeaOut.sentence+=",";
        nmeaOut.sentence += nmeaOut.fields[i];
      }
      nmeaOut.sentence += checksum( nmeaOut.sentence );
      
      #ifdef DEBUG
      debugWrite( " Modified to:"+nmeaOut.sentence);
      #endif
      return nmeaOut;
    }

    //*** current Battery info is in a non NMEA0183 format 
    //*** i.e. $PSTOB,13.2,V
    //*** will be converted to $AOXDR,U,13.2,V,BATT,*CS
    if( nmeaIn.fields[0] == _TOB )
    {
      reset();
      float batt = (nmeaIn.fields[1]).toFloat()+BATTERY_OFFSET;
      nmeaOut.nrOfFields = 5;
      nmeaOut.fields[0] = "$AOXDR";
      nmeaOut.fields[1] ="U";  // the transducer unit
      nmeaOut.fields[2] = String(batt,1);  // the actual measurement value
      nmeaOut.fields[3] = nmeaIn.fields[2]; // unit of measure
      nmeaOut.fields[3].toUpperCase();
      nmeaOut.fields[4] ="BATT";
      for( int i=0; i< nmeaOut.nrOfFields ; i++)
      {
        #ifdef DEBUG
        debugWrite("Field["+String(i)+"] = "+nmeaOut.fields[i]);
        #endif
        if(i>0) nmeaOut.sentence+=",";
        nmeaOut.sentence += nmeaOut.fields[i];
      }
      nmeaOut.sentence += checksum( nmeaOut.sentence );
      return nmeaOut;
    }
  }
  return nmeaOut;
}



// calculate checksum function (thanks to https://mechinations.wordpress.com)
String NMEAParser::checksum( String str )
{
  byte cs = 0;
  for (unsigned int n = 1; n < str.length(); n++)
  {
    if ( str[n] != '$' || str[n] != '!' || str[n] != '*' )
    {
      cs ^= str[n];
    }
  }

  if (cs < 0x10) return "*0" + String(cs, HEX);
  else return "*"+String(cs, HEX);
}

/*
   parse an NMEA sentence into into an NMEAData structure.
*/
void NMEAParser::parseNMEASentence(String nmeaStr)
{
  reset();
  int currentIndex = 0;
  int lastIndex = -1;
  int sentenceLength = nmeaStr.length();
  String newSentence = "";  // used to construct the new senctence
  //*** check for a valid NMEA sentence
  #ifdef DEBUG
    debugWrite(" In te loop to parse for "+String(sentenceLength)+" chars");
    #endif
  if ( nmeaStr[0] == '$' || nmeaStr[0] == '!' || nmeaStr[0] == '~' )
  {
    
    //*** parse the fields from the NMEA string
    //*** keeping in mind that indeOf() can return -1 if not found!
    currentIndex = nmeaStr.indexOf( ',',0);
    while ( lastIndex < sentenceLength  )
    {
      
      //*** remember to sepatrate fields with the ',' character
      //*** but do not end with one!
      if ( lastIndex>0 ) nmeaData.sentence += ',';

      //*** we want the data without the ',' in fields array
      if( currentIndex-lastIndex >1 ) // check for an empty field
      {
        nmeaData.fields[ nmeaData.nrOfFields ] = nmeaStr.substring(lastIndex+1, currentIndex );
        nmeaData.sentence += nmeaData.fields[ nmeaData.nrOfFields];
      } else nmeaData.fields[ nmeaData.nrOfFields ] = "0";
      nmeaData.nrOfFields++;
      lastIndex = currentIndex; // searching from next char of ',' !
      currentIndex = nmeaStr.indexOf( ',', lastIndex+1);
      //*** check if we found the last seperator
      if( currentIndex < 0 )
      {
        //*** and make sure we parse the last part of the string too!
        currentIndex = sentenceLength;
      }
      
    }
    if ( NMEAFilter.indexOf( nmeaData.fields[0] ) > -1)
    {
      nmeaData = nmeaSpecialty( nmeaData );
    } else if(nmeaData.sentence.indexOf('*')<1)  //Check for checksum in sentence
    {
      nmeaData.sentence += checksum( nmeaData.sentence);
    }
    #ifdef DEBUG
    debugWrite("Parsed : "+nmeaData.sentence );
    #endif
    nmeaData.sentence += NMEA_TERMINATOR;
    #ifdef DEBUG
    debugWrite("Parsed & terminated: "+nmeaData.sentence );
    #endif
    ptrNMEAStack->push( nmeaData );   //push the struct to the stack for later use; i.e. buffer it
  }

  return;
}

/*
 * getter function to get the NMEADAta structure
 */
NMEAData NMEAParser::getNMEAData()
{
  return nmeaData;
}


/***********************************************************************************
   Global variables go here
*/
NMEAStack       NmeaStack;
NMEAParser      NmeaParser(&NmeaStack);
NMEAData        NmeaData;

/*
  Initialize the NMEA Talker port and baudrate
  on RX/TX port 2
*/
void initializeTalker(){
  nmeaSerialOut.begin(TALKER_RATE); 
  #ifdef DEBUG
  debugWrite( "Talker initialized...");
  #endif
}

/*
 * Start reading converted NNMEA sentences from the stack
 * and write them to Serial Port 2 to send them to the 
 * external NMEA device.
 * Update the display with the value(s) send
 */
byte startTalking(){
  NMEAData nmeaOut;
  String outStr = "";
  
  #ifdef DISPLAY_ATTACHED
  double tmpVal=0.0;
  #endif
  
  //*** for all  NMEAData opjects on the stack
  //*** NOTE; the stack has a buffer of NMEA_BUFFER_SIZE objects
  //***       normaly only 1 or 2 should be on the stack
  //***       if the stack is full your timing is out of control

  if( NmeaStack.getIndex()>0 ){
      nmeaOut = NmeaStack.pop();
      outStr =nmeaOut.sentence;
      

      for(int i=0; i< (int) outStr.length(); i++){
        //outChar[i]=outStr[i];
        nmeaSerialOut.write( outStr[i]);
        
      }
      
      #ifdef DEBUG
      debugWrite(" Sending :" + outStr );
      #endif
  }
  #ifdef DISPLAY_ATTACHED
  // check which screens is active and update with data
  switch (active_menu_button){
  
    case SPEED:
      if(nmeaOut.fields[0]== _RMC){
        tmpVal=nmeaOut.fields[7].toDouble();
        update_display( tmpVal,screen_units[SPEED],"SOG",Q1);
      }
      if(nmeaOut.fields[0]== _VHW){
        tmpVal=nmeaOut.fields[5].toDouble();
        update_display( tmpVal,screen_units[SPEED],"STW",Q2);
      }
      if(nmeaOut.fields[0]== _VWR){
        tmpVal=nmeaOut.fields[3].toDouble();
        update_display( tmpVal,screen_units[SPEED],"AWS",Q3);
        tmpVal=nmeaOut.fields[1].toDouble();
        char tmpChr[(nmeaOut.fields[2].length())];
        (nmeaOut.fields[2]).toCharArray(tmpChr,nmeaOut.fields[2].length(),0);
      update_display( tmpVal,screen_units[DEGR],"AWA",Q4);
      }
    break;
    case CRS:
        if(nmeaOut.fields[0]== _RMC){
        tmpVal=nmeaOut.fields[8].toDouble();
          update_display( tmpVal,screen_units[DEGR],"TRUE",Q1);
        }
        if(nmeaOut.fields[0]== _hDG){
          tmpVal=nmeaOut.fields[1].toDouble();
          update_display( tmpVal,screen_units[DEGR],"MAG",Q2);
        }
        /*
        if(nmeaOut.fields[0]== _xDR ){
          if(nmeaOut.fields[4]== "PITCH"){
            tmpVal=nmeaOut.fields[2].toDouble();
            update_display( tmpVal,screen_units[DEGR],"PITCH",Q3);
          }
          //*** if we found PITCH we also have ROLL
          if(nmeaOut.fields[8]== "ROLL"){
            tmpVal=nmeaOut.fields[6].toDouble();
            update_display( tmpVal,screen_units[DEGR],"ROLL",Q4);
          }
        }
        */
        if(nmeaOut.fields[0]== _dBK){
          tmpVal=nmeaOut.fields[3].toDouble();
          update_display( tmpVal,screen_units[MTRS],"DPT",Q3);
        }
        if(nmeaOut.fields[0]== _VLW){
          tmpVal=nmeaOut.fields[3].toDouble();
          update_display( tmpVal,screen_units[DIST],"TRIP" ,Q4);
        }
        
    break;
    case ALL:
        if(nmeaOut.fields[0]== _xDR ){
          if(nmeaOut.fields[4]== "BATT"){
            tmpVal=nmeaOut.fields[2].toDouble();
            update_display( tmpVal,screen_units[VOLT],"BAT",Q1);
          }
        }
        if(nmeaOut.fields[0]== _MTW){
            tmpVal=nmeaOut.fields[1].toDouble();
            update_display( tmpVal,screen_units[TEMP],"WTR",Q2);
          }
          if(nmeaOut.fields[0]== _VLW){
            tmpVal=nmeaOut.fields[1].toDouble();
            update_display( tmpVal,screen_units[DIST],"LOG",Q3);
          }
          if(nmeaOut.fields[0]== _VLW){
            tmpVal=nmeaOut.fields[3].toDouble();
            update_display( tmpVal,screen_units[DIST],"TRP",Q4);
          }
    break;
    case LOG:
      default:
      if( show_flag){
        // One time instruction for logging when LOG button pressed
        debugWrite( "Connect a cable to the serial port on" ); 
...

This file has been truncated, please download it to see its full contents.
Log Testrun Textile
of correct working NMEAparser function
16:53:31.587 -> BLINKER: ON
16:53:31.620 -> -----Raw NMEA Data received on serial port 1-----
16:53:31.726 -> $IIVWR,151,R,02.4,N,,,,
16:53:31.793 ->  In te loop to parse for 23 chars
16:53:31.859 -> new sentence is: 
16:53:31.893 -> $IIVWR,151,R,02.4,N,,,,*4e
16:53:31.961 -> -----End Data processed on serial port 1-----
16:53:32.063 -> BLINKER: OFF
16:53:32.101 -> -----Raw NMEA Data received on serial port 1-----
16:53:32.205 -> !AIVDM,1,1,,B,E>jMjb1WT;9h19Q00000000000006jDo>k9<0D2RRSv000,4*30
16:53:32.343 ->  In te loop to parse for 65 chars
16:53:32.410 -> new sentence is: 
16:53:32.443 -> !AIVDM,1,1,,B,E>jMjb1WT;9h19Q00000000000006jDo>k9<0D2RRSv000,4*30
16:53:32.580 -> -----End Data processed on serial port 1-----
16:53:32.682 -> BLINKER: ON
16:53:32.716 -> -----Raw NMEA Data received on serial port 1-----
16:53:32.818 -> !AIVDM,1,1,,A,13aL<mhP000J9:PN?<jf4?vLP88B,0*2B
16:53:32.922 ->  In te loop to parse for 47 chars
16:53:32.989 -> new sentence is: 
16:53:33.023 -> !AIVDM,1,1,,A,13aL<mhP000J9:PN?<jf4?vLP88B,0*2B
16:53:33.126 -> -----End Data processed on serial port 1-----
16:53:33.229 -> BLINKER: OFF
16:53:33.262 -> -----Raw NMEA Data received on serial port 1-----
16:53:33.362 -> $IIDBK,A,0014.4,f,,,,
16:53:33.430 ->  In te loop to parse for 21 chars
16:53:33.497 ->  Specialty found... for filter$IIDBK$PSTOB
16:53:33.598 -> Found $IIDBK
16:53:33.631 -> Field[0] = $AODBK
16:53:33.665 -> Field[1] = 0014.4
16:53:33.699 -> Field[2] = f
16:53:33.736 -> Field[3] = 4.4
16:53:33.771 -> Field[4] = M
16:53:33.808 -> Field[5] = 
16:53:33.808 -> Field[6] = 
16:53:33.841 ->  Modified to:$AODBK,0014.4,f,4.4,M,,,**75
16:53:33.942 -> new sentence is: 
16:53:33.976 -> $AODBK,0014.4,f,4.4,M,,,**75
16:53:34.042 -> -----End Data processed on serial port 1-----
16:53:34.145 -> BLINKER: ON
16:53:34.179 -> -----Raw NMEA Data received on serial port 1-----
16:53:34.279 -> $GPGGA,151314.000,5251.3091,N,00541.8037,E,2,8,1.09,1.6,M,46.8,M,0000,0000*55
16:53:34.454 ->  In te loop to parse for 77 chars
16:53:34.522 -> new sentence is: 
16:53:34.556 -> $GPGGA,151314.000,5251.3091,N,00541.8037,E,2,8,1.09,1.6,M,46.8,M,0000,0000*55
16:53:34.725 -> -----End Data processed on serial port 1-----
16:53:34.830 -> BLINKER: OFF
16:53:34.830 -> -----Raw NMEA Data received on serial port 1-----
16:53:34.933 -> $GPGLL,5251.3091,N,00541.8037,E,151314.000,A,D*5B
16:53:35.071 ->  In te loop to parse for 49 chars
16:53:35.139 -> new sentence is: 
16:53:35.172 -> $GPGLL,5251.3091,N,00541.8037,E,151314.000,A,D*5B
16:53:35.277 -> -----End Data processed on serial port 1-----
16:53:35.377 -> BLINKER: ON
16:53:35.410 -> -----Raw NMEA Data received on serial port 1-----
16:53:35.514 -> $GPRMC,151314.000,A,5251.3091,N,00541.8037,E,0.09,0.00,070520,,,D*65
16:53:35.659 ->  In te loop to parse for 68 chars
16:53:35.730 -> new sentence is: 
16:53:35.763 -> $GPRMC,151314.000,A,5251.3091,N,00541.8037,E,0.09,0.00,070520,,,D*65
16:53:35.905 -> -----End Data processed on serial port 1-----
16:53:36.006 -> BLINKER: OFF
16:53:36.044 -> -----Raw NMEA Data received on serial port 1-----
16:53:36.145 -> $PSTOB,13.0,v
16:53:36.178 ->  In te loop to parse for 13 chars
16:53:36.247 ->  Specialty found... for filter$IIDBK$PSTOB
16:53:36.347 -> Field[0] = $AOXDR
16:53:36.381 -> Field[1] = U
16:53:36.414 -> Field[2] = 13.0
16:53:36.448 -> Field[3] = V
16:53:36.482 -> new sentence is: 
16:53:36.515 -> $AOXDR,U,13.0,V*25
16:53:36.582 -> -----End Data processed on serial port 1-----
16:53:36.650 -> BLINKER: ON
16:53:36.726 -> -----Raw NMEA Data received on serial port 1-----
16:53:36.793 -> $IIVWR,151,R,02.3,N,,,,
16:53:36.861 ->  In te loop to parse for 23 chars
16:53:36.929 -> new sentence is: 
16:53:36.963 -> $IIVWR,151,R,02.3,N,,,,*49
16:53:37.035 -> -----End Data processed on serial port 1-----
16:53:37.106 -> BLINKER: OFF
16:53:37.140 -> -----Raw NMEA Data received on serial port 1-----
16:53:37.242 -> $IIVHW,,,000,M,01.57,N,,
16:53:37.309 ->  In te loop to parse for 24 chars
16:53:37.375 -> new sentence is: 
16:53:37.442 -> $IIVHW,,,000,M,01.57,N,,*7b
16:53:37.475 -> -----End Data processed on serial port 1-----
16:53:37.582 -> BLINKER: ON
16:53:37.615 -> -----Raw NMEA Data received on serial port 1-----
16:53:37.716 -> $IIVHW,,,000,M,01.57,N,,
16:53:37.785 ->  In te loop to parse for 24 chars
16:53:37.853 -> new sentence is: 
16:53:37.890 -> $IIVHW,,,000,M,01.57,N,,*7b
16:53:37.960 -> -----End Data processed on serial port 1-----
16:53:38.066 -> BLINKER: OFF
16:53:38.066 -> -----Raw NMEA Data received on serial port 1-----
16:53:38.201 -> $IIVWR,151,R,02.4,N,,,,
16:53:38.234 ->  In te loop to parse for 23 chars
16:53:38.302 -> new sentence is: 
16:53:38.339 -> $IIVWR,151,R,02.4,N,,,,*4e
16:53:38.407 -> -----End Data processed on serial port 1-----
16:53:38.507 -> BLINKER: ON
16:53:38.540 -> -----Raw NMEA Data received on serial port 1-----
16:53:38.642 -> !AIVDM,1,1,,B,E>jMjb1WT;9h19Q00000000000006jDo>k9<0D2RRSv000,4*30
16:53:38.778 ->  In te loop to parse for 65 chars
16:53:38.845 -> new sentence is: 
16:53:38.912 -> !AIVDM,1,1,,B,E>jMjb1WT;9h19Q00000000000006jDo>k9<0D2RRSv000,4*30
16:53:39.045 -> -----End Data processed on serial port 1-----
16:53:39.149 -> BLINKER: OFF
16:53:39.149 -> -----Raw NMEA Data received on serial port 1-----
16:53:39.290 -> !AIVDM,1,1,,A,13aL<mhP000J9:PN?<jf4?vLP88B,0*2B
16:53:39.358 ->  In te loop to parse for 47 chars
16:53:39.458 -> new sentence is: 
16:53:39.492 -> !AIVDM,1,1,,A,13aL<mhP000J9:PN?<jf4?vLP88B,0*2B
16:53:39.594 -> -----End Data processed on serial port 1-----
16:53:39.695 -> BLINKER: ON
16:53:39.729 -> -----Raw NMEA Data received on serial port 1-----
16:53:39.832 -> $IIDBK,A,0014.4,f,,,,
16:53:39.865 ->  In te loop to parse for 21 chars
16:53:39.932 ->  Specialty found... for filter$IIDBK$PSTOB
16:53:40.039 -> Found $IIDBK
16:53:40.075 -> Field[0] = $AODBK
16:53:40.108 -> Field[1] = 0014.4
16:53:40.142 -> Field[2] = f
16:53:40.175 -> Field[3] = 4.4
16:53:40.210 -> Field[4] = M
16:53:40.243 -> Field[5] = 
16:53:40.276 -> Field[6] = 
16:53:40.310 ->  Modified to:$AODBK,0014.4,f,4.4,M,,,**75
16:53:40.377 -> new sentence is: 
16:53:40.411 -> $AODBK,0014.4,f,4.4,M,,,**75
16:53:40.482 -> -----End Data processed on serial port 1-----
16:53:40.583 -> BLINKER: OFF
16:53:40.618 -> -----Raw NMEA Data received on serial port 1-----
16:53:40.720 -> $GPGGA,151314.000,5251.3091,N,00541.8037,E,2,8,1.09,1.6,M,46.8,M,0000,0000*55
16:53:40.886 ->  In te loop to parse for 77 chars
16:53:40.954 -> new sentence is: 
16:53:40.991 -> $GPGGA,151314.000,5251.3091,N,00541.8037,E,2,8,1.09,1.6,M,46.8,M,0000,0000*55
16:53:41.160 -> -----End Data processed on serial port 1-----
16:53:41.261 -> BLINKER: ON
16:53:41.295 -> -----Raw NMEA Data received on serial port 1-----
16:53:41.398 -> $GPGLL,5251.3091,N,00541.8037,E,151314.000,A,D*5B
16:53:41.499 ->  In te loop to parse for 49 chars
16:53:41.567 -> new sentence is: 
16:53:41.635 -> $GPGLL,5251.3091,N,00541.8037,E,151314.000,A,D*5B
16:53:41.735 -> -----End Data processed on serial port 1-----
16:53:41.837 -> BLINKER: OFF
16:53:41.870 -> -----Raw NMEA Data received on serial port 1-----
16:53:41.970 -> $GPRMC,151314.000,A,5251.3091,N,00541.8037,E,0.09,0.00,070520,,,D*65
16:53:42.109 ->  In te loop to parse for 68 chars
16:53:42.176 -> new sentence is: 
16:53:42.210 -> $GPRMC,151314.000,A,5251.3091,N,00541.8037,E,0.09,0.00,070520,,,D*65
16:53:42.378 -> -----End Data processed on serial port 1-----
16:53:42.483 -> BLINKER: ON
16:53:42.483 -> -----Raw NMEA Data received on serial port 1-----
16:53:42.618 -> $PSTOB,13.0,v
16:53:42.652 ->  In te loop to parse for 13 chars
16:53:42.720 ->  Specialty found... for filter$IIDBK$PSTOB
16:53:42.787 -> Field[0] = $AOXDR
16:53:42.855 -> Field[1] = U
16:53:42.855 -> Field[2] = 13.0
16:53:42.922 -> Field[3] = V
16:53:42.922 -> new sentence is: 
16:53:42.958 -> $AOXDR,U,13.0,V*25
16:53:43.028 -> -----End Data processed on serial port 1-----
16:53:43.128 -> BLINKER: OFF
16:53:43.128 -> -----Raw NMEA Data received on serial port 1-----
16:53:43.268 -> $IIVWR,151,R,02.3,N,,,,
16:53:43.302 ->  In te loop to parse for 23 chars
16:53:43.368 -> new sentence is: 
16:53:43.402 -> $IIVWR,151,R,02.3,N,,,,*49
16:53:43.469 -> -----End Data processed on serial port 1-----
16:53:43.570 -> BLINKER: ON
16:53:43.603 -> -----Raw NMEA Data received on serial port 1-----
16:53:43.705 -> $IIVHW,,,000,M,01.57,N,,
16:53:43.772 ->  In te loop to parse for 24 chars
16:53:43.840 -> new sentence is: 
16:53:43.876 -> $IIVHW,,,000,M,01.57,N,,*7b
16:53:43.944 -> -----End Data processed on serial port 1-----
16:53:44.044 -> BLINKER: OFF
16:53:44.078 -> -----Raw NMEA Data received on serial port 1-----
16:53:44.184 -> $IIVHW,,,000,M,01.57,N,,
16:53:44.217 ->  In te loop to parse for 24 chars
16:53:44.318 -> new sentence is: 
16:53:44.351 -> $IIVHW,,,000,M,01.57,N,,*7b
16:53:44.388 -> -----End Data processed on serial port 1-----
16:53:44.489 -> BLINKER: ON
16:53:44.526 -> -----Raw NMEA Data received on serial port 1-----
16:53:44.631 -> $IIVWR,151,R,02.4,N,,,,
16:53:44.700 ->  In te loop to parse for 23 chars
16:53:44.768 -> new sentence is: 
16:53:44.805 -> $IIVWR,151,R,02.4,N,,,,*4e
16:53:44.872 -> -----End Data processed on serial port 1-----
16:53:44.972 -> BLINKER: OFF
16:53:44.972 -> -----Raw NMEA Data received on serial port 1-----
16:53:45.111 -> !AIVDM,1,1,,B,E>jMjb1WT;9h19Q00000000000006jDo>k9<0D2RRSv000,4*30
16:53:45.247 ->  In te loop to parse for 65 chars
16:53:45.315 -> new sentence is: 
16:53:45.349 -> !AIVDM,1,1,,B,E>jMjb1WT;9h19Q00000000000006jDo>k9<0D2RRSv000,4*30
16:53:45.485 -> -----End Data processed on serial port 1-----
16:53:45.594 -> BLINKER: ON
16:53:45.628 -> -----Raw NMEA Data received on serial port 1-----
16:53:45.729 -> !AIVDM,1,1,,A,13aL<mhP000J9:PN?<jf4?vLP88B,0*2B
16:53:45.831 ->  In te loop to parse for 47 chars
16:53:45.898 -> new sentence is: 
16:53:45.935 -> !AIVDM,1,1,,A,13aL<mhP000J9:PN?<jf4?vLP88B,0*2B
16:53:46.037 -> -----End Data processed on serial port 1-----
16:53:46.139 -> BLINKER: OFF
16:53:46.172 -> -----Raw NMEA Data received on serial port 1-----
16:53:46.273 -> $IIDBK,A,0014.4,f,,,,
16:53:46.345 ->  In te loop to parse for 21 chars
16:53:46.412 ->  Specialty found... for filter$IIDBK$PSTOB
16:53:46.478 -> Found $IIDBK
16:53:46.513 -> Field[0] = $AODBK
16:53:46.547 -> Field[1] = 0014.4
16:53:46.616 -> Field[2] = f
16:53:46.616 -> Field[3] = 4.4
16:53:46.649 -> Field[4] = M
16:53:46.683 -> Field[5] = 
16:53:46.716 -> Field[6] = 
16:53:46.752 ->  Modified to:$AODBK,0014.4,f,4.4,M,,,**75
16:53:46.853 -> new sentence is: 
16:53:46.886 -> $AODBK,0014.4,f,4.4,M,,,**75
16:53:46.954 -> -----End Data processed on serial port 1-----
16:53:47.025 -> BLINKER: ON
16:53:47.063 -> -----Raw NMEA Data received on serial port 1-----
16:53:47.166 -> $GPGGA,151314.000,5251.3091,N,00541.8037,E,2,8,1.09,1.6,M,46.8,M,0000,0000*55
16:53:47.335 ->  In te loop to parse for 77 chars
16:53:47.402 -> new sentence is: 
16:53:47.435 -> $GPGGA,151314.000,5251.3091,N,00541.8037,E,2,8,1.09,1.6,M,46.8,M,0000,0000*55
16:53:47.605 -> -----End Data processed on serial port 1-----
16:53:47.705 -> BLINKER: OFF
16:53:47.740 -> -----Raw NMEA Data received on serial port 1-----
16:53:47.844 -> $GPGLL,5251.3091,N,00541.8037,E,151314.000,A,D*5B
16:53:47.945 ->  In te loop to parse for 49 chars
16:53:48.050 -> new sentence is: 
16:53:48.084 -> $GPGLL,5251.3091,N,00541.8037,E,151314.000,A,D*5B
16:53:48.185 -> -----End Data processed on serial port 1-----
16:53:48.287 -> BLINKER: ON
16:53:48.320 -> -----Raw NMEA Data received on serial port 1-----
16:53:48.428 -> $GPRMC,151314.000,A,5251.3091,N,00541.8037,E,0.09,0.00,070520,,,D*65
16:53:48.571 ->  In te loop to parse for 68 chars
16:53:48.638 -> new sentence is: 
16:53:48.672 -> $GPRMC,151314.000,A,5251.3091,N,00541.8037,E,0.09,0.00,070520,,,D*65
16:53:48.807 -> -----End Data processed on serial port 1-----
16:53:48.907 -> BLINKER: OFF
16:53:48.941 -> -----Raw NMEA Data received on serial port 1-----
16:53:49.044 -> $PSTOB,13.0,v
16:53:49.078 ->  In te loop to parse for 13 chars
16:53:49.145 ->  Specialty found... for filter$IIDBK$PSTOB
16:53:49.247 -> Field[0] = $AOXDR
16:53:49.281 -> Field[1] = U
16:53:49.314 -> Field[2] = 13.0
16:53:49.348 -> Field[3] = V
16:53:49.381 -> new sentence is: 
16:53:49.415 -> $AOXDR,U,13.0,V*25
16:53:49.484 -> -----End Data processed on serial port 1-----
16:53:49.584 -> BLINKER: ON
16:53:49.584 -> -----Raw NMEA Data received on serial port 1-----
16:53:49.718 -> $IIVWR,151,R,02.3,N,,,,
16:53:49.751 ->  In te loop to parse for 23 chars
16:53:49.819 -> new sentence is: 
16:53:49.853 -> $IIVWR,151,R,02.3,N,,,,*49
16:53:49.920 -> -----End Data processed on serial port 1-----
16:53:50.022 -> BLINKER: OFF
16:53:50.059 -> -----Raw NMEA Data received on serial port 1-----
16:53:50.163 -> $IIVHW,,,000,M,01.57,N,,
16:53:50.231 ->  In te loop to parse for 24 chars
16:53:50.300 -> new sentence is: 
16:53:50.335 -> $IIVHW,,,000,M,01.57,N,,*7b
16:53:50.404 -> -----End Data processed on serial port 1-----
16:53:50.505 -> BLINKER: ON
16:53:50.505 -> -----Raw NMEA Data received on serial port 1-----
16:53:50.611 -> $IIVHW,,,000,M,01.57,N,,
16:53:50.681 ->  In te loop to parse for 24 chars
16:53:50.748 -> new sentence is: 
16:53:50.785 -> $IIVHW,,,000,M,01.57,N,,*7b
16:53:50.852 -> -----End Data processed on serial port 1-----
16:53:50.952 -> BLINKER: OFF
16:53:50.986 -> -----Raw NMEA Data received on serial port 1-----
16:53:51.090 -> $IIVWR,151,R,02.4,N,,,,
16:53:51.125 ->  In te loop to parse for 23 chars
16:53:51.225 -> new sentence is: 
16:53:51.260 -> $IIVWR,151,R,02.4,N,,,,*4e
16:53:51.297 -> -----End Data processed on serial port 1-----
16:53:51.397 -> BLINKER: ON
16:53:51.430 -> -----Raw NMEA Data received on serial port 1-----
16:53:51.532 -> !AIVDM,1,1,,B,E>jMjb1WT;9h19Q00000000000006jDo>k9<0D2RRSv000,4*30
16:53:51.701 ->  In te loop to parse for 65 chars
16:53:51.769 -> new sentence is: 
16:53:51.803 -> !AIVDM,1,1,,B,E>jMjb1WT;9h19Q00000000000006jDo>k9<0D2RRSv000,4*30
16:53:51.937 -> -----End Data processed on serial port 1-----
16:53:52.038 -> BLINKER: OFF
16:53:52.072 -> -----Raw NMEA Data received on serial port 1-----
16:53:52.173 -> !AIVDM,1,1,,A,13aL<mhP000J9:PN?<jf4?vLP88B,0*2B
16:53:52.274 ->  In te loop to parse for 47 chars
16:53:52.342 -> new sentence is: 
16:53:52.409 -> !AIVDM,1,1,,A,13aL<mhP000J9:PN?<jf4?vLP88B,0*2B
16:53:52.477 -> -----End Data processed on serial port 1-----
16:53:52.584 -> BLINKER: ON
16:53:52.617 -> -----Raw NMEA Data received on serial port 1-----
16:53:52.718 -> $IIDBK,A,0014.4,f,,,,
16:53:52.785 ->  In te loop to parse for 21 chars
16:53:52.853 ->  Specialty found... for filter$IIDBK$PSTOB
16:53:52.925 -> Found $IIDBK
16:53:52.959 -> Field[0] = $AODBK
16:53:53.029 -> Field[1] = 0014.4
16:53:53.062 -> Field[2] = f
16:53:53.062 -> Field[3] = 4.4
16:53:53.097 -> Field[4] = M
16:53:53.131 -> Field[5] = 
16:53:53.165 -> Field[6] = 
16:53:53.198 ->  Modified to:$AODBK,0014.4,f,4.4,M,,,**75
16:53:53.298 -> new sentence is: 
16:53:53.332 -> $AODBK,0014.4,f,4.4,M,,,**75
16:53:53.400 -> -----End Data processed on serial port 1-----
16:53:53.504 -> BLINKER: OFF
16:53:53.504 -> -----Raw NMEA Data received on serial port 1-----
16:53:53.642 -> $GPGGA,151314.000,5251.3091,N,00541.8037,E,2,8,1.09,1.6,M,46.8,M,0000,0000*55
16:53:53.777 ->  In te loop to parse for 77 chars
16:53:53.848 -> new sentence is: 
16:53:53.916 -> $GPGGA,151314.000,5251.3091,N,00541.8037,E,2,8,1.09,1.6,M,46.8,M,0000,0000*55
16:53:54.084 -> -----End Data processed on serial port 1-----
16:53:54.152 -> BLINKER: ON
16:53:54.186 -> -----Raw NMEA Data received on serial port 1-----
16:53:54.288 -> $GPGLL,5251.3091,N,00541.8037,E,151314.000,A,D*5B
16:53:54.424 ->  In te loop to parse for 49 chars
16:53:54.491 -> new sentence is: 
16:53:54.524 -> $GPGLL,5251.3091,N,00541.8037,E,151314.000,A,D*5B
16:53:54.626 -> -----End Data processed on serial port 1-----
16:53:54.726 -> BLINKER: OFF
16:53:54.760 -> -----Raw NMEA Data received on serial port 1-----
16:53:54.864 -> $GPRMC,151314.000,A,5251.3091,N,00541.8037,E,0.09,0.00,070520,,,D*65
16:53:55.000 ->  In
The required MPU9250.h fileC Header File
/*
MPU9250.h
Brian R Taylor
brian.taylor@bolderflight.com

Copyright (c) 2017 Bolder Flight Systems

Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
and associated documentation files (the "Software"), to deal in the Software without restriction, 
including without limitation the rights to use, copy, modify, merge, publish, distribute, 
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or 
substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#ifndef MPU9250_h
#define MPU9250_h

#include "Arduino.h"
#include "Wire.h"    // I2C library
#include "SPI.h"     // SPI library

class MPU9250{
  public:
    enum GyroRange
    {
      GYRO_RANGE_250DPS,
      GYRO_RANGE_500DPS,
      GYRO_RANGE_1000DPS,
      GYRO_RANGE_2000DPS
    };
    enum AccelRange
    {
      ACCEL_RANGE_2G,
      ACCEL_RANGE_4G,
      ACCEL_RANGE_8G,
      ACCEL_RANGE_16G    
    };
    enum DlpfBandwidth
    {
      DLPF_BANDWIDTH_184HZ,
      DLPF_BANDWIDTH_92HZ,
      DLPF_BANDWIDTH_41HZ,
      DLPF_BANDWIDTH_20HZ,
      DLPF_BANDWIDTH_10HZ,
      DLPF_BANDWIDTH_5HZ
    };
    enum LpAccelOdr
    {
      LP_ACCEL_ODR_0_24HZ = 0,
      LP_ACCEL_ODR_0_49HZ = 1,
      LP_ACCEL_ODR_0_98HZ = 2,
      LP_ACCEL_ODR_1_95HZ = 3,
      LP_ACCEL_ODR_3_91HZ = 4,
      LP_ACCEL_ODR_7_81HZ = 5,
      LP_ACCEL_ODR_15_63HZ = 6,
      LP_ACCEL_ODR_31_25HZ = 7,
      LP_ACCEL_ODR_62_50HZ = 8,
      LP_ACCEL_ODR_125HZ = 9,
      LP_ACCEL_ODR_250HZ = 10,
      LP_ACCEL_ODR_500HZ = 11
    };
    MPU9250(TwoWire &bus,uint8_t address);
    MPU9250(SPIClass &bus,uint8_t csPin);
    int begin();
    int setAccelRange(AccelRange range);
    int setGyroRange(GyroRange range);
    int setDlpfBandwidth(DlpfBandwidth bandwidth);
    int setSrd(uint8_t srd);
    int enableDataReadyInterrupt();
    int disableDataReadyInterrupt();
    int enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr);
    int readSensor();
    float getAccelX_mss();
    float getAccelY_mss();
    float getAccelZ_mss();
    float getGyroX_rads();
    float getGyroY_rads();
    float getGyroZ_rads();
    float getMagX_uT();
    float getMagY_uT();
    float getMagZ_uT();
    float getTemperature_C();
    
    int calibrateGyro();
    float getGyroBiasX_rads();
    float getGyroBiasY_rads();
    float getGyroBiasZ_rads();
    void setGyroBiasX_rads(float bias);
    void setGyroBiasY_rads(float bias);
    void setGyroBiasZ_rads(float bias);
    int calibrateAccel();
    float getAccelBiasX_mss();
    float getAccelScaleFactorX();
    float getAccelBiasY_mss();
    float getAccelScaleFactorY();
    float getAccelBiasZ_mss();
    float getAccelScaleFactorZ();
    void setAccelCalX(float bias,float scaleFactor);
    void setAccelCalY(float bias,float scaleFactor);
    void setAccelCalZ(float bias,float scaleFactor);
    int calibrateMag();
    float getMagBiasX_uT();
    float getMagScaleFactorX();
    float getMagBiasY_uT();
    float getMagScaleFactorY();
    float getMagBiasZ_uT();
    float getMagScaleFactorZ();
    void setMagCalX(float bias,float scaleFactor);
    void setMagCalY(float bias,float scaleFactor);
    void setMagCalZ(float bias,float scaleFactor);
  protected:
    // i2c
    uint8_t _address;
    TwoWire *_i2c;
    const uint32_t _i2cRate = 400000; // 400 kHz
    size_t _numBytes; // number of bytes received from I2C
    // spi
    SPIClass *_spi;
    uint8_t _csPin;
    bool _useSPI;
    bool _useSPIHS;
    const uint8_t SPI_READ = 0x80;
    const uint32_t SPI_LS_CLOCK = 1000000;  // 1 MHz
    const uint32_t SPI_HS_CLOCK = 15000000; // 15 MHz
    // track success of interacting with sensor
    int _status;
    // buffer for reading from sensor
    uint8_t _buffer[21];
    // data counts
    int16_t _axcounts,_aycounts,_azcounts;
    int16_t _gxcounts,_gycounts,_gzcounts;
    int16_t _hxcounts,_hycounts,_hzcounts;
    int16_t _tcounts;
    // data buffer
    float _ax, _ay, _az;
    float _gx, _gy, _gz;
    float _hx, _hy, _hz;
    float _t;
    // wake on motion
    uint8_t _womThreshold;
    // scale factors
    float _accelScale;
    float _gyroScale;
    float _magScaleX, _magScaleY, _magScaleZ;
    const float _tempScale = 333.87f;
    const float _tempOffset = 21.0f;
    // configuration
    AccelRange _accelRange;
    GyroRange _gyroRange;
    DlpfBandwidth _bandwidth;
    uint8_t _srd;
    // gyro bias estimation
    size_t _numSamples = 100;
    double _gxbD, _gybD, _gzbD;
    float _gxb, _gyb, _gzb;
    // accel bias and scale factor estimation
    double _axbD, _aybD, _azbD;
    float _axmax, _aymax, _azmax;
    float _axmin, _aymin, _azmin;
    float _axb, _ayb, _azb;
    float _axs = 1.0f;
    float _ays = 1.0f;
    float _azs = 1.0f;
    // magnetometer bias and scale factor estimation
    uint16_t _maxCounts = 1000;
    float _deltaThresh = 0.3f;
    uint8_t _coeff = 8;
    uint16_t _counter;
    float _framedelta, _delta;
    float _hxfilt, _hyfilt, _hzfilt;
    float _hxmax, _hymax, _hzmax;
    float _hxmin, _hymin, _hzmin;
    float _hxb, _hyb, _hzb;
    float _hxs = 1.0f;
    float _hys = 1.0f;
    float _hzs = 1.0f;
    float _avgs;
    // transformation matrix
    /* transform the accel and gyro axes to match the magnetometer axes */
    const int16_t tX[3] = {0,  1,  0}; 
    const int16_t tY[3] = {1,  0,  0};
    const int16_t tZ[3] = {0,  0, -1};
    // constants
    const float G = 9.807f;
    const float _d2r = 3.14159265359f/180.0f;
    // MPU9250 registers
    const uint8_t ACCEL_OUT = 0x3B;
    const uint8_t GYRO_OUT = 0x43;
    const uint8_t TEMP_OUT = 0x41;
    const uint8_t EXT_SENS_DATA_00 = 0x49;
    const uint8_t ACCEL_CONFIG = 0x1C;
    const uint8_t ACCEL_FS_SEL_2G = 0x00;
    const uint8_t ACCEL_FS_SEL_4G = 0x08;
    const uint8_t ACCEL_FS_SEL_8G = 0x10;
    const uint8_t ACCEL_FS_SEL_16G = 0x18;
    const uint8_t GYRO_CONFIG = 0x1B;
    const uint8_t GYRO_FS_SEL_250DPS = 0x00;
    const uint8_t GYRO_FS_SEL_500DPS = 0x08;
    const uint8_t GYRO_FS_SEL_1000DPS = 0x10;
    const uint8_t GYRO_FS_SEL_2000DPS = 0x18;
    const uint8_t ACCEL_CONFIG2 = 0x1D;
    const uint8_t ACCEL_DLPF_184 = 0x01;
    const uint8_t ACCEL_DLPF_92 = 0x02;
    const uint8_t ACCEL_DLPF_41 = 0x03;
    const uint8_t ACCEL_DLPF_20 = 0x04;
    const uint8_t ACCEL_DLPF_10 = 0x05;
    const uint8_t ACCEL_DLPF_5 = 0x06;
    const uint8_t CONFIG = 0x1A;
    const uint8_t GYRO_DLPF_184 = 0x01;
    const uint8_t GYRO_DLPF_92 = 0x02;
    const uint8_t GYRO_DLPF_41 = 0x03;
    const uint8_t GYRO_DLPF_20 = 0x04;
    const uint8_t GYRO_DLPF_10 = 0x05;
    const uint8_t GYRO_DLPF_5 = 0x06;
    const uint8_t SMPDIV = 0x19;
    const uint8_t INT_PIN_CFG = 0x37;
    const uint8_t INT_ENABLE = 0x38;
    const uint8_t INT_DISABLE = 0x00;
    const uint8_t INT_PULSE_50US = 0x00;
    const uint8_t INT_WOM_EN = 0x40;
    const uint8_t INT_RAW_RDY_EN = 0x01;
    const uint8_t PWR_MGMNT_1 = 0x6B;
    const uint8_t PWR_CYCLE = 0x20;
    const uint8_t PWR_RESET = 0x80;
    const uint8_t CLOCK_SEL_PLL = 0x01;
    const uint8_t PWR_MGMNT_2 = 0x6C;
    const uint8_t SEN_ENABLE = 0x00;
    const uint8_t DIS_GYRO = 0x07;
    const uint8_t USER_CTRL = 0x6A;
    const uint8_t I2C_MST_EN = 0x20;
    const uint8_t I2C_MST_CLK = 0x0D;
    const uint8_t I2C_MST_CTRL = 0x24;
    const uint8_t I2C_SLV0_ADDR = 0x25;
    const uint8_t I2C_SLV0_REG = 0x26;
    const uint8_t I2C_SLV0_DO = 0x63;
    const uint8_t I2C_SLV0_CTRL = 0x27;
    const uint8_t I2C_SLV0_EN = 0x80;
    const uint8_t I2C_READ_FLAG = 0x80;
    const uint8_t MOT_DETECT_CTRL = 0x69;
    const uint8_t ACCEL_INTEL_EN = 0x80;
    const uint8_t ACCEL_INTEL_MODE = 0x40;
    const uint8_t LP_ACCEL_ODR = 0x1E;
    const uint8_t WOM_THR = 0x1F;
    const uint8_t WHO_AM_I = 0x75;
    const uint8_t FIFO_EN = 0x23;
    const uint8_t FIFO_TEMP = 0x80;
    const uint8_t FIFO_GYRO = 0x70;
    const uint8_t FIFO_ACCEL = 0x08;
    const uint8_t FIFO_MAG = 0x01;
    const uint8_t FIFO_COUNT = 0x72;
    const uint8_t FIFO_READ = 0x74;
    // AK8963 registers
    const uint8_t AK8963_I2C_ADDR = 0x0C;
    const uint8_t AK8963_HXL = 0x03; 
    const uint8_t AK8963_CNTL1 = 0x0A;
    const uint8_t AK8963_PWR_DOWN = 0x00;
    const uint8_t AK8963_CNT_MEAS1 = 0x12;
    const uint8_t AK8963_CNT_MEAS2 = 0x16;
    const uint8_t AK8963_FUSE_ROM = 0x0F;
    const uint8_t AK8963_CNTL2 = 0x0B;
    const uint8_t AK8963_RESET = 0x01;
    const uint8_t AK8963_ASA = 0x10;
    const uint8_t AK8963_WHO_AM_I = 0x00;
    // private functions
    int writeRegister(uint8_t subAddress, uint8_t data);
    int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest);
    int writeAK8963Register(uint8_t subAddress, uint8_t data);
    int readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest);
    int whoAmI();
    int whoAmIAK8963();
};

class MPU9250FIFO: public MPU9250 {
  public:
    using MPU9250::MPU9250;
    int enableFifo(bool accel,bool gyro,bool mag,bool temp);
    int readFifo();
    void getFifoAccelX_mss(size_t *size,float* data);
    void getFifoAccelY_mss(size_t *size,float* data);
    void getFifoAccelZ_mss(size_t *size,float* data);
    void getFifoGyroX_rads(size_t *size,float* data);
    void getFifoGyroY_rads(size_t *size,float* data);
    void getFifoGyroZ_rads(size_t *size,float* data);
    void getFifoMagX_uT(size_t *size,float* data);
    void getFifoMagY_uT(size_t *size,float* data);
    void getFifoMagZ_uT(size_t *size,float* data);
    void getFifoTemperature_C(size_t *size,float* data);
  protected:
    // fifo
    bool _enFifoAccel,_enFifoGyro,_enFifoMag,_enFifoTemp;
    size_t _fifoSize,_fifoFrameSize;
    float _axFifo[85], _ayFifo[85], _azFifo[85];
    size_t _aSize;
    float _gxFifo[85], _gyFifo[85], _gzFifo[85];
    size_t _gSize;
    float _hxFifo[73], _hyFifo[73], _hzFifo[73];
    size_t _hSize;
    float _tFifo[256];
    size_t _tSize;
};

#endif
The required MPU9250 and quaternion filesC/C++
/*
  This "Sparkfun MPU-9250 Breakout Arduino Library",
  https://github.com/sparkfun/SparkFun_MPU-9250_Breakout_Arduino_Library, was
  forked from https://github.com/kriswiner/MPU9250.

  Modified 28 November 2019, by LINGIB
  https://www.instructables.com/member/lingib/instructables/

  ---------------
  Hardware setup:
  ---------------
  MPU9250 Breakout -------- - Arduino
  VDD ---------------------- 5V
  SDA ---------------------- A4
  SCL ---------------------- A5
  GND ---------------------- GND

  External pull - ups are not required as the MPU9250 has internal pull - ups
  to an internal 3.3V supply.

  The MPU9250 is an I2C sensor that uses the Arduino Wire library. Because
  the sensor is not 5V tolerant, we must disable the internal Arduino pull-ups
  used by the Wire library in the Wire.h/twi.c utility file.

  This may be achieved by editing lines 75,76,77 in your
  "C:\Users\Your_name\Documents\Arduino\libraries\Wire\utility\wire.c" file to read:

  // deactivate internal pullups for twi.
  digitalWrite(SDA, 0);
  digitalWrite(SCL, 0);

  ---------------
  Terms of use:
  ---------------
  The software is provided "AS IS", without any warranty of any kind, express or implied,
  including but not limited to the warranties of mechantability, fitness for a particular
  purpose and noninfringement. In no event shall the authors or copyright holders be liable
  for any claim, damages or other liability, whether in an action of contract, tort or
  otherwise, arising from, out of or in connection with the software or the use or other
  dealings in the software.
*/

#include "MPU9250.h"

// =================================================
//  Set of useful functions to access acceleration,
//  gyroscope, magnetometer, and temperature data.
// =================================================

// ----- SPI communication
/*
    If used with sparkfun breakout board
    https://www.sparkfun.com/products/13762 , change the pre-soldered JP2 to
    enable SPI (solder middle and left instead of middle and right) pads are
    very small and re-soldering can be very tricky. I2C highly recommended.
*/
MPU9250::MPU9250( int8_t csPin, SPIClass &spiInterface, uint32_t spi_freq )
{
  _csPin = csPin;
  _spi = &spiInterface;
  _wire = NULL;

  _interfaceSpeed = spi_freq;

  _spi->begin();
  pinMode(_csPin, OUTPUT);
  deselect();
}

// =====================================
// ----- I2C communication
MPU9250::MPU9250( uint8_t address, TwoWire &wirePort, uint32_t clock_frequency )
{
  _I2Caddr = address;
  _wire = &wirePort;
  _spi = NULL;

  _interfaceSpeed = clock_frequency;

  _csPin = NOT_SPI; // Used to tell the library that the sensor is using I2C

  _wire->begin();
  _wire->setClock(_interfaceSpeed);
}

// =====================================
void MPU9250::setupMagForSPI()
{
  // Use slave 4 for talking to the magnetometer
  writeByteSPI(49, ((1 << 7) | AK8963_ADDRESS));    // Set the SLV_4_ADDR register to the magnetometer's address
  writeByteSPI(52, 0b00000000);                     // Setup SLV_4 control as needed (but not set to do an operation yet)

  writeByteSPI(36, 0b10000000);   // Enable the multi-master mode
}

// =====================================
void MPU9250::getMres()
{
  switch (Mscale)
  {
    // Possible magnetometer scales (and their register bit settings) are:
    // 14 bit resolution (0) and 16 bit resolution (1)
    case MFS_14BITS:
      mRes = 10.0f * 4912.0f / 8190.0f; // Proper scale to return milliGauss
      break;
    case MFS_16BITS:
      mRes = 10.0f * 4912.0f / 32760.0f; // Proper scale to return milliGauss
      break;
  }
}

// =====================================
void MPU9250::getGres()
{
  switch (Gscale)
  {
    // Possible gyro scales (and their register bit settings) are:
    // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
    // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
    // 2-bit value:
    case GFS_250DPS:
      gRes = 250.0f / 32768.0f;
      break;
    case GFS_500DPS:
      gRes = 500.0f / 32768.0f;
      break;
    case GFS_1000DPS:
      gRes = 1000.0f / 32768.0f;
      break;
    case GFS_2000DPS:
      gRes = 2000.0f / 32768.0f;
      break;
  }
}

// =====================================
void MPU9250::getAres()
{
  switch (Ascale)
  {
    // Possible accelerometer scales (and their register bit settings) are:
    // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11).
    // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
    // 2-bit value:
    case AFS_2G:
      aRes = 2.0f / 32768.0f;
      break;
    case AFS_4G:
      aRes = 4.0f / 32768.0f;
      break;
    case AFS_8G:
      aRes = 8.0f / 32768.0f;
      break;
    case AFS_16G:
      aRes = 16.0f / 32768.0f;
      break;
  }
}

// =====================================
void MPU9250::readAccelData(int16_t * destination)
{
  uint8_t rawData[6];  // x/y/z accel register data stored here
  // Read the six raw data registers into data array
  readBytes(_I2Caddr, ACCEL_XOUT_H, 6, &rawData[0]);

  // Turn the MSB and LSB into a signed 16-bit value
  destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;
  destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
  destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
}

// =====================================
void MPU9250::readGyroData(int16_t * destination)
{
  uint8_t rawData[6];  // x/y/z gyro register data stored here
  // Read the six raw data registers sequentially into data array
  readBytes(_I2Caddr, GYRO_XOUT_H, 6, &rawData[0]);

  // Turn the MSB and LSB into a signed 16-bit value
  destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;
  destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
  destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
}

// =====================================
void MPU9250::readMagData(int16_t * destination)
{
  // x/y/z gyro register data, ST2 register stored here, must read ST2 at end
  // of data acquisition
  uint8_t rawData[7];
  // Wait for magnetometer data ready bit to be set
  if (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01)
  {
    // Read the six raw data and ST2 registers sequentially into data array
    readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);
    uint8_t c = rawData[6]; // End data read by reading ST2 register
    // Check if magnetic sensor overflow set, if not then report data
    if (!(c & 0x08))
    {
      // Turn the MSB and LSB into a signed 16-bit value
      destination[0] = ((int16_t)rawData[1] << 8) | rawData[0];
      // Data stored as little Endian
      destination[1] = ((int16_t)rawData[3] << 8) | rawData[2];
      destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
    }
  }
}

// =====================================
int16_t MPU9250::readTempData()
{
  uint8_t rawData[2]; // x/y/z gyro register data stored here
  // Read the two raw data registers sequentially into data array
  readBytes(_I2Caddr, TEMP_OUT_H, 2, &rawData[0]);
  // Turn the MSB and LSB into a 16-bit value
  return ((int16_t)rawData[0] << 8) | rawData[1];
}

// =====================================
// Calculate the time the last update took for use in the quaternion filters
// TODO: This doesn't really belong in this class.
void MPU9250::updateTime()
{
  Now = micros();

  // Set integration time by time elapsed since last filter update
  deltat = ((Now - lastUpdate) / 1000000.0f);
  lastUpdate = Now;

  sum += deltat; // sum for averaging filter update rate
  sumCount++;
}

// =====================================
void MPU9250::initAK8963(float * destination)
{
  // First extract the factory calibration for each magnetometer axis
  uint8_t rawData[3];  // x/y/z gyro calibration data stored here
  // TODO: Test this!! Likely doesn't work
  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
  delay(10);
  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
  delay(10);

  // Read the x-, y-, and z-axis calibration values
  readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]);

  // Return x-axis sensitivity adjustment values, etc.
  destination[0] =  (float)(rawData[0] - 128) / 256. + 1.;
  destination[1] =  (float)(rawData[1] - 128) / 256. + 1.;
  destination[2] =  (float)(rawData[2] - 128) / 256. + 1.;
  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
  delay(10);

  // Configure the magnetometer for continuous read and highest resolution.
  // Set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL
  // register, and enable continuous mode data acquisition Mmode (bits [3:0]),
  // 0010 for 8 Hz and 0110 for 100 Hz sample rates.

  // Set magnetometer data resolution and sample ODR
  writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode);
  delay(10);

  if (_csPin != NOT_SPI)
  {
    setupMagForSPI();
  }
}

// =====================================
void MPU9250::initMPU9250()
{
  // wake up device
  // Clear sleep mode bit (6), enable all sensors
  writeByte(_I2Caddr, PWR_MGMT_1, 0x00);
  delay(100); // Wait for all registers to reset

  // Get stable time source
  // Auto select clock source to be PLL gyroscope reference if ready else
  writeByte(_I2Caddr, PWR_MGMT_1, 0x01);
  delay(200);

  // Configure Gyro and Thermometer
  // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz,
  // respectively;
  // minimum delay time for this setting is 5.9 ms, which means sensor fusion
  // update rates cannot be higher than 1 / 0.0059 = 170 Hz
  // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
  // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!),
  // 8 kHz, or 1 kHz
  writeByte(_I2Caddr, CONFIG, 0x03);

  // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
  // Use a 200 Hz rate; a rate consistent with the filter update rate
  // determined inset in CONFIG above.
  writeByte(_I2Caddr, SMPLRT_DIV, 0x04);

  // Set gyroscope full scale range
  // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are
  // left-shifted into positions 4:3

  // get current GYRO_CONFIG register value
  uint8_t c = readByte(_I2Caddr, GYRO_CONFIG);
  // c = c & ~0xE0; // Clear self-test bits [7:5]
  c = c & ~0x02; // Clear Fchoice bits [1:0]
  c = c & ~0x18; // Clear AFS bits [4:3]
  c = c | Gscale << 3; // Set full scale range for the gyro
  // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of
  // GYRO_CONFIG
  // c =| 0x00;
  // Write new GYRO_CONFIG value to register
  writeByte(_I2Caddr, GYRO_CONFIG, c );

  // Set accelerometer full-scale range configuration
  // Get current ACCEL_CONFIG register value
  c = readByte(_I2Caddr, ACCEL_CONFIG);
  // c = c & ~0xE0; // Clear self-test bits [7:5]
  c = c & ~0x18;  // Clear AFS bits [4:3]
  c = c | Ascale << 3; // Set full scale range for the accelerometer
  // Write new ACCEL_CONFIG register value
  writeByte(_I2Caddr, ACCEL_CONFIG, c);

  // Set accelerometer sample rate configuration
  // It is possible to get a 4 kHz sample rate from the accelerometer by
  // choosing 1 for accel_fchoice_b bit [3]; in this case the bandwidth is
  // 1.13 kHz
  // Get current ACCEL_CONFIG2 register value
  c = readByte(_I2Caddr, ACCEL_CONFIG2);
  c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
  c = c | 0x03;  // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
  // Write new ACCEL_CONFIG2 register value
  writeByte(_I2Caddr, ACCEL_CONFIG2, c);
  // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
  // but all these rates are further reduced by a factor of 5 to 200 Hz because
  // of the SMPLRT_DIV setting

  // Configure Interrupts and Bypass Enable
  // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH
  // until interrupt cleared, clear on read of INT_STATUS, and enable
  // I2C_BYPASS_EN so additional chips can join the I2C bus and all can be
  // controlled by the Arduino as master.
  writeByte(_I2Caddr, INT_PIN_CFG, 0x22);
  // Enable data ready (bit 0) interrupt
  writeByte(_I2Caddr, INT_ENABLE, 0x01);
  delay(100);

  if (_csPin != NOT_SPI)
  {
    setupMagForSPI();
  }
}


// =====================================
// Function which accumulates gyro and accelerometer data after device
// initialization. It calculates the average of the at-rest readings and then
// loads the resulting offsets into accelerometer and gyro bias registers.
void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias)
{
  uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
  uint16_t ii, packet_count, fifo_count;
  int32_t gyro_bias[3]  = {0, 0, 0}, accel_bias[3] = {0, 0, 0};

  // reset device
  // Write a one to bit 7 reset bit; toggle reset device
  writeByte(_I2Caddr, PWR_MGMT_1, READ_FLAG);
  delay(100);

  // get stable time source; Auto select clock source to be PLL gyroscope
  // reference if ready else use the internal oscillator, bits 2:0 = 001
  writeByte(_I2Caddr, PWR_MGMT_1, 0x01);
  writeByte(_I2Caddr, PWR_MGMT_2, 0x00);
  delay(200);

  // Configure device for bias calculation
  // Disable all interrupts
  writeByte(_I2Caddr, INT_ENABLE, 0x00);
  // Disable FIFO
  writeByte(_I2Caddr, FIFO_EN, 0x00);
  // Turn on internal clock source
  writeByte(_I2Caddr, PWR_MGMT_1, 0x00);
  // Disable I2C master
  writeByte(_I2Caddr, I2C_MST_CTRL, 0x00);
  // Disable FIFO and I2C master modes
  writeByte(_I2Caddr, USER_CTRL, 0x00);
  // Reset FIFO and DMP
  writeByte(_I2Caddr, USER_CTRL, 0x0C);
  delay(15);

  // Configure MPU6050 gyro and accelerometer for bias calculation
  // Set low-pass filter to 188 Hz
  writeByte(_I2Caddr, CONFIG, 0x01);
  // Set sample rate to 1 kHz
  writeByte(_I2Caddr, SMPLRT_DIV, 0x00);
  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
  writeByte(_I2Caddr, GYRO_CONFIG, 0x00);
  // Set accelerometer full-scale to 2 g, maximum sensitivity
  writeByte(_I2Caddr, ACCEL_CONFIG, 0x00);

  uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec
  uint16_t  accelsensitivity = 16384; // = 16384 LSB/g

  // Configure FIFO to capture accelerometer and gyro data for bias calculation
  writeByte(_I2Caddr, USER_CTRL, 0x40);  // Enable FIFO
  // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in
  // MPU-9150)
  writeByte(_I2Caddr, FIFO_EN, 0x78);
  delay(40);  // accumulate 40 samples in 40 milliseconds = 480 bytes

  // At end of sample accumulation, turn off FIFO sensor read
  // Disable gyro and accelerometer sensors for FIFO
  writeByte(_I2Caddr, FIFO_EN, 0x00);
  // Read FIFO sample count
  readBytes(_I2Caddr, FIFO_COUNTH, 2, &data[0]);
  fifo_count = ((uint16_t)data[0] << 8) | data[1];
  // How many sets of full gyro and accelerometer data for averaging
  packet_count = fifo_count / 12;

  for (ii = 0; ii < packet_count; ii++)
  {
    int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
    // Read data for averaging
    readBytes(_I2Caddr, FIFO_R_W, 12, &data[0]);
    // Form signed 16-bit integer for each sample in FIFO
    accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  );
    accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  );
    accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  );
    gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  );
    gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  );
    gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]);

    // Sum individual signed 16-bit biases to get accumulated signed 32-bit
    // biases.
    accel_bias[0] += (int32_t) accel_temp[0];
    accel_bias[1] += (int32_t) accel_temp[1];
    accel_bias[2] += (int32_t) accel_temp[2];
    gyro_bias[0]  += (int32_t) gyro_temp[0];
    gyro_bias[1]  += (int32_t) gyro_temp[1];
    gyro_bias[2]  += (int32_t) gyro_temp[2];
  }
  // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
  accel_bias[0] /= (int32_t) packet_count;
  accel_bias[1] /= (int32_t) packet_count;
  accel_bias[2] /= (int32_t) packet_count;
  gyro_bias[0]  /= (int32_t) packet_count;
  gyro_bias[1]  /= (int32_t) packet_count;
  gyro_bias[2]  /= (int32_t) packet_count;

  // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
  if (accel_bias[2] > 0L)
  {
    accel_bias[2] -= (int32_t) accelsensitivity;
  }
  else
  {
    accel_bias[2] += (int32_t) accelsensitivity;
  }

  // Construct the gyro biases for push to the hardware gyro bias registers,
  // which are reset to zero upon device startup.
  // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input
  // format.
  data[0] = (-gyro_bias[0] / 4  >> 8) & 0xFF;
  // Biases are additive, so change sign on calculated average gyro biases
  data[1] = (-gyro_bias[0] / 4)       & 0xFF;
  data[2] = (-gyro_bias[1] / 4  >> 8) & 0xFF;
  data[3] = (-gyro_bias[1] / 4)       & 0xFF;
  data[4] = (-gyro_bias[2] / 4  >> 8) & 0xFF;
  data[5] = (-gyro_bias[2] / 4)       & 0xFF;

  // Push gyro biases to hardware registers
  writeByte(_I2Caddr, XG_OFFSET_H, data[0]);
  writeByte(_I2Caddr, XG_OFFSET_L, data[1]);
  writeByte(_I2Caddr, YG_OFFSET_H, data[2]);
  writeByte(_I2Caddr, YG_OFFSET_L, data[3]);
  writeByte(_I2Caddr, ZG_OFFSET_H, data[4]);
  writeByte(_I2Caddr, ZG_OFFSET_L, data[5]);

  // Output scaled gyro biases for display in the main program
  gyroBias[0] = (float) gyro_bias[0] / (float) gyrosensitivity;
  gyroBias[1] = (float) gyro_bias[1] / (float) gyrosensitivity;
  gyroBias[2] = (float) gyro_bias[2] / (float) gyrosensitivity;

  // Construct the accelerometer biases for push to the hardware accelerometer
  // bias registers. These registers contain factory trim values which must be
  // added to the calculated accelerometer biases; on boot up these registers
  // will hold non-zero values. In addition, bit 0 of the lower byte must be
  // preserved since it is used for temperature compensation calculations.
  // Accelerometer bias registers expect bias input as 2048 LSB per g, so that
  // the accelerometer biases calculated above must be divided by 8.

  // A place to hold the factory accelerometer trim biases
  int32_t accel_bias_reg[3] = {0, 0, 0};
  // Read factory accelerometer trim values
  readBytes(_I2Caddr, XA_OFFSET_H, 2, &data[0]);
  accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  readBytes(_I2Caddr, YA_OFFSET_H, 2, &data[0]);
  accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  readBytes(_I2Caddr, ZA_OFFSET_H, 2, &data[0]);
  accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);

  // Define mask for temperature compensation bit 0 of lower byte of
  // accelerometer bias registers
  uint32_t mask = 1uL;
  // Define array to hold mask bit for each accelerometer bias axis
  uint8_t mask_bit[3] = {0, 0, 0};

  for (ii = 0; ii < 3; ii++)
  {
    // If temperature compensation bit is set, record that fact in mask_bit
    if ((accel_bias_reg[ii] & mask))
    {
      mask_bit[ii] = 0x01;
    }
  }

  // Construct total accelerometer bias, including calculated average
  // accelerometer bias from above
  // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g
  // (16 g full scale)
  accel_bias_reg[0] -= (accel_bias[0] / 8);
  accel_bias_reg[1] -= (accel_bias[1] / 8);
  accel_bias_reg[2] -= (accel_bias[2] / 8);

  data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
  data[1] = (accel_bias_reg[0])      & 0xFF;
  // preserve temperature compensation bit when writing back to accelerometer
  // bias registers
  data[1] = data[1] | mask_bit[0];
  data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
  data[3] = (accel_bias_reg[1])      & 0xFF;
  // Preserve temperature compensation bit when writing back to accelerometer
  // bias registers
  data[3] = data[3] | mask_bit[1];
  data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
  data[5] = (accel_bias_reg[2])      & 0xFF;
  // Preserve temperature compensation bit when writing back to accelerometer
  // bias registers
  data[5] = data[5] | mask_bit[2];

  // Apparently this is not working for the acceleration biases in the MPU-9250
  // Are we handling the temperature correction bit properly?
  // Push accelerometer biases to hardware registers
  writeByte(_I2Caddr, XA_OFFSET_H, data[0]);
  writeByte(_I2Caddr, XA_OFFSET_L, data[1]);
  writeByte(_I2Caddr, YA_OFFSET_H, data[2]);
  writeByte(_I2Caddr, YA_OFFSET_L, data[3]);
  writeByte(_I2Caddr, ZA_OFFSET_H, data[4]);
  writeByte(_I2Caddr, ZA_OFFSET_L, data[5]);

  // Output scaled accelerometer biases for display in the main program
  accelBias[0] = (float)accel_bias[0] / (float)accelsensitivity;
  accelBias[1] = (float)accel_bias[1] / (float)accelsensitivity;
  accelBias[2] = (float)accel_bias[2] / (float)accelsensitivity;
}

// =====================================
// Accelerometer and gyroscope self test; check calibration wrt factory settings
// Should return percent deviation from factory trim values, +/- 14 or less
// deviation is a pass.
void MPU9250::MPU9250SelfTest(float * destination)
{
  uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
  uint8_t selfTest[6];
  int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
  float factoryTrim[6];
  uint8_t FS = GFS_250DPS;

  writeByte(_I2Caddr, SMPLRT_DIV, 0x00);    // Set gyro sample rate to 1 kHz
  writeByte(_I2Caddr, CONFIG, 0x02);        // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
  writeByte(_I2Caddr, GYRO_CONFIG, FS << 3); // Set full scale range for the gyro to 250 dps
  writeByte(_I2Caddr, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
  writeByte(_I2Caddr, ACCEL_CONFIG, FS << 3); // Set full scale range for the accelerometer to 2 g

  for ( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer

    readBytes(_I2Caddr, ACCEL_XOUT_H, 6, &rawData[0]);        // Read the six raw data registers into data array
    aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
    aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
    aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;

    readBytes(_I2Caddr, GYRO_XOUT_H, 6, &rawData[0]);       // Read the six raw data registers sequentially into data array
    gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
    gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
    gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
  }

  // Get average of 200 values and store as average current readings
  for (int ii = 0; ii < 3; ii++)
  {
    aAvg[ii] /= 200;
    gAvg[ii] /= 200;
  }

  // Configure the accelerometer for self-test
  // Enable self test on all three axes and set accelerometer range to +/- 2 g
  writeByte(_I2Caddr, ACCEL_CONFIG, 0xE0);
  // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
  writeByte(_I2Caddr, GYRO_CONFIG,  0xE0);
  delay(25);  // Delay a while to let the device stabilize

  // Get average self-test values of gyro and acclerometer
  for (int ii = 0; ii < 200; ii++)
  {
    // Read the six raw data registers into data array
    readBytes(_I2Caddr, ACCEL_XOUT_H, 6, &rawData[0]);
    // Turn the MSB and LSB into a signed 16-bit value
    aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
    aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
    aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;

    // Read the six raw data registers sequentially into data array
    readBytes(_I2Caddr, GYRO_XOUT_H, 6, &rawData[0]);
    // Turn the MSB and LSB into a signed 16-bit value
    gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
    gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
    gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
  }

  // Get average of 200 values and store as average self-test readings
  for (int ii = 0; ii < 3; ii++)
  {
    aSTAvg[ii] /= 200;
    gSTAvg[ii] /= 200;
  }

  // Configure the gyro and accelerometer for normal operation
  writeByte(_I2Caddr, ACCEL_CONFIG, 0x00);
  writeByte(_I2Caddr, GYRO_CONFIG,  0x00);
  delay(25);  // Delay a while to let the device stabilize

  // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
  // X-axis accel self-test results
  selfTest[0] = readByte(_I2Caddr, SELF_TEST_X_ACCEL);
  // Y-axis accel self-test results
  selfTest[1] = readByte(_I2Caddr, SELF_TEST_Y_ACCEL);
  // Z-axis accel self-test results
  selfTest[2] = readByte(_I2Caddr, SELF_TEST_Z_ACCEL);
  // X-axis gyro self-test results
  selfTest[3] = readByte(_I2Caddr, SELF_TEST_X_GYRO);
  // Y-axis gyro self-test results
  selfTest[4] = readByte(_I2Caddr, SELF_TEST_Y_GYRO);
  // Z-axis gyro self-test results
  selfTest[5] = readByte(_I2Caddr, SELF_TEST_Z_GYRO);

  // Retrieve factory self-test value from self-test code reads
  // FT[Xa] factory trim calculation
  factoryTrim[0] = (float)(2620 / 1 << FS) * (pow(1.01 , ((float)selfTest[0] - 1.0) ));
  // FT[Ya] factory trim calculation
  factoryTrim[1] = (float)(2620 / 1 << FS) * (pow(1.01 , ((float)selfTest[1] - 1.0) ));
  // FT[Za] factory trim calculation
  factoryTrim[2] = (float)(2620 / 1 << FS) * (pow(1.01 , ((float)selfTest[2] - 1.0) ));
  // FT[Xg] factory trim calculation
  factoryTrim[3] = (float)(2620 / 1 << FS) * (pow(1.01 , ((float)selfTest[3] - 1.0) ));
  // FT[Yg] factory trim calculation
  factoryTrim[4] = (float)(2620 / 1 << FS) * (pow(1.01 , ((float)selfTest[4] - 1.0) ));
  // FT[Zg] factory trim calculation
  factoryTrim[5] = (float)(2620 / 1 << FS) * (pow(1.01 , ((float)selfTest[5] - 1.0) ));

  // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim
  // of the Self-Test Response
  // To get percent, must multiply by 100
  for (int i = 0; i < 3; i++)
  {
    // Report percent differences
    destination[i] = 100.0 * ((float)(aSTAvg[i] - aAvg[i])) / factoryTrim[i]
                     - 100.;
    // Report percent differences
    destination[i + 3] = 100.0 * ((float)(gSTAvg[i] - gAvg[i])) / factoryTrim[i + 3]
                         - 100.;
  }
}

// =====================================
// Function which accumulates magnetometer data after device initialization.
// It calculates the bias and scale in the x, y, and z axes.
void MPU9250::magCalMPU9250(float * bias_dest, float * scale_dest)
{
  uint16_t ii = 0, sample_count = 0;
  int32_t mag_bias[3]  = {0, 0, 0},
          mag_scale[3] = {0, 0, 0};
  int16_t mag_max[3]  = { -32768, -32768, -32768},  // Wrote out decimal (signed) values to remove a conversion warning
          mag_min[3]  = {32767, 32767, 32767},
          mag_temp[3] = {0, 0, 0};

  // Make sure resolution has been calculated
  getMres();

  Serial.println(F("Mag Calibration:"));
  Serial.println(F("Wave device for 30 seconds in a figure 8"));
  Serial.println(F(""));

  delay(2000);

  // shoot for ~thirty seconds of mag data
  // at 8 Hz ODR, new mag data is available every 125 ms
  if (Mmode == M_8HZ)
  {
    sample_count = 240;     // 240*125mS=30 seconds
  }
  // at 100 Hz ODR, new mag data is available every 10 ms
  if (Mmode == M_100HZ)
  {
    sample_count = 3000;    // 3000*10mS=30 seconds
  }

  for (ii = 0; ii < sample_count; ii++)
  {
    readMagData(mag_temp);  // Read the mag data

    for (int jj = 0; jj < 3; jj++)
    {
      if (mag_temp[jj] > mag_max[jj])
      {
        mag_max[jj] = mag_temp[jj];
      }
      if (mag_temp[jj] < mag_min[jj])
      {
        mag_min[jj] = mag_temp[jj];
      }
    }

    if (Mmode == M_8HZ)
    {
      delay(135); // At 8 Hz ODR, new mag data is available every 125 ms
    }
    if (Mmode == M_100HZ)
    {
      delay(12);  // At 100 Hz ODR, new mag data is available every 10 ms
    }
  }

  // Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]);
  // Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]);
  // Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]);

  // Get hard iron correction
  // Get 'average' x mag bias in counts
  mag_bias[0]  = (mag_max[0] + mag_min[0]) / 2;
  // Get 'average' y mag bias in counts
  mag_bias[1]  = (mag_max[1] + mag_min[1]) / 2;
  // Get 'average' z mag bias in counts
  mag_bias[2]  = (mag_max[2] + mag_min[2]) / 2;

  // Save mag biases in G for main program
  bias_dest[0] = (float)mag_bias[0] * mRes * factoryMagCalibration[0];
  bias_dest[1] = (float)mag_bias[1] * mRes * factoryMagCalibration[1];
  bias_dest[2] = (float)mag_bias[2] * mRes * factoryMagCalibration[2];

  // Get soft iron correction estimate
  // Get average x axis max chord length in counts
  mag_scale[0]  = (mag_max[0] - mag_min[0]) / 2;
  // Get average y axis max chord length in counts
  mag_scale[1]  = (mag_max[1] - mag_min[1]) / 2;
  // Get average z axis max chord length in counts
  mag_scale[2]  = (mag_max[2] - mag_min[2]) / 2;

  float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
  avg_rad /= 3.0;

  scale_dest[0] = avg_rad / ((float)mag_scale[0]);
  scale_dest[1] = avg_rad / ((float)mag_scale[1]);
  scale_dest[2] = avg_rad / ((float)mag_scale[2]);

  Serial.println(F("Mag Calibration done!"));
  Serial.println("");
}

// =====================================
// Wire.h read and write protocols
uint8_t MPU9250::writeByte(uint8_t deviceAddress, uint8_t registerAddress, uint8_t data)
{
  if (_csPin != NOT_SPI)
  {
    return writeByteSPI(registerAddress, data);
  }
  else
  {
    return writeByteWire(deviceAddress, registerAddress, data);
  }
}

// =====================================
uint8_t MPU9250::writeByteSPI(uint8_t registerAddress, uint8_t writeData)
{
  uint8_t returnVal;

  _spi->beginTransaction(SPISettings(_interfaceSpeed, MSBFIRST, SPI_MODE));
  select();

  _spi->transfer(registerAddress);
  returnVal = _spi->transfer(writeData);

  deselect();
  _spi->endTransaction();
  // #ifdef SERIAL_DEBUG
  //   Serial.print("MPU9250::writeByteSPI slave returned: 0x");
  //   Serial.println(returnVal, HEX);
  // #endif
  return returnVal;
}

// =====================================
uint8_t MPU9250::writeByteWire(uint8_t deviceAddress, uint8_t registerAddress,
                               uint8_t data)
{
  _wire->setClock(_interfaceSpeed);     // Reset to the desired speed, in case other devices required a slowdown
  _wire->beginTransmission(deviceAddress);    // Initialize the Tx buffer
  _wire->write(registerAddress);          // Put slave register address in Tx buffer
  _wire->write(data);                     // Put data in Tx buffer
  _wire->endTransmission();               // Send the Tx buffer
  // TODO: Fix this to return something meaningful
  // return NULL; // In the meantime fix it to return the right type
  return 0;
}

// =====================================
// Read a byte from given register on device. Calls necessary SPI or I2C
// implementation. This was configured in the constructor.
uint8_t MPU9250::readByte(uint8_t deviceAddress, uint8_t registerAddress)
{
  if (_csPin != NOT_SPI)
  {
    if (deviceAddress == AK8963_ADDRESS)
    {
      return readMagByteSPI(registerAddress);
    }
    else
    {
      return readByteSPI(registerAddress);
    }
  }
  else
  {
    return readByteWire(deviceAddress, registerAddress);
  }
}

// =====================================
uint8_t MPU9250::readMagByteSPI(uint8_t registerAddress)
{
  setupMagForSPI();

  writeByteSPI(49, ((1 << 7) | AK8963_ADDRESS));
  writeByteSPI(50, registerAddress);
  writeByteSPI(52, 0b11000000);         // Command the read into I2C_SLV4_DI register, cause an interrupt when complete

  // Wait for the data to be ready
  uint8_t I2C_MASTER_STATUS = readByteSPI(54);

  uint32_t count = 0;
  while (((I2C_MASTER_STATUS & 0b01000000) == 0) && (count++ < 100000))           // Checks against the I2C_SLV4_DONE bit in the I2C master status register
  {
    I2C_MASTER_STATUS = readByteSPI(54);
  }
  if (count > 10000)
  {
    Serial.println(F("Timed out"));
  }
  return readByteSPI(53);   // Read the data that is in the SLV4_DI register
}

// =====================================
uint8_t MPU9250::writeMagByteSPI(uint8_t registerAddress, uint8_t data)
{
  setupMagForSPI();

  writeByteSPI(49, ((1 << 7) | AK8963_ADDRESS));
  writeByteSPI(50, registerAddress);
  writeByteSPI(51, data);
  writeByteSPI(52, 0b11000000);         // Command the read into I2C_SLV4_DI register, cause an interrupt when complete

  uint8_t I2C_MASTER_STATUS = readByteSPI(54);
  uint32_t count = 0;
  while (((I2C_MASTER_STATUS & 0b01000000) == 0) && (count++ < 10000))           // Checks against the I2C_SLV4_DONE bit in the I2C master status register
  {
    I2C_MASTER_STATUS = readByteSPI(54);
  }
  if (count > 10000)
  {
    Serial.println(F("Timed out"));
  }
  return 0x00;
}

// =====================================
// Read a byte from the given register address from device using I2C
uint8_t MPU9250::readByteWire(uint8_t deviceAddress, uint8_t registerAddress)
{
  uint8_t data; // `data` will store the register data

  // Initialize the Tx buffer
  _wire->beginTransmission(deviceAddress);
  // Put slave register address in Tx buffer
  _wire->write(registerAddress);
  // Send the Tx buffer, but send a restart to keep connection alive
  _wire->endTransmission(false);
  // Read one byte from slave register address
  _wire->requestFrom(deviceAddress, (uint8_t) 1);
  // Fill Rx buffer with result
  data = _wire->read();
  // Return data read from slave register
  return data;
}

// =====================================
// Read a byte from the given register address using SPI
uint8_t MPU9250::readByteSPI(uint8_t registerAddress)
{
  return writeByteSPI(registerAddress | READ_FLAG, 0xFF /*0xFF is arbitrary*/);
}

// =====================================
// Read 1 or more bytes from given register and device using I2C
uint8_t MPU9250::readBytesWire(uint8_t deviceAddress, uint8_t registerAddress,
                               uint8_t count, uint8_t * dest)
{
  // Initialize the Tx buffer
  _wire->beginTransmission(deviceAddress);
  // Put slave register address in Tx buffer
  _wire->write(registerAddress);
  // Send the Tx buffer, but send a restart to keep connection alive
  _wire->endTransmission(false);

  uint8_t i = 0;
  // Read bytes from slave register address
  _wire->requestFrom(deviceAddress, count);
  while (_wire->available())
  {
    // Put read results in the Rx buffer
    dest[i++] = _wire->read();
  }

  return i; // Return number of bytes written
}

// =====================================
// Select slave IC by asserting CS pin
void MPU9250::select()
{
  digitalWrite(_csPin, LOW);
}

// =====================================
// Select slave IC by deasserting CS pin
void MPU9250::deselect()
{
  digitalWrite(_csPin, HIGH);
}

// =====================================
uint8_t MPU9250::readBytesSPI(uint8_t registerAddress, uint8_t count,
                              uint8_t * dest)
{
  _spi->beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE));
  select();

  _spi->transfer(registerAddress | READ_FLAG);

  uint8_t i;

  for (i = 0; i < count; i++)
  {
    dest[i] = _spi->transfer(0x00);
    // #ifdef SERIAL_DEBUG
    //     Serial.print("readBytesSPI::Read byte: 0x");
    //     Serial.println(dest[i], HEX);
    // #endif
  }

  _spi->endTransaction();
  deselect();

  delayMicroseconds(50);

  return i; // Return number of bytes written

  /*
    #ifdef SERIAL_DEBUG
    Serial.print("MPU9250::writeByteSPI slave returned: 0x");
    Serial.println(returnVal, HEX);
    #endif
    return returnVal;
  */

  /*
    // Set slave address of AK8963 and set AK8963 for read
    writeByteSPI(I2C_SLV0_ADDR, AK8963_ADDRESS | READ_FLAG);

    Serial.print("\nBHW::I2C_SLV0_ADDR set to: 0x");
    Serial.println(readByte(_I2Caddr, I2C_SLV0_ADDR), HEX);

    // Set address to start read from
    writeByteSPI(I2C_SLV0_REG, registerAddress);
    // Read bytes from magnetometer
    //
    Serial.print("\nBHW::I2C_SLV0_CTRL gets 0x");
    Serial.println(READ_FLAG | count, HEX);

...

This file has been truncated, please download it to see its full contents.
The required include files for the MPU9250C/C++
/*
  This "Sparkfun MPU-9250 Breakout Arduino Library",
  https://github.com/sparkfun/SparkFun_MPU-9250_Breakout_Arduino_Library, was
  forked from https://github.com/kriswiner/MPU9250.

  Modified 28 November 2019, by LINGIB
  https://www.instructables.com/member/lingib/instructables/

  ---------------
  Hardware setup:
  ---------------
  MPU9250 Breakout -------- - Arduino
  VDD ---------------------- 5V
  SDA ---------------------- A4
  SCL ---------------------- A5
  GND ---------------------- GND

  External pull - ups are not required as the MPU9250 has internal pull - ups
  to an internal 3.3V supply.

  The MPU9250 is an I2C sensor that uses the Arduino Wire library. Because
  the sensor is not 5V tolerant, we must disable the internal Arduino pull-ups
  used by the Wire library in the Wire.h/twi.c utility file.

  This may be achieved by editing lines 75,76,77 in your
  "C:\Users\Your_name\Documents\Arduino\libraries\Wire\utility\wire.c" file to read:

  // deactivate internal pullups for twi.
  digitalWrite(SDA, 0);
  digitalWrite(SCL, 0);

  ---------------
  Terms of use:
  ---------------
 The software is provided "AS IS", without any warranty of any kind, express or implied,
  including but not limited to the warranties of mechantability, fitness for a particular
  purpose and noninfringement. In no event shall the authors or copyright holders be liable
  for any claim, damages or other liability, whether in an action of contract, tort or
  otherwise, arising from, out of or in connection with the software or the use or other
  dealings in the software.
*/

#ifndef _MPU9250_H_
#define _MPU9250_H_

#include <SPI.h>
#include <Wire.h>

#define SERIAL_DEBUG true

// See also MPU-9250 Register Map and Descriptions, Revision 4.0,
// RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in above
// document; the MPU9250 and MPU9150 are virtually identical but the latter has
// a different register map

//Magnetometer Registers
#define WHO_AM_I_AK8963  0x00   // (AKA WIA) should return 0x48
#define INFO             0x01
#define AK8963_ST1       0x02   // data ready status bit 0
#define AK8963_XOUT_L    0x03   // data
#define AK8963_XOUT_H    0x04
#define AK8963_YOUT_L    0x05
#define AK8963_YOUT_H    0x06
#define AK8963_ZOUT_L    0x07
#define AK8963_ZOUT_H    0x08
#define AK8963_ST2       0x09   // Data overflow bit 3 and data read error status bit 2
#define AK8963_CNTL      0x0A   // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
#define AK8963_ASTC      0x0C   // Self test control
#define AK8963_I2CDIS    0x0F   // I2C disable
#define AK8963_ASAX      0x10   // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_ASAY      0x11   // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_ASAZ      0x12   // Fuse ROM z-axis sensitivity adjustment value

#define SELF_TEST_X_GYRO 0x00
#define SELF_TEST_Y_GYRO 0x01
#define SELF_TEST_Z_GYRO 0x02

/*#define X_FINE_GAIN      0x03   // [7:0] fine gain
  #define Y_FINE_GAIN      0x04
  #define Z_FINE_GAIN      0x05
  #define XA_OFFSET_H      0x06   // User-defined trim values for accelerometer
  #define XA_OFFSET_L_TC   0x07
  #define YA_OFFSET_H      0x08
  #define YA_OFFSET_L_TC   0x09
  #define ZA_OFFSET_H      0x0A
  #define ZA_OFFSET_L_TC   0x0B */

#define SELF_TEST_X_ACCEL 0x0D
#define SELF_TEST_Y_ACCEL 0x0E
#define SELF_TEST_Z_ACCEL 0x0F

#define SELF_TEST_A       0x10

#define XG_OFFSET_H       0x13    // User-defined trim values for gyroscope
#define XG_OFFSET_L       0x14
#define YG_OFFSET_H       0x15
#define YG_OFFSET_L       0x16
#define ZG_OFFSET_H       0x17
#define ZG_OFFSET_L       0x18
#define SMPLRT_DIV        0x19
#define CONFIG            0x1A
#define GYRO_CONFIG       0x1B
#define ACCEL_CONFIG      0x1C
#define ACCEL_CONFIG2     0x1D
#define LP_ACCEL_ODR      0x1E
#define WOM_THR           0x1F

// Duration counter threshold for motion interrupt generation, 1 kHz rate,
// LSB = 1 ms
#define MOT_DUR           0x20
// Zero-motion detection threshold bits [7:0]
#define ZMOT_THR          0x21
// Duration counter threshold for zero motion interrupt generation, 16 Hz rate,
// LSB = 64 ms
#define ZRMOT_DUR         0x22

#define FIFO_EN            0x23
#define I2C_MST_CTRL       0x24
#define I2C_SLV0_ADDR      0x25
#define I2C_SLV0_REG       0x26
#define I2C_SLV0_CTRL      0x27
#define I2C_SLV1_ADDR      0x28
#define I2C_SLV1_REG       0x29
#define I2C_SLV1_CTRL      0x2A
#define I2C_SLV2_ADDR      0x2B
#define I2C_SLV2_REG       0x2C
#define I2C_SLV2_CTRL      0x2D
#define I2C_SLV3_ADDR      0x2E
#define I2C_SLV3_REG       0x2F
#define I2C_SLV3_CTRL      0x30
#define I2C_SLV4_ADDR      0x31
#define I2C_SLV4_REG       0x32
#define I2C_SLV4_DO        0x33
#define I2C_SLV4_CTRL      0x34
#define I2C_SLV4_DI        0x35
#define I2C_MST_STATUS     0x36
#define INT_PIN_CFG        0x37
#define INT_ENABLE         0x38
#define DMP_INT_STATUS     0x39   // Check DMP interrupt
#define INT_STATUS         0x3A
#define ACCEL_XOUT_H       0x3B
#define ACCEL_XOUT_L       0x3C
#define ACCEL_YOUT_H       0x3D
#define ACCEL_YOUT_L       0x3E
#define ACCEL_ZOUT_H       0x3F
#define ACCEL_ZOUT_L       0x40
#define TEMP_OUT_H         0x41
#define TEMP_OUT_L         0x42
#define GYRO_XOUT_H        0x43
#define GYRO_XOUT_L        0x44
#define GYRO_YOUT_H        0x45
#define GYRO_YOUT_L        0x46
#define GYRO_ZOUT_H        0x47
#define GYRO_ZOUT_L        0x48
#define EXT_SENS_DATA_00   0x49
#define EXT_SENS_DATA_01   0x4A
#define EXT_SENS_DATA_02   0x4B
#define EXT_SENS_DATA_03   0x4C
#define EXT_SENS_DATA_04   0x4D
#define EXT_SENS_DATA_05   0x4E
#define EXT_SENS_DATA_06   0x4F
#define EXT_SENS_DATA_07   0x50
#define EXT_SENS_DATA_08   0x51
#define EXT_SENS_DATA_09   0x52
#define EXT_SENS_DATA_10   0x53
#define EXT_SENS_DATA_11   0x54
#define EXT_SENS_DATA_12   0x55
#define EXT_SENS_DATA_13   0x56
#define EXT_SENS_DATA_14   0x57
#define EXT_SENS_DATA_15   0x58
#define EXT_SENS_DATA_16   0x59
#define EXT_SENS_DATA_17   0x5A
#define EXT_SENS_DATA_18   0x5B
#define EXT_SENS_DATA_19   0x5C
#define EXT_SENS_DATA_20   0x5D
#define EXT_SENS_DATA_21   0x5E
#define EXT_SENS_DATA_22   0x5F
#define EXT_SENS_DATA_23   0x60
#define MOT_DETECT_STATUS  0x61
#define I2C_SLV0_DO        0x63
#define I2C_SLV1_DO        0x64
#define I2C_SLV2_DO        0x65
#define I2C_SLV3_DO        0x66
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET  0x68
#define MOT_DETECT_CTRL    0x69
#define USER_CTRL          0x6A   // Bit 7 enable DMP, bit 3 reset DMP
#define PWR_MGMT_1         0x6B   // Device defaults to the SLEEP mode
#define PWR_MGMT_2         0x6C
#define DMP_BANK           0x6D   // Activates a specific bank in the DMP
#define DMP_RW_PNT         0x6E   // Set read/write pointer to a specific start address in specified DMP bank
#define DMP_REG            0x6F   // Register in DMP from which to read or to which to write
#define DMP_REG_1          0x70
#define DMP_REG_2          0x71
#define FIFO_COUNTH        0x72
#define FIFO_COUNTL        0x73
#define FIFO_R_W           0x74

#define WHO_AM_I_MPU9250   0x75   // Should return 0x71
// Some Aliexpress MPU9250 boards have an MPU9255 which returns 0x73 ... LINGIB

#define XA_OFFSET_H        0x77
#define XA_OFFSET_L        0x78
#define YA_OFFSET_H        0x7A
#define YA_OFFSET_L        0x7B
#define ZA_OFFSET_H        0x7D
#define ZA_OFFSET_L        0x7E

// Using the MPU-9250 breakout board, ADO is set to 0
// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
// The previous preprocessor directives were sensitive to the location that the user defined AD1
// Now simply define MPU9250_ADDRESS as one of the two following depending on your application
#define MPU9250_ADDRESS_AD0 0x68                // Device address when ADO = 0
#define MPU9250_ADDRESS_AD1 0x69                // Device address when ADO = 1
#define AK8963_ADDRESS  0x0C                    // Address of magnetometer

#define READ_FLAG 0x80
#define NOT_SPI -1
#define SPI_DATA_RATE 1000000                   // 1MHz is the max speed of the MPU-9250
#define SPI_MODE SPI_MODE3

class MPU9250
{
  protected:

  public: // temporary

    // Set initial input parameters
    enum Ascale
    {
      AFS_2G = 0,
      AFS_4G,
      AFS_8G,
      AFS_16G
    };

    enum Gscale {
      GFS_250DPS = 0,
      GFS_500DPS,
      GFS_1000DPS,
      GFS_2000DPS
    };

    enum Mscale {
      MFS_14BITS = 0,                           // 0.6 mG per LSB
      MFS_16BITS                                // 0.15 mG per LSB
    };

    enum M_MODE {
      M_8HZ = 0x02,  // 8 Hz update
      M_100HZ = 0x06 // 100 Hz continuous magnetometer
    };


    TwoWire * _wire;                            // Allows for use of various I2C ports
    uint8_t _I2Caddr = MPU9250_ADDRESS_AD0;     // Use AD0 by default

    SPIClass * _spi;                            // Allows for use of different SPI ports
    int8_t _csPin;                              // SPI chip select pin

    uint32_t _interfaceSpeed;                   // Stores the desired I2C or SPi clock rate

    // TODO: Add setter methods for this hard coded stuff
    // Specify sensor full scale
    uint8_t Gscale = GFS_250DPS;
    uint8_t Ascale = AFS_2G;

    // Choose either 14-bit or 16-bit magnetometer resolution
    uint8_t Mscale = MFS_14BITS;                // MPU9250 mag. resolution is 14-bits (see data sheet)

    // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
    uint8_t Mmode = M_8HZ;

    uint8_t writeByteWire(uint8_t, uint8_t, uint8_t);
    uint8_t writeByteSPI(uint8_t, uint8_t);
    uint8_t writeMagByteSPI(uint8_t subAddress, uint8_t data);
    uint8_t readByteSPI(uint8_t subAddress);
    uint8_t readMagByteSPI(uint8_t subAddress);
    uint8_t readByteWire(uint8_t address, uint8_t subAddress);
    bool magInit();
    void kickHardware();
    void select();
    void deselect();
    void setupMagForSPI();

    // TODO: Remove this next line
  public:
    uint8_t ak8963WhoAmI_SPI();

  public:
    float pitch, yaw, roll;
    float temperature;                          // Stores the real internal chip temperature in Celsius
    int16_t tempCount;                          // Temperature raw count output
    uint32_t delt_t = 0;                        // Used to control display output rate

    uint32_t count = 0, sumCount = 0;           // used to control display output rate
    float deltat = 0.0f, sum = 0.0f;            // integration interval for both filter schemes
    uint32_t lastUpdate = 0, firstUpdate = 0;   // used to calculate integration interval
    uint32_t Now = 0;                           // used to calculate integration interval

    int16_t gyroCount[3];                       // Stores the 16-bit signed gyro sensor output
    int16_t magCount[3];                        // Stores the 16-bit signed magnetometer sensor output
    // Scale resolutions per LSB for the sensors
    float aRes, gRes, mRes;
    // Variables to hold latest sensor data values
    float ax, ay, az, gx, gy, gz, mx, my, mz;
    // Factory mag calibration and mag bias
    float factoryMagCalibration[3] = {0, 0, 0}, factoryMagBias[3] = {0, 0, 0};
    // Bias corrections for gyro, accelerometer, and magnetometer
    float gyroBias[3] = {0, 0, 0},
                        accelBias[3] = {0, 0, 0},
                                       magBias[3] = {0, 0, 0},
                                           magScale[3] = {0, 0, 0};
    float selfTest[6];
    // Stores the 16-bit signed accelerometer sensor output
    int16_t accelCount[3];

    // Public method declarations
    MPU9250( int8_t csPin, SPIClass &spiInterface = SPI, uint32_t spi_freq = SPI_DATA_RATE);
    MPU9250( uint8_t address = MPU9250_ADDRESS_AD0, TwoWire &wirePort = Wire, uint32_t clock_frequency = 100000 );
    void getMres();
    void getGres();
    void getAres();
    void readAccelData(int16_t *);
    void readGyroData(int16_t *);
    void readMagData(int16_t *);
    int16_t readTempData();
    void updateTime();
    void initAK8963(float *);
    void initMPU9250();
    void calibrateMPU9250(float * gyroBias, float * accelBias);
    void MPU9250SelfTest(float * destination);
    void magCalMPU9250(float * dest1, float * dest2);
    uint8_t writeByte(uint8_t, uint8_t, uint8_t);
    uint8_t readByte(uint8_t, uint8_t);
    uint8_t readBytes(uint8_t, uint8_t, uint8_t, uint8_t *);
    // TODO: make SPI/Wire private
    uint8_t readBytesSPI(uint8_t, uint8_t, uint8_t *);
    uint8_t readBytesWire(uint8_t, uint8_t, uint8_t, uint8_t *);
    bool isInI2cMode() {
      return _csPin == -1;
    }
    bool begin();
};  // class MPU9250

#endif // _MPU9250_H_
The required Quaternion filter filesC/C++
/*
  Modified 28 November 2019, by LINGIB
  https://www.instructables.com/member/lingib/instructables/

  Changes to the original code are minimal.
   - The code has been rearranged to make it more readable
   - Invalid links have been updated
   - Kp, as used in the Mahony filter and fusion scheme, has been increased from 10.0f to 40.0f. 
     This value appears to produce a smoother compass reading|response
  
  ---------------
  Terms of use:
  ---------------
  The software is provided "AS IS", without any warranty of any kind, express or implied,
  including but not limited to the warranties of mechantability, fitness for a particular
  purpose and noninfringement. In no event shall the authors or copyright holders be liable
  for any claim, damages or other liability, whether in an action of contract, tort or
  otherwise, arising from, out of or in connection with the software or the use or other
  dealings in the software.
*/

// Implementation of Sebastian Madgwick's "...efficient orientation filter for ... inertial/magnetic sensor arrays"
// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute device
// orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. More examples
// and details may be found at https://x-io.co.uk/archive/ 
//
// The following examples are on this site:
// https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/)
// https://x-io.co.uk/quaternions/
// https://x-io.co.uk/open-source-ahrs-with-x-imu/
// https://x-io.co.uk/oscillatory-motion-tracking-with-x-imu/
// https://x-io.co.uk/gait-tracking-with-x-imu/
//
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms 
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
//
// ----------------------------------------------------------------------------------
// Global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
// ----------------------------------------------------------------------------------
// There is a tradeoff in the beta parameter between accuracy and response speed.
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.

#include "quaternionFilters.h"

#define GyroMeasError PI * (40.0f / 180.0f)       // gyroscope measurement error in rads/s (shown as 3 deg/s)
#define GyroMeasDrift PI * (0.0f / 180.0f)        // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)

#define beta sqrtf(3.0f / 4.0f) * GyroMeasError   // free parameter in the Madgwick scheme, compute beta, 
#define zeta sqrtf(3.0f / 4.0f) * GyroMeasDrift   // free parameter in the Madgwick scheme, compute zeta, usually set to a small or zero value

//#define Kp 2.0f * 5.0f                          // free parameter in the Mahony filter and fusion scheme, Kp for proportional feedback 
#define Kp 40.0f                                  // free parameter in the Mahony filter and fusion scheme, Kp for proportional feedback 
#define Ki 0.0f                                   // free parameter in the Mahony filter and fusion scheme, Ki for integral feedback

static float eInt[3] = {0.0f, 0.0f, 0.0f};        // Vector to hold integral error for Mahony method
static float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};     // Vector to hold quaternion

//----------------------------
// Madgwick Quaternion Scheme
//----------------------------
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
{
  // short name local variable for readability
  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
  float norm;
  float hx, hy, _2bx, _2bz;
  float s1, s2, s3, s4;
  float qDot1, qDot2, qDot3, qDot4;

  // Auxiliary variables to avoid repeated arithmetic
  float _2q1mx;
  float _2q1my;
  float _2q1mz;
  float _2q2mx;
  float _4bx;
  float _4bz;
  float _2q1 = 2.0f * q1;
  float _2q2 = 2.0f * q2;
  float _2q3 = 2.0f * q3;
  float _2q4 = 2.0f * q4;
  float _2q1q3 = 2.0f * q1 * q3;
  float _2q3q4 = 2.0f * q3 * q4;
  float q1q1 = q1 * q1;
  float q1q2 = q1 * q2;
  float q1q3 = q1 * q3;
  float q1q4 = q1 * q4;
  float q2q2 = q2 * q2;
  float q2q3 = q2 * q3;
  float q2q4 = q2 * q4;
  float q3q3 = q3 * q3;
  float q3q4 = q3 * q4;
  float q4q4 = q4 * q4;

  // Normalise accelerometer measurement
  norm = sqrt(ax * ax + ay * ay + az * az);
  if (norm == 0.0f) return; // handle NaN
  norm = 1.0f / norm;
  ax *= norm;
  ay *= norm;
  az *= norm;

  // Normalise magnetometer measurement
  norm = sqrt(mx * mx + my * my + mz * mz);
  if (norm == 0.0f) return; // handle NaN
  norm = 1.0f / norm;
  mx *= norm;
  my *= norm;
  mz *= norm;

  // Reference direction of Earth's magnetic field
  _2q1mx = 2.0f * q1 * mx;
  _2q1my = 2.0f * q1 * my;
  _2q1mz = 2.0f * q1 * mz;
  _2q2mx = 2.0f * q2 * mx;
  hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 +
       _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
  hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
  _2bx = sqrt(hx * hx + hy * hy);
  _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
  _4bx = 2.0f * _2bx;
  _4bz = 2.0f * _2bz;

  // Gradient decent algorithm corrective step
  s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
  s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
  s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
  s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
  norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
  norm = 1.0f / norm;
  s1 *= norm;
  s2 *= norm;
  s3 *= norm;
  s4 *= norm;

  // Compute rate of change of quaternion
  qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
  qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
  qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
  qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

  // Integrate to yield quaternion
  q1 += qDot1 * deltat;
  q2 += qDot2 * deltat;
  q3 += qDot3 * deltat;
  q4 += qDot4 * deltat;
  norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
  norm = 1.0f / norm;
  q[0] = q1 * norm;
  q[1] = q2 * norm;
  q[2] = q3 * norm;
  q[3] = q4 * norm;
}

//----------------------------
// Mahoney Quaternion Scheme 
//----------------------------
// Similar to Madgwick scheme but uses proportional and integral filtering 
// on the error between estimated reference vectors and measured ones.

void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
{
  // short name local variable for readability
  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
  float norm;
  float hx, hy, bx, bz;
  float vx, vy, vz, wx, wy, wz;
  float ex, ey, ez;
  float pa, pb, pc;

  // Auxiliary variables to avoid repeated arithmetic
  float q1q1 = q1 * q1;
  float q1q2 = q1 * q2;
  float q1q3 = q1 * q3;
  float q1q4 = q1 * q4;
  float q2q2 = q2 * q2;
  float q2q3 = q2 * q3;
  float q2q4 = q2 * q4;
  float q3q3 = q3 * q3;
  float q3q4 = q3 * q4;
  float q4q4 = q4 * q4;

  // Normalise accelerometer measurement
  norm = sqrt(ax * ax + ay * ay + az * az);
  if (norm == 0.0f) return; // Handle NaN
  norm = 1.0f / norm;       // Use reciprocal for division
  ax *= norm;
  ay *= norm;
  az *= norm;

  // Normalise magnetometer measurement
  norm = sqrt(mx * mx + my * my + mz * mz);
  if (norm == 0.0f) return; // Handle NaN
  norm = 1.0f / norm;       // Use reciprocal for division
  mx *= norm;
  my *= norm;
  mz *= norm;

  // Reference direction of Earth's magnetic field
  hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
  hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
  bx = sqrt((hx * hx) + (hy * hy));
  bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);

  // Estimated direction of gravity and magnetic field
  vx = 2.0f * (q2q4 - q1q3);
  vy = 2.0f * (q1q2 + q3q4);
  vz = q1q1 - q2q2 - q3q3 + q4q4;
  wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
  wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
  wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);

  // Error is cross product between estimated direction and measured direction of gravity
  ex = (ay * vz - az * vy) + (my * wz - mz * wy);
  ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
  ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
  if (Ki > 0.0f)
  {
    eInt[0] += ex;      // accumulate integral error
    eInt[1] += ey;
    eInt[2] += ez;
  }
  else
  {
    eInt[0] = 0.0f;     // prevent integral wind up
    eInt[1] = 0.0f;
    eInt[2] = 0.0f;
  }

  // Apply feedback terms
  gx = gx + Kp * ex + Ki * eInt[0];
  gy = gy + Kp * ey + Ki * eInt[1];
  gz = gz + Kp * ez + Ki * eInt[2];

  // Integrate rate of change of quaternion
  pa = q2;
  pb = q3;
  pc = q4;
  q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
  q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
  q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
  q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);

  // Normalise quaternion
  norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
  norm = 1.0f / norm;
  q[0] = q1 * norm;
  q[1] = q2 * norm;
  q[2] = q3 * norm;
  q[3] = q4 * norm;
}

// ============================================
const float * getQ () {
  return q;
}
The required Quaternion filter include fileC/C++
/*
  This code is unchanged
  
  ---------------
  Terms of use:
  ---------------
  The software is provided "AS IS", without any warranty of any kind, express or implied,
  including but not limited to the warranties of mechantability, fitness for a particular
  purpose and noninfringement. In no event shall the authors or copyright holders be liable
  for any claim, damages or other liability, whether in an action of contract, tort or
  otherwise, arising from, out of or in connection with the software or the use or other
  dealings in the software.
*/

#ifndef _QUATERNIONFILTERS_H_
#define _QUATERNIONFILTERS_H_

#include <Arduino.h>

// ============================================
void MadgwickQuaternionUpdate(float ax, float ay, float az,
                              float gx, float gy, float gz,
                              float mx, float my, float mz,
                              float deltat);

// ============================================
void MahonyQuaternionUpdate(float ax, float ay, float az,
                            float gx, float gy, float gz,
                            float mx, float my, float mz,
                            float deltat);

// ============================================
const float * getQ();

#endif // _QUATERNIONFILTERS_H_
The NMEAtor
A project to parse NMEA0183 V1.5 data without checksum or invalid format to a compliant NMEA0183 V2 or above format

Schematics

Current situation on board
Boordnetwerk v4 4xkmnzfxjm
1st result as show in the serial monitor
Screenshot 2020 05 14 at 17 33 03 dum63zehot
Electronic schema for the project
Yazz nmeator pqzmbajelk
Task scheduler
An event driven approach to execute all tastks

Comments

Similar projects you might like

Robot for supercool indoor navigation

Project tutorial by Team oblu

  • 24,897 views
  • 9 comments
  • 64 respects

Using the RAK811 LoRa module with Arduino

Project tutorial by Naresh krish

  • 15,848 views
  • 5 comments
  • 14 respects

Remote Lamp

Project tutorial by Kutluhan Aktar

  • 2,780 views
  • 0 comments
  • 7 respects

Simple UNO calculator

Project tutorial by Joprp05

  • 18,214 views
  • 3 comments
  • 18 respects

Smartwatch - Arduino LED Control

Project in progress by dspenas

  • 9,643 views
  • 8 comments
  • 28 respects

Using I2C Communication Protocol to Connect 6 Arduino Megas

Project showcase by Sherwin Chiu

  • 6,428 views
  • 11 comments
  • 26 respects
Add projectSign up / Login