diff Discovery/Src/motion.c @ 621:6826731ff2be

Use roll and pitch agles in 0-360 format: The roll and pitch values are provided as pos/neg angle values which causes problems in the vector calculation for the motion detection => changed the implementation to store the calibration data with a 180? offset. Also shift value by 180? before they are used for vector calculations.
author Ideenmodellierer
date Wed, 03 Feb 2021 21:44:22 +0100
parents 96af74455420
children 028d8f3a9410
line wrap: on
line diff
--- a/Discovery/Src/motion.c	Wed Jan 27 22:12:44 2021 +0100
+++ b/Discovery/Src/motion.c	Wed Feb 03 21:44:22 2021 +0100
@@ -274,6 +274,7 @@
 
 void InitMotionDetection(void)
 {
+	float sensorPitch = settingsGetPointer()->viewPitch - 180.0;	/* calib values are stored as 360° values. Sensor uses +/- 180° */
 	sectorDetection.target = 0;
 	sectorDetection.current = 0;
 	sectorDetection.size = 0;
@@ -281,12 +282,12 @@
 
 	switch(settingsGetPointer()->MotionDetection)
 	{
-		case MOTION_DETECT_SECTOR: DefinePitchSectors(settingsGetPointer()->viewPitch,CUSTOMER_DEFINED_VIEWS);
+		case MOTION_DETECT_SECTOR: DefinePitchSectors(sensorPitch,CUSTOMER_DEFINED_VIEWS);
 									MapCVToSector();
 			break;
-		case MOTION_DETECT_MOVE: DefinePitchSectors(settingsGetPointer()->viewPitch,SECTOR_MAX);
+		case MOTION_DETECT_MOVE: DefinePitchSectors(sensorPitch,SECTOR_MAX);
 			break;
-		case MOTION_DETECT_SCROLL: DefinePitchSectors(settingsGetPointer()->viewPitch,SECTOR_SCROLL);
+		case MOTION_DETECT_SCROLL: DefinePitchSectors(sensorPitch,SECTOR_SCROLL);
 			break;
 		default:
 			break;
@@ -531,8 +532,8 @@
 {
     SSettings* pSettings = settingsGetPointer();
 
-    pSettings->viewPitch = pitch;
-	pSettings->viewRoll = roll;
+    pSettings->viewPitch = pitch + 180;
+	pSettings->viewRoll = roll+ 180;
 	pSettings->viewYaw = yaw;
 }
 
@@ -560,8 +561,11 @@
 
 	SSettings* pSettings = settingsGetPointer();
 
+	roll += 180;
+	pitch += 180;
+
 	/* calculate base vector taking calibration delta into account yaw (heading) */
-	float compYaw = yaw + pSettings->viewYaw;
+	float compYaw;
 
 	compYaw = 360.0 - yaw; 				/* turn to 0° */
 	compYaw +=  pSettings->viewYaw; 	/* consider calib yaw value */