=============== Utility 2: ===============

...... The MAG3110 Data Recorder, Disussion and Source Code. .......

To autonomously steer the drone to the target and back, next to GPS based steering capabilities, an accurate tilt compensated e-compass plays a role. After testing various magnetometers, the HW351 MAG3110 pointed out to be the most accurate sensor. After calibration a yaw accuracy of +/-2 deg. was achieved

sec1-util2-Fig.1: HW351 MAG3110 Magnetometer Sensor.

The calibration of the 3-axis magnetometer as discussed in this web-site takes place in two phases, and goes beyond a limited and simple "Hard-Iron" offset calibration. To correct for errors caaused by effects like axis-gain differences, cross-axis gains and orthogonality errors, the implemented calibration is based on a rotated ellipsoid fit-model as discussed in [Ref.2] and [Ref.3]. In this Section the first phase, the aquisition of the required data will be discussed. Phase 2, processing the recorded magnetometer data, and calculating from these the calibration values will be discussed in Section-2 of this web-site.

- Phase-1: To get stable magnetometer readings at arbitrary 3D sensor orientations a PVC gimball as shown below is used.

Sec1-Util2-Fig.2: PVC-Gimball used for Magnetometer Calibration.

The wooden cube is fixed to the gimball with a non-magnetic screw. With this set-up "sufficient" different 3-axis measurements will be collected and recorded in a .txt file. For this, a Teensy (Arduino compatible) based data aquisition sketch, named "T-MAG3110-magsensor-n-data-recorder-v_18-08-21.ino", has been developed. In an automatic sensor reading loop a total of 6-yaw x 6-roll x 6-pitch = 216 sets of X, Y, Z measurements are collected. Each recorded measurement is the average of 10 samples. The recording is followed by a bleep as signl to change the position of the gimball mounted sensor for the next recording. To get the 216 measurements captured in a .txt file stored on the PC's hard-disk, the Windows application CoolTerm.exe is used. CoolTerm.exe is a freeware serial-port terminal. For details see Sec5-Ref2-Software. Once the 216 measurements are completed and CoolTerm.exe shut-down, the saved .txt file has to be loaded in a text editor like Notepad. A snippet of this file with the user given sample name "mag3110-no1_0x01_0x80_coolterm capture 2021-08-18 13-18-46.txt" is shown below.

Sec1-Util2-Fig.3: Snippet of recorded file "mag3110-no1_0x01_0x80_coolterm capture 2021-08-18 13-18-46.txt"
showing Raw Magnetometer Measurements.

To make the data fit for Phase-2 calibration processing in Section-2, all records down to where the following text starts: "Save below data under file-name: "recorded-sensordata.txt" must be deleted A snippet of the remaining measurements renamed to "recorded-sensordata.txt", written in the for further processing required array format is shown below. The data stored in this file are bit-values.
Note: The user should create Directory d:\Pelles C-SourceCode\sensordata\ and copy file "recorded-sensordata.txt" in this directory.

Sec1-Util2-Fig.4: Snippet of Array Format Magnetometer Raw Measurements.

...........................................................................................................................

.... The MAG3110 Data Recorder Source Code.....

// Last Update: 23-9-21  

// Settinttg MAG3110: 0x01 0x80, LSB at High address !!!!!
// NOTE: The sensor axis x,y,z are NOT aligned with the axis X,Y,Z of the wooden cube.
// Note: Make sure the size of BitVal_Vec is within the Error-check range.
// Data recorder for Magnetometer calibration with N measurements
// Use for N: 6yaw x 6roll x 6pitch= 216 readings
// To record the data on txt-file, use the program coolterm.exe with 
// the option => Connection=> Capture to Textfile. 
// The file will be saved in C:\arduino-c-dev\coolterm-record-to-filev160\coolterm capture 20xx-yy-zz hr-min-sec.txt
// Copy the saved datafile to directory d:\Pelles C-SourceCode\sensordata\ and rename
// Finally copy the measurements to file recorded-sensordata.txt and reduce this file
// Further process the reduced datafile with application:
// PELLES-ST-ROTATED-ELLIPSOID-FITTING-FOR-SENSOR-CALIBRATION.C
// The Pelles-C application assumes a rotated 3D ellipsoid.
// Note: The orientation i.e. the signs of the sensor with respect to cube axis
// Note: All Results are Bit-Values and NOT Gauss or the like.!!!!
// plug the SPEAKER signal wire in digital pin 9

  #include <Wire.h>    // with DUE all Wire has to be replaced by Wire1 
  #define I2C Wire     // Teensy

// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
// The Magnetometer Sensor that will be used is MAG3110. 
// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm

   #define KOMPASS 0x0E    // I2C 7bit address of MAG3110.

// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
  
  #define SPKR 9       // 9 is the digital pin that you plugged the speaker "red" wire into

  int auto_switch=0;   //switch for automatic (=>1)run, or user confirmed (=>0)

  const int _row= 216; //Deze waarde aanpassen mbt het aantal 6yaw x 6roll x 6pitch=216 gewenste metingen.
  int n, j;
  int mag[3]; 

  int magshift_x = 0;
  int magshift_y = 0;
  int magshift_z = 0;
  
  char dest[20];
  char desth[10];
  
  const char str1[] = "D[";
  const char str2[] = "][";
  const char str3[] = "]=";
  const char str4[] = ";";

  int X[_row][3];     // X matrix m x p (m=_rows, p=columns=3)
                      // m rows is aantal metingen, bv 6x6x6 = 216   
  int BitVal_Vec;

// ===============================================================
  void setup() 
 {
    Serial.begin(9600);    // start serial for output. Make sure you set your Serial Monitor to the same! 
    I2C.begin();           // join i2c bus (address optional for master) 
    delay(100);
    pinMode(SPKR, OUTPUT); //set the speaker as output
    delay(100);
  
// ++++++++++++ Configure KOMPASS Magnetometers +++++++++++++++
    Serial.println("Note:A MAG3110 is the installed magnetometer."); 
    config_MAG3110(); //Puts the Mag-sensor in the correct operating mode  
    
    Serial.println();
    Serial.print("Magnetometer Calibration measurements at ");
    Serial.print (_row);
    Serial.print (" arbitrary cube positions"); 
    Serial.println();
    Serial.println();

// ============= Start Sensor Reading Loop =====================    
    beep();
    delay(50);
    beep();
    beep();
    Serial.println("Press any key to start Sensor Reading Loop.");
    while (!Serial.available()){}       // wait for a character

  for (n=0; n<_row; n++)
    {
       Serial.println(" ");
       Serial.println(" ");
       Serial.print ("Measurement ");
       Serial.print(n+1);
       Serial.print (" of "); 
       Serial.print (_row); 
       
       if (n>0)
        {
         Serial.print ("  Put Cube in an arbitrary position.....");
        }
       Serial.println();
    
     tone(9,800,300); 
     delay(30);
     tone(9,400,300);
     delay(10);
     beep();
     delay(10);
     beep();
     beep();
     delay(10);
     tone(9,200,300);  
    
     delay(1000);  //Set cube in new position.
     // Measurement will now be taken.

     if (auto_switch==1) //switch for manual or auto measurements
       {
         while (!Serial.available()){}  // wait for a character
          {
            Serial.read();  // clear the input buffer
          }
       }
  
    Serial.println("Reading Sensor.");   //Start of data measurement and recording

    int  xsum=0;  //Bij DUE is int 4byte = 32 bit = 2.147.483.648      
    int  ysum=0;      
    int  zsum=0;      
 
  for (j=1  ; j<11 ; j++)  //j<11 means j<11
   {
    read_MAG3110_MAGSENSOR(mag); // read x/y/z and store in global mag[3] van type integer
    xsum += (mag[0] -magshift_x);      // x-as raw-data.
    ysum += (mag[1] -magshift_y);      // y-as raw-data.
    zsum += (mag[2] -magshift_z);      // z-as raw-data.
   }
  
   j=j-1;
   Serial.print("Number of samples taken for averaging the measurement is: "); 
   Serial.println( j);

   X[n][0]=(int)(xsum/j); //average raw bitvalue as integer
   X[n][1]=(int)(ysum/j); //average raw bitvalue as integer
   X[n][2]=(int)(zsum/j); //average raw bitvalue as integer

   BitVal_Vec = (int)sqrt( X[n][0] * X[n][0] + X[n][1] * X[n][1] + X[n][2] * X[n][2]); //integer

   Serial.print("X= ");
   Serial.print(X[n][0]);
   Serial.print("   Y= ");
   Serial.print(X[n][1]);
   Serial.print("   Z= ");
   Serial.print(X[n][2]);
   Serial.print("   Vector= ");
   Serial.println(BitVal_Vec);
   Serial.println(" ");

   if (BitVal_Vec>15000)     //in case of error reading
     {
        beep();
        beep();
        beep();
        beep();
        beep();
        beep();
        break;
     }
    
    Serial.print("Measurement ");
    Serial.print (n+1);
    Serial.print (" of "); 
    Serial.print (_row); 
    Serial.print (" is written to file...");
    Serial.println();
    Serial.println();
  
  }                 //end for-loop

//###############################################################
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
//========== All N Cube positions have been recorded ============

// MatrixMathPrint((int*) X, _row ,3 , 0, "Recorded Data Matrix");
   int  i;
   float vector;
   for (i=0; i<_row; i++)
     {
       Serial.print("X= ");
       Serial.print(X[i][0]);
       Serial.print("   Y= ");
       Serial.print(X[i][1]);
       Serial.print("   Z= ");
       Serial.print(X[i][2]);

       vector=sqrt(X[i][0]*X[i][0] + X[i][1]*X[i][1] + X[i][2]*X[i][2]);
       Serial.print("   Vector= ");
       Serial.print(vector,0);
       Serial.println(" ");
     }

Serial.println();
Serial.println();

Serial.println("Save below data under file-name: recorded-sensordata.txt");
Serial.println("The raw sensor measurements x,y,z are integer bit-values. ");
     for (int i=0;i<_row;i++)
      {
       for (int j=0;j<3;j++)
        {
          strcpy(dest, str1);
          itoa(i,desth,10);
          strcat(dest, desth); 
          strcat(dest, str2);
          itoa(j,desth,10);
          strcat(dest, desth);
          strcat(dest, str3);
          int val = X[i][j];
          itoa(val,desth,10);
          strcat(dest, desth);
          strcat(dest, str4);
          Serial.println(dest);
          delay(10); 
        }
      }               
 }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void loop()
 {
  Serial.println("Data Recording Finished..........");  
  delay(3000);
 }
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


// ++++++++++++++++ Start of (support) functions ++++++++++++++

// ***************** Matrix Printing Routine ****************
// Uses tabs to separate numbers under assumption printed float width won't cause problems
   void MatrixMathPrint(int* A, int m, int n, int decimals, String label)
     {
 //    A = input matrix (m x p)
 //    m = number of rows in A
 //    p = number of columns in A 
    
       int i,j;
       Serial.println();
       Serial.println(label);
       for (i=0; i<m; i++)
        {
         for (j=0;j<n;j++)
          {
 //          Serial.print(A[n*i+j],decimals);
           Serial.print(A[n*i+j]);
//           Serial.print("\t");
           Serial.print("    ");
          }
         Serial.println();
        }
   }

//**************************************************************
//++++++++++++++ Configure MAG3110 KOMPASS +++++++++++++++++++++
void config_MAG3110(void)
{
 I2C.beginTransmission(KOMPASS); // transmit to device 0x0E
  I2C.write(0x10);              // cntrl register1
  I2C.write(0x01);              // send 0x01, active mode
//  0x01 => 0b 000 00 0 0 1  
//  I2C.write(0b00000001);
  I2C.endTransmission();        // stop transmitting

  delay(10);
  
  I2C.beginTransmission(KOMPASS); // transmit to device 0x0E
  I2C.write(0x11);              // cntrl register2
  I2C.write(0x80);              // send 0x80, enable auto resets
//  0x80 => 0b 1 0 0 0 0000  
//  I2C.write(0b10000000);   
  I2C.endTransmission();       // stop transmitting
}

//===============================================================
void read_MAG3110_MAGSENSOR(int * result)
 {
   int regAddress = 0x01;   //first axis-Kompass-data register of the MAG3110
    byte howManyBytesToRead = 6;   //uint8_t means: give me an unsigned int of exactly 8 bits.
    byte _buff[6];
    short readings[3];    // short=2bytes
//  Read the Mag-data from the MAG3110
//  Each axis reading comes in 10 bit resolution, ie 2 bytes.  
//  Most Significat Byte first.

   readFrom(KOMPASS, regAddress, howManyBytesToRead, _buff);  //read the acceleration data from the ADXL345 

// now we are merging both bytes into one 2byte short 
  readings[0] = (_buff[0] << 8) | _buff[1]; // X_H=0x01=buff[0]  X_L=0x02=buff[1]
  readings[1] = (_buff[2] << 8) | _buff[3]; // Y_H=0x03=buff[2]  Y_L=0x04=buff[3]
  readings[2] = (_buff[4] << 8) | _buff[5]; // Z_M=0x05=buff[4]  Z_L=0x06=buff[5]
  delay(5);

//NOTE: Do NOT align here the sensor axis x,y,z with the X,Y,Z axis of the wooden cube!!
//Do this in the calc_pitch_roll_yaw_4_pid.ino function of Kompass and Flight-Control applications. 

  result[0] = (int)readings[0];  //bit-values
  result[1] = (int)readings[1];
  result[2] = (int)readings[2];

/*
  Serial.print("Magnetometer raw readings: ");
  Serial.print(result[0]);  //X  
  Serial.print("  ");
  Serial.print(result[1]);  //Y
  Serial.print("  ");
  Serial.println(result[2]); //Z
*/ 
   }
 
//==============================================================
// Reads num bytes starting from address register of DEVICE into _buff array
void readFrom(int DEVICE, byte address, int num, byte _buff[])
 {
  I2C.beginTransmission(DEVICE); // start transmission to device
  I2C.write(address);             // sends address to read from
  I2C.endTransmission();         // end transmission
  delay(2);

  I2C.beginTransmission(DEVICE); // start transmission to device
  I2C.requestFrom(DEVICE, num);    // request 6 bytes from device

  int i = 0;
  while (I2C.available())        // device may send less than requested (abnormal)
  {
    _buff[i] = I2C.read();    // receive a byte
    delay(5);
    i++;
  }
  I2C.endTransmission();         // end transmission
 }

//===============================================================
void writeTo(int DEVICE, byte address, byte val)
{
  I2C.beginTransmission(DEVICE);  // start transmission to device
  I2C.write(address);             // send register address
  I2C.write(val);                 // send value to write
  I2C.endTransmission();          // end transmission
}

//==========================================================
void beep()
  {
  for (int i=0; i<500; i++)   // generate a 1KHz tone for 1/2 second
   {
     digitalWrite(SPKR, HIGH);
     delayMicroseconds(500);
     digitalWrite(SPKR, LOW);
     delayMicroseconds(500);
    }
  return;
 }
//==========================================================
//==========================================================
//==========================================================

Free-Drones Company 2022