simplified measuring of axis, commented out CMPS2_set(char reset)

DanialBruce · Mar 5, 2022 · b504d5b · b504d5b
1 parent 2b0ea5b
commit b504d5b
Show file tree

Hide file tree

Showing 2 changed files with 146 additions and 44 deletions.
diff --git a/main.cpp b/main.cpp
@@ -21,20 +21,14 @@ I2C i2compass(PB_14, PB_13);
 int RWcompasSlaveAddr = 0x60;
 
 char buf[2];
-float Max[2], Mid[2], Min[2], X, Y;     // index 1 : x , index 2 : y or 
+//float Max[2], Mid[2], Min[2], X, Y;     // index 1 : x , index 2 : y or 
 
+float data[3];
+unsigned int raw[3];
+float offset[3];
 
 int main() {
-    /*char testArray[1];
-    for (int i = 0; i<256;i++) {
-        //int rwbitVersion = i << 1;
-        if (i2compass.read(i, testArray,1) == 0){
-            printf("0x%02x ACK\r\n",i);
-        }
-        else{
-            printf("No Slave found with address 0x%x\n", i);
-        }
-    }*/
+
 
     i2compass.frequency(400000);
     // Pmod Compass is init. here
@@ -53,7 +47,7 @@ int main() {
 }
 
 /***********************************
-** Pmod Compass stuff starts here **
+** Pmod Compass functions defined here **
 ************************************/
 
 //reads measurements in mG
@@ -89,21 +83,24 @@ void CMPS2_read_XYZ(){
     i2compass.read(RWcompasSlaveAddr, tmpData, 6);
 
     //initialize array for data
-    float measured_data[2];  // !!! or measured_data[3] if Z axis is needed
 
-    debug("raw measured data x: %d\n", (tmpData[0] << 8 | tmpData[1]));
-    debug("raw measured data y: %d\n", (tmpData[2] << 8 | tmpData[3]));
+    raw[0] = tmpData[1] << 8 | tmpData[0];
+    raw[1] = tmpData[3] << 8 | tmpData[2];
+    raw[2] = tmpData[5] << 8 | tmpData[4];
+    debug("raw measured data x: %d\n", raw[0]);
+    debug("raw measured data x: %d\n", raw[1]);
+    debug("raw measured data y: %d\n", raw[2]);
     //reconstruct raw data
-    measured_data[0] = 1.0 * (int) (tmpData[0] << 8 | tmpData[1]); //x
-    measured_data[1] = 1.0 * (int) (tmpData[2] << 8 | tmpData[3]); //y
+    //measured_data[0] = 1.0 * (int) (tmpData[0] << 8 | tmpData[1]); //x
+    //measured_data[1] = 1.0 * (int) (tmpData[2] << 8 | tmpData[3]); //y
     //measured_data[3] = 1.0 * (int) (tmpData[4] << 8 | tmpData[5]); //y    // if Z axis is also included
 
     // convert raw data to mG
-    for (int i = 0; i < 2; i++) {
-        measured_data[i] = 0.48828125 * (float) measured_data[i];
+    for (int i = 0; i < 3; i++) {
+        data[i] = 0.48828125 * (float) raw[i];
     }
 
-    X = measured_data[0];
+    /*X = measured_data[0];
     Y = measured_data[1];
 
     //correct minimum, mid and maximum values
@@ -118,54 +115,89 @@ void CMPS2_read_XYZ(){
     }
     if (measured_data[1] < Min[1]) { //Y min
         Min[1] = measured_data[1];
-    }
+    }*/
     /*if (measured_data[2] > Max[2]) { //Z max
         Max[2] = measured_data[2];
     }
     if (measured_data[2] > Max[2]) { //Z max
         Min[2] = measured_data[2];
     }*/
 
-    for (int i = 0; i < 2; i++) { //mid
+    /*for (int i = 0; i < 2; i++) { //mid
         Mid[i] = (Max[i] + Min[i]) / 2;
-    }
+    }*/
 
     // if Z axis is included
     /*for (int i = 0; i < 3; i++) { //mid
         Mid[i] = (Max[i] + Min[i]) / 3;
     }*/
 }
 
+
 //initialize the compass
+//Update: this should follow Calibration steps
 void CMPS2_init(){
-
-    //command internal control register 0 for set operation
+    float out1[3];
+    float out2[3];
+    int i;
+
+    //calibration: SET
+    buf[0] = 0x07;
+    buf[1] = 0x80;
+    i2compass.write(RWcompasSlaveAddr, buf, 2);
+    ThisThread::sleep_for(60ms);
+
+
     buf[0] = 0x07;
     buf[1] = 0x20;
     i2compass.write(RWcompasSlaveAddr, buf, 2);
     ThisThread::sleep_for(10ms);
 
-    //command internal control register 1 to 16 bit resolution, 8ms measurement time
-    buf[0] = 0x08;
-    buf[1] = 0x00;
+    CMPS2_read_XYZ();
+    for (int i = 0; i < 3; i++) { //mid
+        out1[i] = data[i];
+    }
+    printf("Raw SET = %d \t%d \t%d\n",raw[0], raw[1], raw[2]);
+
+    //calibration: RESET
+    buf[0] = 0x07;
+    buf[1] = 0x80;
+    i2compass.write(RWcompasSlaveAddr, buf, 2);
+    ThisThread::sleep_for(60ms);
+
+
+    buf[0] = 0x07;
+    buf[1] = 0x40;
     i2compass.write(RWcompasSlaveAddr, buf, 2);
     ThisThread::sleep_for(10ms);
 
-    //initialize minimum, mid and maximum values
-    for (int i = 0; i < 2; i++) { //mid
-        Max[i] = -32768;    //smallest int on 16 bits
-        Min[i] = 32768;     //largest int on 16 bits
-        Mid[i] = 0;
+    CMPS2_read_XYZ();
+    for (int i = 0; i < 3; i++) { //mid
+        out2[i] = data[i];
     }
-
-    // if Z axis is included
-    /*for (int i = 0; i < 3; i++) { //mid
-        Mid[i] = (Max[i] + Min[i]) / 3;
-    }*/
+    printf("Raw RESET = %d \t%d \t%d\n",raw[0], raw[1], raw[2]);
+
+    //offset calculation
+    for (int i = 0; i < 3; i++) {
+        offset[i] = (out1[i] + out2[i])*0.5;
+    }
+
+    //command internal control register 0 for set operation
+    buf[0] = 0x07;
+    buf[1] = 0x40;  //SET
+    i2compass.write(RWcompasSlaveAddr, buf, 2);
+    ThisThread::sleep_for(10ms);
+
+
+    buf[0] = 0x08;
+    buf[1] = 0x00;
+    i2compass.write(RWcompasSlaveAddr, buf, 2);
+    ThisThread::sleep_for(10ms);
 
 }
+
 //sets/resets the sensor, changing the magnetic polarity of the sensing element
-void CMPS2_set(char reset){
+/*void CMPS2_set(char reset){
     
     //command internal control register 0 bit 7 (capacitor recharge)
     buf[0] = 0x07;
@@ -186,10 +218,78 @@ void CMPS2_set(char reset){
         i2compass.write(RWcompasSlaveAddr, buf, 2);
         ThisThread::sleep_for(10ms);
     }
-}
+}*/
+
+
 float CMPS2_getHeading(){
-
-    float components[2];
+    CMPS2_read_XYZ();  //read X, Y, Z data of the magnetic field
+
+    //eliminate offset before continuing
+    for (int i=0;i<3;i++)
+    {
+    data[i] = data[i]-offset[i];
+    }
+    //variables for storing partial results
+    float temp0 = 0;
+    float temp1 = 0;
+    //and for storing the final result
+    float deg = 0;
+
+
+    //calculate heading from data of the magnetic field
+  //the formula is different in each quadrant
+    if (data[0] < 0)
+    {
+        if (data[1] > 0)
+        {
+        //Quadrant 1
+        temp0 = data[1];
+        temp1 = -data[0];
+        deg = 90 - atan(temp0 / temp1) * (180 / 3.14159);
+        }
+        else
+        {
+        //Quadrant 2
+        temp0 = -data[1];
+        temp1 = -data[0];
+        deg = 90 + atan(temp0 / temp1) * (180 / 3.14159);
+        }
+    }
+    else {
+        if (data[1] < 0)
+        {
+        //Quadrant 3
+        temp0 = -data[1];
+        temp1 = data[0];
+        deg = 270 - atan(temp0 / temp1) * (180 / 3.14159);
+        }
+        else
+        {
+        //Quadrant 4
+        temp0 = data[1];
+        temp1 = data[0];
+        deg = 270 + atan(temp0 / temp1) * (180 / 3.14159);
+        }
+    }
+
+        //correct heading
+        deg += DECLINATION;
+        if (DECLINATION > 0)
+        {
+        if (deg > 360) {
+            deg -= 360;
+        }
+        }
+        else{
+            if (deg < 0) {
+                deg += 360;
+            }
+        }
+
+        return deg;
+
+
+    /*float components[2];
     CMPS2_set(false);  // set the polarity to normal
     CMPS2_read_XYZ();  // read X, Y, Z components of the magnetic field
     components[0] = X; // save current results
@@ -250,8 +350,10 @@ float CMPS2_getHeading(){
       }
     }
 
-    return deg;
+    return deg;*/
 }
+
+
 void CMPS2_decodeHeading(float measured_angle){
 
     if (measured_angle > 337.25 | measured_angle < 22.5) {

diff --git a/mbed-os.lib b/mbed-os.lib
@@ -1 +1 @@
-https://github.com/ARMmbed/mbed-os.git#d147abc3e556c58e5e343d34b729bc2192e18bd3
+https://github.com/ARMmbed/mbed-os.git#2eb06e76208588afc6cb7580a8dd64c5429a10ce