Update EM7180_BMX055_MS5637_BasicAHRS_t3.ino

EM7180_BMX055_MS5637_BasicAHRS_t3.ino

@@ -59,8 +59,8 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(7, 6, 5, 3, 4);
 
 // See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet http://www.meas-spec.com/downloads/MS5637-02BA03.pdf
 #define MS5637_RESET      0x1E
-#define NS5637_CONVERT_D1 0x40
-#define NS5637_CONVERT_D2 0x50
+#define MS5637_CONVERT_D1 0x40
+#define MS5637_CONVERT_D2 0x50
 #define MS5637_ADC_READ   0x00
 
 // BMX055 data sheet http://ae-bst.resource.bosch.com/media/products/dokumente/bmx055/BST-BMX055-DS000-01v2.pdf
@@ -402,7 +402,7 @@ double Temperature, Pressure; // stores MS5637 pressures sensor pressure and tem
 // BMX055 variables
 int16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output
 int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output
-int16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output
+int16_t magCount[3];    // Stores the 13/15-bit signed magnetometer sensor output
 float Quat[4] = {0, 0, 0, 0}; // quaternion data register
 float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0};  // Bias corrections for gyro, accelerometer, mag
 int16_t tempCount;            // temperature raw count output
@@ -426,13 +426,13 @@ float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;   // compute zeta, the other fre
 #define Ki 0.0f
 
 uint32_t delt_t = 0, count = 0, sumCount = 0;  // used to control display output rate
-float pitch, yaw, roll;
+float pitch, yaw, roll, Yaw, Pitch, Roll;
 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
 
 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 
-float q[4] = {0.0f, 0.0f, 0.0f, 1.0f};    // vector to hold quaternion
+float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
 float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method
 
 bool passThru = false;
@@ -446,29 +446,6 @@ void setup()
   delay(5000);
   Serial.begin(38400);
 
- // Set up the interrupt pin, its set as active high, push-pull
-  pinMode(myLed, OUTPUT);
-  digitalWrite(myLed, HIGH);
-  
-  display.begin(); // Initialize the display
-  display.setContrast(58); // Set the contrast
-  
-// Start device display with ID of sensor
-  display.clearDisplay();
-  display.setTextSize(2);
-  display.setCursor(0,0); display.print("EM7180");
-  display.setTextSize(1);
-  display.setCursor(0, 20); display.print("BMX055");
-  display.setCursor(0, 30); display.print("motion sensor");
-  display.setCursor(20,40); display.print("1 mg LSB");
-  display.display();
-  delay(1000);
-
-// Set up for data display
-  display.setTextSize(1); // Set text size to normal, 2 is twice normal etc.
-  display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen
-  display.clearDisplay();   // clears the screen and buffer
-  
   I2Cscan(); // should detect SENtral at 0x28
   
   // Read SENtral device information
@@ -482,12 +459,6 @@ void setup()
   Serial.print("EM7180 ProductID: 0x"); Serial.print(PID, HEX); Serial.println(" Should be: 0x80");
   uint8_t RID = readByte(EM7180_ADDRESS, EM7180_RevisionID);
   Serial.print("EM7180 RevisionID: 0x"); Serial.print(RID, HEX); Serial.println(" Should be: 0x02");
-  uint8_t FF = readByte(EM7180_ADDRESS, EM7180_FeatureFlags);
-  Serial.println("EM7180 extra Features: "); 
-  if(FF & 0x01) Serial.println("Barometer supported");
-  if(FF & 0x02) Serial.println("Humidity sensor supported");
-  if(FF & 0x04) Serial.println("Temperature sensor supported");
-  
   
   delay(1000); // give some time to read the screen
 
@@ -517,10 +488,8 @@ void setup()
     
   // Set up the SENtral as sensor bus in normal operating mode
 if(!passThru) {
-    
 // Enter EM7180 initialized state
 writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
-
 writeByte(EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off
 // Set accel/gyro/mage desired ODR rates
 writeByte(EM7180_ADDRESS, EM7180_QRateDivisor, 0x02); // 100 Hz
@@ -532,7 +501,6 @@ void setup()
 // Enable interrupt to host upon certain events
 // choose interrupts when quaternions updated (0x04), an error occurs (0x02), or the SENtral needs to be reset(0x01)
 writeByte(EM7180_ADDRESS, EM7180_EnableEvents, 0x07);
-
 // Enable EM7180 run mode
 writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode
 delay(100);
@@ -561,23 +529,13 @@ void setup()
   
   // Check sensor status
   uint8_t sensorStatus = readByte(EM7180_ADDRESS, EM7180_SensorStatus);
-  if(sensorStatus & 0x01) Serial.println("Magnetometer not acknowledging!");
-  if(sensorStatus & 0x02) Serial.println("Accelerometer not acknowledging!");
-  if(sensorStatus & 0x04) Serial.println("Gyro not acknowledging!");
-  if(sensorStatus & 0x10) Serial.println("Magnetometer ID not recognized!");
-  if(sensorStatus & 0x20) Serial.println("Accelerometer ID not recognized!");
-  if(sensorStatus & 0x40) Serial.println("Gyro ID not recognized!");
-  
-  // check error sttus
-  uint8_t errorStatus = readByte(EM7180_ADDRESS, EM7180_ErrorRegister);
-  if(errorStatus & 0x11) Serial.println("Magnetometer failure!");
-  if(errorStatus & 0x12) Serial.println("Accelerometer failure!");
-  if(errorStatus & 0x14) Serial.println("Gyro failure!");
-  if(errorStatus & 0x21) Serial.println("Magnetometer initialization failure!");
-  if(errorStatus & 0x22) Serial.println("Accelerometer initialization failure!");
-  if(errorStatus & 0x24) Serial.println("Gyro initialization failure!");
-  if(errorStatus & 0x30) Serial.println("Math error!");
-  if(errorStatus & 0x80) Serial.println("Invalid sample rate!");
+  Serial.print(" EM7180 sensor status = "); Serial.println(sensorStatus);
+  if(sensorStatus & 0x01) Serial.print("Magnetometer not acknowledging!");
+  if(sensorStatus & 0x02) Serial.print("Accelerometer not acknowledging!");
+  if(sensorStatus & 0x04) Serial.print("Gyro not acknowledging!");
+  if(sensorStatus & 0x10) Serial.print("Magnetometer ID not recognized!");
+  if(sensorStatus & 0x20) Serial.print("Accelerometer ID not recognized!");
+  if(sensorStatus & 0x40) Serial.print("Gyro ID not recognized!");
   
   Serial.print("Actual MagRate = "); Serial.print(readByte(EM7180_ADDRESS, EM7180_ActualMagRate)); Serial.println(" Hz"); 
   Serial.print("Actual AccelRate = "); Serial.print(10*readByte(EM7180_ADDRESS, EM7180_ActualAccelRate)); Serial.println(" Hz"); 
@@ -604,6 +562,29 @@ void setup()
      Serial.print(data[i], HEX); Serial.print(" ");
    }
 
+  // Set up the interrupt pin, its set as active high, push-pull
+  pinMode(myLed, OUTPUT);
+  digitalWrite(myLed, HIGH);
+  
+  display.begin(); // Initialize the display
+  display.setContrast(58); // Set the contrast
+  
+// Start device display with ID of sensor
+  display.clearDisplay();
+  display.setTextSize(2);
+  display.setCursor(0,0); display.print("BMX055");
+  display.setTextSize(1);
+  display.setCursor(0, 20); display.print("9-DOF 16-bit");
+  display.setCursor(0, 30); display.print("motion sensor");
+  display.setCursor(20,40); display.print("1 mg LSB");
+  display.display();
+  delay(1000);
+
+// Set up for data display
+  display.setTextSize(1); // Set text size to normal, 2 is twice normal etc.
+  display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen
+  display.clearDisplay();   // clears the screen and buffer
+
   // Read the BMX-055 WHO_AM_I registers, this is a good test of communication
   Serial.println("BMX055 accelerometer...");
   byte c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_WHOAMI);  // Read ACC WHO_AM_I register for BMX055
@@ -717,6 +698,7 @@ void setup()
 }
 }
 
+
 void loop()
 {  
   if(!passThru) {
@@ -724,8 +706,6 @@ void loop()
   // Check event status register, way to chech data ready by polling rather than interrupt
   uint8_t eventStatus = readByte(EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register
   
-  if(eventStatus & 0x01) Serial.print("CPU Reset required");
- 
    // Check for errors
   if(eventStatus & 0x02) { // error detected, what is it?
   
@@ -756,7 +736,7 @@ void loop()
     az = (float)accelCount[2]*0.000488;  
   }
   
-   if(eventStatus & 0x20) { // new gyro data available
+   if(readByte(EM7180_ADDRESS, EM7180_EventStatus) & 0x20) { // new gyro data available
     readSENtralGyroData(gyroCount);
   
     // Now we'll calculate the gyro value into actual dps's
@@ -765,24 +745,18 @@ void loop()
     gz = (float)gyroCount[2]*0.153;  
    }
 
-  if(eventStatus & 0x08) { // new mag data available
+  if(readByte(EM7180_ADDRESS, EM7180_EventStatus) & 0x08) { // new mag data available
     readSENtralMagData(magCount);
   
-    // Now we'll calculate the mag value into actual mG's
-    mx = (float)magCount[0]*305.176*1000.;  // get actual mG value
-    my = (float)magCount[1]*305.176*1000.;    
-    mz = (float)magCount[2]*305.176*1000.;  
+    // Now we'll calculate the mag value into actual G's
+    mx = (float)magCount[0]*0.305176;  // get actual G value
+    my = (float)magCount[1]*0.305176;    
+    mz = (float)magCount[2]*0.305176;  
    }
    
-    if(eventStatus & 0x04) { // new quaternion data available
+    if(readByte(EM7180_ADDRESS, EM7180_EventStatus) & 0x04) { // new quaternion data available
     readSENtralQuatData(Quat); 
-    
-    q[1] = Quat[0]/4294967296.0; // Qx, normalized to 1
-    q[2] = Quat[1]/4294967296.0; // Qy
-    q[3] = Quat[2]/4294967296.0; // Qz
-    q[0] = Quat[3]/4294967296.0; // Qw
    }
-
   }
  
   if(passThru) {
@@ -814,6 +788,7 @@ void loop()
  //   }
   } 
  
+ 
  // keep track of rates
   Now = micros();
   deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
@@ -829,7 +804,7 @@ void loop()
   // in the MPU9250 sensor. This rotation can be modified to allow any convenient orientation convention.
   // This is ok by aircraft orientation standards!  
   // Pass gyro rate as rad/s
-  if(passThru) {MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  -my,  mx, mz);}
+    MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  -my,  mx, mz);
 //  if(passThru)MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, -my, mx, mz);
 
     // Serial print and/or display at 0.5 s rate independent of data rates
@@ -847,12 +822,19 @@ void loop()
     Serial.print(" my = "); Serial.print( (int)my); 
     Serial.print(" mz = "); Serial.print( (int)mz); Serial.println(" mG");
     
+    Serial.println("Software quaternions:"); 
     Serial.print("q0 = "); Serial.print(q[0]);
     Serial.print(" qx = "); Serial.print(q[1]); 
     Serial.print(" qy = "); Serial.print(q[2]); 
     Serial.print(" qz = "); Serial.println(q[3]); 
+    Serial.println("Hardware quaternions:"); 
+    Serial.print("Q0 = "); Serial.print(Quat[3]);
+    Serial.print(" Qx = "); Serial.print(Quat[0]); 
+    Serial.print(" Qy = "); Serial.print(Quat[1]); 
+    Serial.print(" Qz = "); Serial.println(Quat[2]); 
     }               
  
+
 // tempCount = readTempData();  // Read the gyro adc values
 //    temperature = ((float) tempCount) / 333.87 + 21.0; // Gyro chip temperature in degrees Centigrade
    // Print temperature in degrees Centigrade      
@@ -902,7 +884,8 @@ void loop()
     }
    }
    
-  // Define output variables from updated quaternion (q[0] = qw, q[1] = qx)---these are Tait-Bryan angles, commonly used in aircraft orientation.
+    
+  // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
   // In this coordinate system, the positive z-axis is down toward Earth. 
   // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
@@ -911,6 +894,7 @@ void loop()
   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
+    //Software AHRS:
     yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
     pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
     roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
@@ -918,6 +902,14 @@ void loop()
     yaw   *= 180.0f / PI; 
     yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
     roll  *= 180.0f / PI;
+    //Hardware AHRS:
+    Yaw   = atan2(2.0f * (Quat[0] * Quat[1] + Quat[3] * Quat[2]), Quat[3] * Quat[3] + Quat[0] * Quat[0] - Quat[1] * Quat[1] - Quat[2] * Quat[2]);   
+    Pitch = -asin(2.0f * (Quat[0] * Quat[2] - Quat[3] * Quat[1]));
+    Roll  = atan2(2.0f * (Quat[3] * Quat[0] + Quat[1] * Quat[2]), Quat[3] * Quat[3] - Quat[0] * Quat[0] - Quat[1] * Quat[1] + Quat[2] * Quat[2]);
+    Pitch *= 180.0f / PI;
+    Yaw   *= 180.0f / PI; 
+    Yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
+    Roll  *= 180.0f / PI;
      
     // Or define output variable according to the Android system, where heading (0 to 260) is defined by the angle between the y-axis 
     // and True North, pitch is rotation about the x-axis (-180 to +180), and roll is rotation about the y-axis (-90 to +90)
@@ -926,16 +918,23 @@ void loop()
     //
     
     if(SerialDebug) {
-    Serial.print("Yaw, Pitch, Roll: ");
+    Serial.print("Software yaw, pitch, roll: ");
     Serial.print(yaw, 2);
     Serial.print(", ");
     Serial.print(pitch, 2);
     Serial.print(", ");
     Serial.println(roll, 2);
 
+    Serial.print("Hardware Yaw, Pitch, Roll: ");
+    Serial.print(Yaw, 2);
+    Serial.print(", ");
+    Serial.print(Pitch, 2);
+    Serial.print(", ");
+    Serial.println(Roll, 2);
+    
     Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz");
     }
-   
+ /*  
     display.clearDisplay();    
  
     display.setCursor(0, 0); display.print(" x   y   z ");
@@ -964,23 +963,12 @@ void loop()
     display.setCursor(48, 32); display.print((int)(roll)); 
     display.setCursor(66, 32); display.print("ypr");  
   
-    // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and 
-    // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz.
-    // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, 
-    // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR:
-    // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
-    // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. 
-    // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
-    // This filter update rate should be fast enough to maintain accurate platform orientation for 
-    // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
-    // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
-    // The 3.3 V 8 MHz Pro Mini is doing pretty well!
  
 //    display.setCursor(0, 40); display.print(altitude, 0); display.print("ft"); 
 //    display.setCursor(68, 0); display.print(9.*Temperature/5. + 32., 0); 
     display.setCursor(42, 40); display.print((float) sumCount / (1000.*sum), 2); display.print("kHz"); 
     display.display();
-
+*/
     digitalWrite(myLed, !digitalRead(myLed));
     count = millis(); 
     sumCount = 0;
@@ -1037,20 +1025,28 @@ void getAres() {
   }
 }
 
+float uint32_reg_to_float (uint8_t *buf)
+{
+  union {
+    uint32_t ui32;
+    float f;
+  } u;
+
+  u.ui32 =     (((uint32_t)buf[0]) +
+               (((uint32_t)buf[1]) <<  8) +
+               (((uint32_t)buf[2]) << 16) +
+               (((uint32_t)buf[3]) << 24));
+  return u.f;
+}
+
 void readSENtralQuatData(float * destination)
 {
   uint8_t rawData[16];  // x/y/z quaternion register data stored here
-  uint32_t temp[4] = {0, 0, 0, 0};
   readBytes(EM7180_ADDRESS, EM7180_QX, 16, &rawData[0]);       // Read the sixteen raw data registers into data array
-  temp[0] = (uint32_t) ((uint32_t)rawData[3] << 24 | (uint32_t)rawData[2] << 16 |(uint32_t)rawData[1] << 8 |  rawData[0]);  // Turn the MSB and LSB into a signed 16-bit value
-  temp[1] = (uint32_t) ((uint32_t)rawData[7] << 24 | (uint32_t)rawData[6] << 16 |(uint32_t)rawData[5] << 8 |  rawData[4]); 
-  temp[2] = (uint32_t) ((uint32_t)rawData[11] << 24 | (uint32_t)rawData[10] << 16 |(uint32_t)rawData[9] << 8 |  rawData[8]);
-  temp[3] = (uint32_t) ((uint32_t)rawData[15] << 24 | (uint32_t)rawData[14] << 16 |(uint32_t)rawData[13] << 8 |  rawData[12]);
-
-  destination[0] = (float) temp[0];
-  destination[1] = (float) temp[1]; 
-  destination[2] = (float) temp[2]; 
-  destination[3] = (float) temp[3];
+  destination[0] = uint32_reg_to_float (&rawData[0]);
+  destination[1] = uint32_reg_to_float (&rawData[4]);
+  destination[2] = uint32_reg_to_float (&rawData[8]);
+  destination[3] = uint32_reg_to_float (&rawData[12]);
 
 }