Merge pull request #326 from hjr/master

Boot-up printf for IMU check

components/MPUdriver/include/mpu/types.hpp

@@ -475,6 +475,8 @@ struct axes_t
     void setZero() { x=y=z=.0; }
     axes_t<T>& operator+=(const axes_t<int16_t>& right)
         { x += right.x; y += right.y; z += right.z; return *this; }
+    axes_t<T>& operator+=(const axes_t<float>& right)
+        { x += right.x; y += right.y; z += right.z; return *this; }
     axes_t<T> operator+(const axes_t<T>& right)
         { axes_t<T> tmp(*this); tmp += right; return tmp; }
     axes_t<T>& operator-=(const axes_t<int16_t>& right)
@@ -487,6 +489,7 @@ struct axes_t
     	return ("X:" + String(x) + " Y:" + String(y) + " Z:" + String(z)).c_str();
     };
 };
+
 // Ready-to-use axes types
 typedef axes_t<int16_t> raw_axes_t;  //!< Axes type to hold gyroscope, accelerometer, magnetometer raw data.
 typedef axes_t<float> float_axes_t;  //!< Axes type to hold converted sensor data.

main/Compass.h

@@ -20,6 +20,7 @@ Last update: 2021-03-07
 #pragma once
 
 #include "QMC5883L.h"
+#include "average.h"
 #include "Deviation.h"
 
 typedef struct float_axes {

main/KalmanMPU6050.cpp

@@ -212,7 +212,7 @@ void IMU::Process()
 
 		// Centripetal forces to keep angle of bank while circling
 		petal.a = -sin(pitch);               // Nose down (positive Y turn) results in negative X force
-		petal.b = sin(roll)*cos(pitch);      // Rigrht wing down (or positive X roll) results in positive Y force
+		petal.b = sin(roll)*cos(pitch);      // Right wing down (or positive X roll) results in positive Y force
 		petal.c = cos(roll)*cos(pitch);      // Any roll or pitch creates a smaller positive Z, gravity Z is positive
 		// trust in gyro at load factors unequal 1 g
 		gravity_trust = (ahrs_min_gyro_factor.get() + (ahrs_gyro_factor.get() * ( pow(10, abs(loadFactor-1) * ahrs_dynamic_factor.get()) - 1)));

main/QMC5883L.cpp

@@ -201,7 +201,6 @@ esp_err_t QMC5883L::initialize2( int a_odr, int a_osr )
 {
 	esp_err_t e1, e2, e3, e4;
 	e1 = e2 = e3 = e4 = 0;
-	X = Y = Z = 0;
 
 	// Soft Reset
 	e1 = writeRegister( addr, REG_CONTROL2, SOFT_RST );
@@ -234,7 +233,7 @@ esp_err_t QMC5883L::initialize2( int a_odr, int a_osr )
 	return ESP_FAIL;
 }
 
-bool QMC5883L::rawAxes( t_magn_axes &axes )
+bool QMC5883L::rawAxes( t_magn_axes &axes_p )
 {
 	uint8_t data[6];
 	uint8_t status = 0;
@@ -282,13 +281,11 @@ bool QMC5883L::rawAxes( t_magn_axes &axes )
 	// Data can be read in every case
 	if( count == 6 )
 	{
-		X = (int)( (int16_t)(( data[1] << 8 ) | data[0]) );
-		Y = (int)( (int16_t)(( data[3] << 8 ) | data[2]) );
-		Z = (int)( (int16_t)(( data[5] << 8 ) | data[4]) );
+		axes.x = (int)( (int16_t)(( data[1] << 8 ) | data[0]) );
+		axes.y = (int)( (int16_t)(( data[3] << 8 ) | data[2]) );
+		axes.z = (int)( (int16_t)(( data[5] << 8 ) | data[4]) );
 
-		axes.x = filterX( X );
-		axes.y = filterY( Y );
-		axes.z = filterZ( Z );
+		axes_p = axes;
 
 		// ESP_LOGI( FNAME, "Mag Average: X:%d Y:%d Z:%d  Raw: X:%d Y:%d Z:%d", axes.x, axes.y, axes.z, x, y, z );
 

main/QMC5883L.h

@@ -25,7 +25,6 @@ Last update: 2021-03-28
 #include "logdef.h"
 #include "esp_log.h"
 #include "I2Cbus.hpp"
-#include "average.h"
 #include "WString.h"
 #include "MagnetSensor.h"
 
@@ -61,10 +60,10 @@ class QMC5883L: public MagnetSensor
 	//  Write with data part
 	bool overflowFlag()	{ return overflowWarning; }
 	// Read out the registers X, Y, Z (0...5) in raw format into variables, return true if success
-	bool rawAxes( t_magn_axes &axes );
-	int curX(){ return X; };
-	int curY(){ return X; };
-	int curZ(){ return X; };
+	bool rawAxes( t_magn_axes &axes_p );
+	int curX(){ return axes.x; };
+	int curY(){ return axes.y; };
+	int curZ(){ return axes.z; };
 private:
 	// Configure the device with the set parameters and set the mode to continuous.
 	esp_err_t initialize2( int a_odr=0, int a_osr=0 );
@@ -87,9 +86,6 @@ class QMC5883L: public MagnetSensor
 	uint8_t range; // magnetic resolution of sensor
 	uint8_t osr; // over sample ratio
 	bool overflowWarning;
-	Average<20, int16_t> filterX;
-	Average<20, int16_t> filterY;
-	Average<20, int16_t> filterZ;
-	int X,Y,Z;
+	t_magn_axes axes;
 	int age;
 };

main/sensor.cpp

@@ -1004,19 +1004,24 @@ void system_startup(void *args){
 			MPU.setGyroOffset(gb);
 		}
 		mpud::raw_axes_t accelRaw;
-		float accel = 0;
+		mpud::float_axes_t accelG;
+		float samples = 0;
 		delay(200);
 		for( auto i=0; i<10; i++ ){
 			esp_err_t err = MPU.acceleration(&accelRaw);  // fetch raw data from the registers
-			if( err != ESP_OK )
+			if( err != ESP_OK ) {
 				ESP_LOGE(FNAME, "AHRS acceleration I2C read error");
-			mpud::float_axes_t accelG = mpud::accelGravity(accelRaw, mpud::ACCEL_FS_8G);  // raw data to gravity
+				continue;
+			}
+			samples++;
+			accelG += mpud::accelGravity(accelRaw, mpud::ACCEL_FS_8G);  // raw data to gravity
 			ESP_LOGI( FNAME,"MPU %.2f", accelG[0] );
 			delay( 10 );
-			accel += accelG[0];
 		}
 		char ahrs[30];
-		sprintf( ahrs,"AHRS Sensor: OK (%.2f g)", accel/10 );
+		accelG /= samples;
+		float accel = sqrt(accelG[0]*accelG[0]+accelG[1]*accelG[1]+accelG[2]*accelG[2]);
+		sprintf( ahrs,"AHRS Sensor: OK (%.2f g)", accel );
 		display->writeText( line++, ahrs );
 		logged_tests += "MPU6050 AHRS test: PASSED\n";
 		IMU::init();