Mercurial > public > ostc4

diff Small_CPU/Src/compass.c @ 1007:65d35e66efb9 GasConsumption

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Improve compass calibration dialog: The previous calibration dialog showed some "magic" numbers and a 60 second count down. The new version is trying to guide the user through the calibration process: first rotate pitch, then roll and at last yaw angle. A step to the next angle is taken when enough data per angle is collected (change from red to green). To enable the yaw visualization a simple calibration is done while rotating the axis. The function behind the calibration was not modified => the suggested process can be ignored and the same handling as the with old dialog may be applied. With the new process the dialog may be left early. Anyhow it will still be left after 60 seconds and the fine calibration is performed in the same way as before.
author Ideenmodellierer
date Mon, 05 May 2025 21:02:34 +0200
parents 08af5d707c5a
children 08f76d3e9856
line wrap: on
line diff
--- a/Small_CPU/Src/compass.c	Sat May 03 17:52:05 2025 +0200
+++ b/Small_CPU/Src/compass.c	Mon May 05 21:02:34 2025 +0200
@@ -86,6 +86,17 @@
 int16_t compass_CZ_f; ///< calibration value
 
 
+float compass_DX_min;
+float compass_DX_max;
+
+float compass_DY_min;
+float compass_DY_max;
+
+float compass_DZ_min;
+float compass_DZ_max;
+
+
+
 /// The (filtered) components of the accelerometer sensor
 int16_t accel_DX_f; ///< output from sensor
 int16_t accel_DY_f; ///< output from sensor
@@ -1070,8 +1081,25 @@
 //  ===============================================================================
 void compass_calc_roll_pitch_only(void)
 {
-	 float sinPhi, cosPhi;
+	 float sinPhi, cosPhi, sinTeta, cosTeta;
 	 float Phi, Teta;
+	 float mx_corr, my_corr;
+
+	 float offsetX, offsetY, offsetZ;
+	 float scaleX, scaleY,scaleZ;
+
+	 float local_DX = compass_DX_f;
+	 float local_DY = compass_DY_f;
+	 float local_DZ = compass_DZ_f;
+
+
+	 if(local_DX < compass_DX_min) compass_DX_min = local_DX;
+	 if(local_DY < compass_DY_min) compass_DY_min = local_DY;
+	 if(local_DZ < compass_DZ_min) compass_DZ_min = local_DZ;
+
+	 if(local_DX > compass_DX_max) compass_DX_max = local_DX;
+	 if(local_DY > compass_DY_max) compass_DY_max = local_DY;
+	 if(local_DZ > compass_DZ_max) compass_DZ_max = local_DZ;
 
 	//---- Calculate sine and cosine of roll angle Phi -----------------------
 	Phi= atan2f(accel_DY_f, accel_DZ_f) ;
@@ -1079,9 +1107,38 @@
 	sinPhi = sinf(Phi);
 	cosPhi = cosf(Phi);
 
-	//---- calculate sin and cosine of pitch angle Theta ---------------------
-	Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
-	compass_pitch = Teta * 180.0f /PI;
+	Teta = atan2((double)accel_DX_f, sqrt((double)accel_DY_f * accel_DY_f + (double)accel_DZ_f * accel_DZ_f));
+	compass_pitch = Teta * (180.0 / PI);
+
+	sinTeta = sinf(Teta);
+	cosTeta = cosf(Teta);
+
+	offsetX = (compass_DX_max + compass_DX_min) / 2.0;
+	offsetY = (compass_DY_max + compass_DY_min) / 2.0;
+	offsetZ = (compass_DZ_max + compass_DZ_min) / 2.0;
+
+	scaleX = 2.0 / (compass_DX_max - compass_DX_min);
+	scaleY = 2.0 / (compass_DY_max - compass_DY_min);
+	scaleZ = 2.0 / (compass_DZ_max - compass_DZ_min);
+
+	local_DX -= offsetX;
+	local_DX *= scaleX;
+
+	local_DY -= offsetY;
+	local_DY *= scaleY;
+
+	local_DZ -= offsetZ;
+	local_DZ *= scaleZ;
+
+	mx_corr = local_DX * cosTeta + local_DZ * sinTeta;
+	my_corr = local_DX * sinPhi * sinTeta + local_DY * cosPhi - local_DZ * sinPhi * cosTeta;
+
+	compass_heading = atan2(my_corr, mx_corr) * (180.0 / PI);
+
+
+    if (compass_heading < 0) {
+    	compass_heading += 360.0;
+    }
 }
 
 
@@ -1172,6 +1229,14 @@
     g->Suuu = g->Svvv = g->Swww = 0.0;
     g->Suuv = g->Suuw = g->Svvu = g->Svvw = g->Swwu = g->Swwv = 0.0;
     compass_CX_f = compass_CY_f = compass_CZ_f = 0.0;
+
+    compass_DX_min = 10000;
+    compass_DY_min = 10000;
+    compass_DZ_min = 10000;
+
+    compass_DX_max = -10000;
+    compass_DY_max = -10000;
+    compass_DZ_max = -10000;
 }