Added regsettowall command

ENSTABretagneRobotics · Nov 19, 2023 · 2559cbc · 2559cbc
1 parent e9ee56b
commit 2559cbc
Show file tree

Hide file tree

Showing 7 changed files with 45 additions and 1 deletion.
diff --git a/CHANGELOG.txt b/CHANGELOG.txt
@@ -1,3 +1,6 @@
+402 
+Added regsettowall command.
+
 401
 Updated MAVLinkInterface to improve compatibility with TBS Crossfire receiver.
 Added regsettoagl and waitaglt commands.

diff --git a/Commands.h b/Commands.h
@@ -849,6 +849,8 @@ inline int Commands(char* line)
 		EnterCriticalSection(&WallCS);
 		EnterCriticalSection(&StateVariablesCS);
 		u_wall = u;
+		d_wall = 0;
+		psi_wall = 0;
 		bWallDetection = TRUE;
 		LeaveCriticalSection(&StateVariablesCS);
 		LeaveCriticalSection(&WallCS);
@@ -877,6 +879,8 @@ inline int Commands(char* line)
 		EnterCriticalSection(&WallCS);
 		EnterCriticalSection(&StateVariablesCS);
 		u_wall = u;
+		d_wall = 0;
+		psi_wall = 0;
 		bWallTrackingControl = TRUE;
 		LeaveCriticalSection(&StateVariablesCS);
 		LeaveCriticalSection(&WallCS);
@@ -895,6 +899,8 @@ inline int Commands(char* line)
 		EnterCriticalSection(&WallCS);
 		EnterCriticalSection(&StateVariablesCS);
 		u_wall = u;
+		d_wall = 0;
+		psi_wall = 0;
 		bWallAvoidanceControl = TRUE;
 		LeaveCriticalSection(&StateVariablesCS);
 		LeaveCriticalSection(&WallCS);
@@ -3866,7 +3872,9 @@ inline int Commands(char* line)
 		else
 		{
 			EnterCriticalSection(&RegistersCS);
+			EnterCriticalSection(&StateVariablesCS);
 			registers[ival] = Center(psihat);
+			LeaveCriticalSection(&StateVariablesCS);
 			LeaveCriticalSection(&RegistersCS);
 		}
 	}
@@ -3879,7 +3887,9 @@ inline int Commands(char* line)
 		else
 		{
 			EnterCriticalSection(&RegistersCS);
+			EnterCriticalSection(&StateVariablesCS);
 			registers[ival] = Center(xhat);
+			LeaveCriticalSection(&StateVariablesCS);
 			LeaveCriticalSection(&RegistersCS);
 		}
 	}
@@ -3892,7 +3902,9 @@ inline int Commands(char* line)
 		else
 		{
 			EnterCriticalSection(&RegistersCS);
+			EnterCriticalSection(&StateVariablesCS);
 			registers[ival] = Center(yhat);
+			LeaveCriticalSection(&StateVariablesCS);
 			LeaveCriticalSection(&RegistersCS);
 		}
 	}
@@ -3905,7 +3917,9 @@ inline int Commands(char* line)
 		else
 		{
 			EnterCriticalSection(&RegistersCS);
+			EnterCriticalSection(&StateVariablesCS);
 			registers[ival] = Center(zhat);
+			LeaveCriticalSection(&StateVariablesCS);
 			LeaveCriticalSection(&RegistersCS);
 		}
 	}
@@ -3918,10 +3932,30 @@ inline int Commands(char* line)
 		else
 		{
 			EnterCriticalSection(&RegistersCS);
+			EnterCriticalSection(&StateVariablesCS);
 			registers[ival] = altitude_AGL;
+			LeaveCriticalSection(&StateVariablesCS);
 			LeaveCriticalSection(&RegistersCS);
 		}
 	}
+#ifndef DISABLE_OPENCV_SUPPORT
+	else if (sscanf(line, "regsettowall %d %d", &ival1, &ival2) == 2)
+	{
+		if ((ival1 < 0)||(ival1 >= MAX_NB_REGISTERS)||(ival2 < 0)||(ival2 >= MAX_NB_REGISTERS))
+		{
+			printf("Invalid parameter.\n");
+		}
+		else
+		{
+			EnterCriticalSection(&RegistersCS);
+			EnterCriticalSection(&WallCS);
+			registers[ival1] = d_wall;
+			registers[ival2] = psi_wall;
+			LeaveCriticalSection(&WallCS);
+			LeaveCriticalSection(&RegistersCS);
+		}
+	}
+#endif // !DISABLE_OPENCV_SUPPORT
 	else if (sscanf(line, "regprint %d", &ival) == 1)
 	{
 		if ((ival < 0)||(ival >= MAX_NB_REGISTERS))

diff --git a/Globals.cpp b/Globals.cpp
@@ -414,6 +414,7 @@ int bLat_wall = 0;
 int bBrake_wall = 0;
 int procid_wall = 0;
 double u_wall = 0;
+double d_wall = 0, psi_wall = 0;
 
 // Ball variables.
 BOOL bBallTrackingControl[MAX_NB_BALL];

diff --git a/Globals.h b/Globals.h
@@ -655,6 +655,7 @@ extern int bLat_wall;
 extern int bBrake_wall;
 extern int procid_wall;
 extern double u_wall;
+extern double d_wall, psi_wall;
 
 // Ball variables.
 extern BOOL bBallTrackingControl[MAX_NB_BALL];

diff --git a/Main.cpp b/Main.cpp
@@ -227,7 +227,7 @@ int main(int argc, char* argv[])
 
 	srand(GetTickCount());
 
-	printf("\nUxVCtrl V401\n");
+	printf("\nUxVCtrl V402\n");
 	fflush(stdout);
 
 	// Will launch a mission file if specified as argument.

diff --git a/Wall.cpp b/Wall.cpp
@@ -161,6 +161,9 @@ THREAD_PROC_RETURN_VALUE WallThread(void* pParam)
 
 		delta_psi = LineFollowing(phi, e, gamma_infinite_wall, r_wall);
 
+		d_wall = distance;
+		psi_wall = Center(psihat)+phi;
+
 		char szText[256];
 		sprintf(szText, "RNG=%.2fm,ORN=%ddeg", distance, (int)((fmod_2PI(-angle_env-(Center(psihat)+phi)+3.0*M_PI/2.0)+M_PI)*180.0/M_PI));
 		cvPutText(overlayimage, szText, cvPoint(10,videoimgheight-20), &font, CV_RGB_CvScalar(255,0,128));

diff --git a/mission_spec.txt b/mission_spec.txt
@@ -424,6 +424,8 @@
 %
 %regsettoagl regid (regid : int [0,32[)
 %
+%regsettowall regid1 regid2 (regid1 : int [0,32[, regid2 : int [0,32[, set regid1 to the current distance to the wall in m and regid2 to the current orientation of the wall in rad (a command running the wall detection needs to be currently running))
+%
 %regprint regid (regid : int [0,32[)
 %
 %regadd regid1 regid2 (regid1 : int [0,32[, regid2 : int [0,32[, set regid1 to regid1+regid2)