changeset 1007:65d35e66efb9 GasConsumption

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 (3 months ago)
parents 75f958ca5d0e
children 6233e5877e3e
files Discovery/Inc/gfx_engine.h Discovery/Inc/tInfo.h Discovery/Inc/tInfoCompass.h Discovery/Src/data_central.c Discovery/Src/gfx_engine.c Discovery/Src/tInfo.c Discovery/Src/tInfoCompass.c Small_CPU/Src/compass.c
diffstat 8 files changed, 320 insertions(+), 40 deletions(-) [+]
line wrap: on
line diff
--- a/Discovery/Inc/gfx_engine.h	Sat May 03 17:52:05 2025 +0200
+++ b/Discovery/Inc/gfx_engine.h	Mon May 05 21:02:34 2025 +0200
@@ -140,6 +140,7 @@
 void GFX_clear_window_immediately(GFX_DrawCfgWindow* hgfx);
 
 //void GFX_draw_circle_with_MEMORY(uint8_t use_memory, GFX_DrawCfgScreen *hgfx, point_t center, uint8_t radius, int8_t color);
+void GFX_draw_pixel(GFX_DrawCfgScreen *hgfx, int16_t x, int16_t y, uint8_t color);
 void GFX_draw_circle(GFX_DrawCfgScreen *hgfx, point_t center, uint8_t radius, int8_t color);
 void GFX_draw_colorline(GFX_DrawCfgScreen *hgfx, point_t start, point_t stop, uint8_t color);
 void GFX_draw_thick_line(uint8_t thickness, GFX_DrawCfgScreen *hgfx, point_t start, point_t stop, uint8_t color);
--- a/Discovery/Inc/tInfo.h	Sat May 03 17:52:05 2025 +0200
+++ b/Discovery/Inc/tInfo.h	Mon May 05 21:02:34 2025 +0200
@@ -60,6 +60,10 @@
 void tInfo_write_buttonTextline(GFX_DrawCfgScreen *screenPtr, uint8_t left2ByteCode, char middle2ByteCode, char right2ByteCode);
 void tInfo_write_buttonTextline_simple(uint8_t left2ByteCode, char middle2ByteCode, char right2ByteCode);
 
+void tInfo_drawPixel(int16_t x, int16_t y, uint8_t color);
+void tInfo_draw_colorline(point_t start, point_t stop, uint8_t color);
+void t_Info_draw_circle(point_t center, uint8_t radius, int8_t color);
+
 void tInfo_setEvent(uint32_t inputEventID, uint32_t inputFunctionCall);
 
 void tInfo_set_on_off(uint32_t editID, uint8_t int1);
--- a/Discovery/Inc/tInfoCompass.h	Sat May 03 17:52:05 2025 +0200
+++ b/Discovery/Inc/tInfoCompass.h	Mon May 05 21:02:34 2025 +0200
@@ -28,6 +28,15 @@
 #ifndef TINFO_COMPASS_H
 #define TINFO_COMPASS_H
 
+#include "gfx.h"
+
+typedef struct
+{
+	point_t coord;
+	point_t eclipse;
+	uint8_t check[360];
+} axisIndicator_t;
+
 /* Exported functions --------------------------------------------------------*/
 void openInfo_Compass(void);
 void refreshInfo_Compass(GFX_DrawCfgScreen s);
--- a/Discovery/Src/data_central.c	Sat May 03 17:52:05 2025 +0200
+++ b/Discovery/Src/data_central.c	Mon May 05 21:02:34 2025 +0200
@@ -843,30 +843,33 @@
 {
 	float newTarget = newHeading;
 
-	if(settingsGetPointer()->compassInertia == 0)
+	if((newHeading > 0.0) && (newHeading < 360))
 	{
-		compass_compensated = newHeading;
-	}
-	else
-	{
-		if((compass_compensated > 270.0) && (newHeading < 90.0))		/* transition passing 0 clockwise */
+		if(settingsGetPointer()->compassInertia == 0)
 		{
-			newTarget = newHeading + 360.0;
+			compass_compensated = newHeading;
 		}
+		else
+		{
+			if((compass_compensated > 270.0) && (newHeading < 90.0))		/* transition passing 0 clockwise */
+			{
+				newTarget = newHeading + 360.0;
+			}
 
-		if((compass_compensated < 90.0) && (newHeading > 270.0))		/* transition passing 0 counter clockwise */
-		{
-			newTarget = newHeading - 360.0;
-		}
+			if((compass_compensated < 90.0) && (newHeading > 270.0))		/* transition passing 0 counter clockwise */
+			{
+				newTarget = newHeading - 360.0;
+			}
 
-		compass_compensated = compass_compensated + ((newTarget - compass_compensated) / (COMPASS_FRACTION * (settingsGetPointer()->compassInertia)));
-		if(compass_compensated < 0.0)
-		{
-			compass_compensated += 360.0;
-		}
-		if(compass_compensated >= 360.0)
-		{
-			compass_compensated -= 360.0;
+			compass_compensated = compass_compensated + ((newTarget - compass_compensated) / (COMPASS_FRACTION * (settingsGetPointer()->compassInertia)));
+			if(compass_compensated < 0.0)
+			{
+				compass_compensated += 360.0;
+			}
+			if(compass_compensated >= 360.0)
+			{
+				compass_compensated -= 360.0;
+			}
 		}
 	}
 }
--- a/Discovery/Src/gfx_engine.c	Sat May 03 17:52:05 2025 +0200
+++ b/Discovery/Src/gfx_engine.c	Mon May 05 21:02:34 2025 +0200
@@ -1075,7 +1075,7 @@
 
 
 /* this is NOT fast nor optimized */
-static void GFX_draw_pixel(GFX_DrawCfgScreen *hgfx, int16_t x, int16_t y, uint8_t color)
+void GFX_draw_pixel(GFX_DrawCfgScreen *hgfx, int16_t x, int16_t y, uint8_t color)
 {
 	uint16_t* pDestination;
 
--- a/Discovery/Src/tInfo.c	Sat May 03 17:52:05 2025 +0200
+++ b/Discovery/Src/tInfo.c	Mon May 05 21:02:34 2025 +0200
@@ -298,6 +298,32 @@
 
 }
 
+
+void tInfo_drawPixel(int16_t x, int16_t y, uint8_t color)
+{
+	int8_t xoff;
+	int8_t yoff;
+
+	for (xoff = -1; xoff < 2; xoff++)
+	{
+		for (yoff = -1; yoff < 2; yoff++)
+		{
+			GFX_draw_pixel(&tIscreen, x + xoff, y + yoff, color);
+		}
+	}
+	GFX_draw_pixel(&tIscreen, x, y, color);
+}
+
+void tInfo_draw_colorline(point_t start, point_t stop, uint8_t color)
+{
+	GFX_draw_colorline(&tIscreen, start, stop, color);
+}
+
+void t_Info_draw_circle(point_t center, uint8_t radius, int8_t color)
+{
+	GFX_draw_circle(&tIscreen, center, radius, color);
+}
+
 void tInfo_write_content_simple(uint16_t XleftGimpStyle, uint16_t XrightGimpStyle, uint16_t YtopGimpStyle,  const tFont *Font, const char *text, uint8_t color)
 {
     GFX_DrawCfgWindow	hgfx;
--- a/Discovery/Src/tInfoCompass.c	Sat May 03 17:52:05 2025 +0200
+++ b/Discovery/Src/tInfoCompass.c	Mon May 05 21:02:34 2025 +0200
@@ -28,34 +28,77 @@
 
 /* Includes ------------------------------------------------------------------*/
 
-#include "gfx_engine.h"
+
 #include "gfx_fonts.h"
 #include "tHome.h"
 #include "tInfo.h"
 #include "tInfoCompass.h"
 
 #include <string.h>
+#include <math.h>
+
+#define PI 3.14159265358979323846
+
+#define PITCH 	(0)
+#define ROLL	(1)
+#define YAW 	(2)
 
 /* Private variables ---------------------------------------------------------*/
 
-uint16_t tInfoCompassTimeout = 0;
-int16_t minMaxCompassDX[3][2] = { 0 };
+static uint16_t tInfoCompassTimeout = 0;
+static int16_t minMaxCompassDX[3][2] = { 0 };
+
+static axisIndicator_t axis[3];
+static uint8_t activeAxis = PITCH;
+static uint32_t checkTick = 0;
 
 /* Exported functions --------------------------------------------------------*/
 void openInfo_Compass(void)
 {
+	uint16_t angle = 0;
+	uint8_t axisIndex = 0;
     set_globalState(StICOMPASS);
     tInfoCompassTimeout = settingsGetPointer()->timeoutInfoCompass;
     tInfoCompassTimeout *= 10;
 
+    for(axisIndex = 0; axisIndex < 3; axisIndex++)
+    {
+    	memset(axis[axisIndex].check,0, 360);
+    }
+    for(angle = 170; angle < 360; angle++)
+    {
+       	axis[PITCH].check[angle] = 1;	/* pitch only from -90 to +90 */
+    }
+    axis[YAW].coord.x = 100;
+    axis[YAW].coord.y = 70;
+    axis[PITCH].coord.x = 100;
+    axis[PITCH].coord.y = 300;
+    axis[ROLL].coord.x = 100;
+    axis[ROLL].coord.y = 200;
+
+    axis[YAW].eclipse.x = 50;
+    axis[YAW].eclipse.y = 50;
+    axis[PITCH].eclipse.x = 20;
+    axis[PITCH].eclipse.y = 50;
+    axis[ROLL].eclipse.x = 50;
+    axis[ROLL].eclipse.y = 20;
+
     for(int i = 0; i<3;i ++)
     {
             minMaxCompassDX[i][0] = 999;
             minMaxCompassDX[i][1] = -999;
     }
+    checkTick = HAL_GetTick();
+
+    activeAxis = PITCH;
 }
 
-
+void getElipsePoint(uint16_t elipseX , int16_t elipseY, int16_t degree, int16_t *x, int16_t *y)
+{
+    double rad = degree * (PI / 180.0);
+    *x = (int16_t) (elipseX * cos(rad));
+    *y = (int16_t) (elipseY * sin(rad));
+}
 //  ===============================================================================
 //	refreshInfo_Compass
 /// @brief	there is only compass_DX_f, compass_DY_f, compass_DZ_f output during this mode
@@ -63,6 +106,20 @@
 //  ===============================================================================
 void refreshInfo_Compass(GFX_DrawCfgScreen s)
 {
+	static int16_t cursorAngle = 0;
+
+	int16_t angle = 0.0;
+	int16_t drawX = 0;
+	int16_t drawY = 0;
+	uint8_t color = 0;
+	point_t start;
+	point_t stop;
+	point_t center;
+
+	int8_t offset = 0;
+	uint8_t axisIndex = 0;
+	int16_t index = 0;
+	uint8_t textIndex = 0;
 
 	tHome_show_lost_connection_count(&s);
     tInfoCompassTimeout--;
@@ -80,6 +137,106 @@
     compassValues[1] = stateUsed->lifeData.compass_DY_f;
     compassValues[2] = stateUsed->lifeData.compass_DZ_f;
 
+   /* draw indicator */
+    for(axisIndex = 0; axisIndex < 3; axisIndex++)
+    {
+    	switch(axisIndex)
+    	{
+    		default:
+    		case YAW:	index = (uint16_t)(stateUsed->lifeData.compass_heading);
+    					textIndex = snprintf(text,80,"yaw %.1f", stateUsed->lifeData.compass_heading);
+
+    					if((tInfoCompassTimeout < 50) && (activeAxis == YAW))
+    					{
+    						snprintf(&text[textIndex],80,"\023\t(%i, %i)", minMaxCompassDX[YAW][0], minMaxCompassDX[YAW][1]);
+    					}
+
+    					start.x = axis[axisIndex].coord.x - 1;
+    		    	    start.y = axis[axisIndex].coord.y;
+    		    	    stop.x = axis[axisIndex].coord.x + 1;
+    		    	    stop.y = axis[axisIndex].coord.y;
+    			break;
+    		case PITCH:	index = (uint16_t)(stateUsed->lifeData.compass_pitch + 90);
+    					textIndex = snprintf(text,80,"pitch %.1f", stateUsed->lifeData.compass_pitch);
+    					if((tInfoCompassTimeout < 50) && (activeAxis == YAW))
+    					{
+    						snprintf(&text[textIndex],80,"\023\t(%i, %i)", minMaxCompassDX[PITCH][0], minMaxCompassDX[PITCH][1]);
+    					}
+    					start.x = axis[axisIndex].coord.x - 30;
+    					start.y = axis[axisIndex].coord.y;
+    					stop.x = axis[axisIndex].coord.x + 30;
+    					stop.y = axis[axisIndex].coord.y;
+
+    			break;
+    		case ROLL: index = (uint16_t)(stateUsed->lifeData.compass_roll + 180);
+    					textIndex = snprintf(text,80,"roll %.1f", stateUsed->lifeData.compass_roll);
+    					if((tInfoCompassTimeout < 50) && (activeAxis == YAW))
+    					{
+    						snprintf(&text[textIndex],80,"\023\t(%i, %i)", minMaxCompassDX[ROLL][0], minMaxCompassDX[ROLL][1]);
+    					}
+    					start.x = axis[axisIndex].coord.x;
+    					start.y = axis[axisIndex].coord.y - 30;
+    					stop.x = axis[axisIndex].coord.x;
+    					stop.y = axis[axisIndex].coord.y +30;
+    			break;
+    	}
+		if(settingsGetPointer()->FlipDisplay)
+		{
+			start.x = 800 - start.x;
+			stop.x = 800 - stop.x;
+			start.y = 480 - start.y;
+			stop.y = 480 - stop.y;
+		}
+    	if(activeAxis == axisIndex)			/* only check one axis at a time. reason: yaw will be unstable at the beginning of calibration */
+    	{
+    		for(offset = -6; offset <= 6; offset++)	/* it is hard to hit every single angle and the resolution is not needed */
+    		{
+				if(( (index + offset) >= 0) && (index + offset < 360))
+				{
+					axis[axisIndex].check[index + offset] = 1;							/* => check surrounding angles as well */
+				}
+    		}
+    	}
+    	if(axisIndex == activeAxis)
+    	{
+    		color = CLUT_InfoCompass;
+    		getElipsePoint(axis[axisIndex].eclipse.x,axis[axisIndex].eclipse.y,cursorAngle, &drawX, &drawY);
+    		center.x = axis[axisIndex].coord.x + drawX;
+    		center.y = axis[axisIndex].coord.y + drawY;
+
+    		t_Info_draw_circle(center, 4, CLUT_Font020);
+    		cursorAngle += 15;
+    		if(cursorAngle >= 360)
+    		{
+    			cursorAngle = 0;
+    		}
+    	}
+    	else
+    	{
+    		color = CLUT_Font021;
+    	}
+    	tInfo_write_content_simple(  200, 600, 480 - axis[axisIndex].coord.y - 35 , &FontT42, text, color);
+  	    tInfo_draw_colorline(start, stop, CLUT_Font020);
+  	    for(angle = 0; angle < 360; angle = angle + 3)
+  	    {
+  	    	if(axis[axisIndex].check[angle])
+  	    	{
+  	    		color = CLUT_NiceGreen;
+  	    	}
+  	    	else
+  	    	{
+  	    		color = CLUT_WarningRed;
+  	    	}
+  	    	getElipsePoint(axis[axisIndex].eclipse.x,axis[axisIndex].eclipse.y,angle, &drawX, &drawY);
+  	    	tInfo_drawPixel(axis[axisIndex].coord.x + drawX, axis[axisIndex].coord.y + drawY,color);
+  	    	if((axisIndex == PITCH) && (angle == 180))		/* pitch only from -90 to +90 */
+  	    	{
+  	    		break;
+  	    	}
+  	    }
+
+    }
+
     for(int i = 0; i<3;i ++)
     {
         // do not accept zero
@@ -100,18 +257,33 @@
     snprintf(text,80,"Time left: %u s",(tInfoCompassTimeout+9)/10);
     tInfo_write_content_simple(  20,800,  25, &FontT42, text, CLUT_InfoCompass);
 
-    for(int i = 0; i<3;i ++)
+
+    if(time_elapsed_ms(checkTick, HAL_GetTick() > 1000))
     {
-        snprintf(text,80,"%c: %i" "\t(%i, %i)",
-            'X'+i,
-            compassValues[i],
-            minMaxCompassDX[i][0],
-            minMaxCompassDX[i][1]);
-        tInfo_write_content_simple(  20,800, 96 + (i*96), &FontT48, text, CLUT_InfoCompass);
+    	index = 0;
+    	 for(angle = 0; angle < 360; angle++)
+    	 {
+    		 if(axis[activeAxis].check[angle])
+    		 {
+    			 index++;
+    		 }
+    	 }
+    	 if(index > 350)
+    	 {
+    		 if(activeAxis < 2)
+    		 {
+    			 activeAxis++;
+    		 }
+    		 else
+    		 {
+    			 if(tInfoCompassTimeout > 50)
+    			 {
+    				 tInfoCompassTimeout = 50;	/* reduce exit time to five seconds */
+    			 }
+    		 }
+    	 }
+
+
+    	checkTick = HAL_GetTick();
     }
-
-    snprintf(text,80,"roll %.1f" "\tpitch %.1f",
-        stateUsed->lifeData.compass_roll,
-        stateUsed->lifeData.compass_pitch);
-    tInfo_write_content_simple(  20,800, 96 * 4, &FontT42, text, CLUT_InfoCompass);
 }
--- 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;
 }