ekf2: verbose (DEBUG_BUILD) print status with Matrix improved print

* ekf2: verbose print status
 * matrix/Matrix improve print output
 * bold diagonal elements, print ring buffer entry size
 * print in scientific notation when >= 10 to respect max size

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>

src/lib/matrix/matrix/Matrix.hpp

@@ -365,13 +365,55 @@ public:
 		}
 	}
 
-	void print() const
+	void print(float eps = 0.00001f) const
 	{
-		// element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end
-		static const size_t n = 15 * N * M + M + 1;
-		char string[n];
-		write_string(string, n);
-		printf("%s\n", string);
+		// print column numbering
+		if (N > 1) {
+			printf("  ");
+
+			for (unsigned i = 0; i < N; i++) {
+				printf("|%2u      ", i);
+
+			}
+
+			printf("\n");
+		}
+
+		const Matrix<Type, M, N> &self = *this;
+
+		for (unsigned i = 0; i < M; i++) {
+			printf("%2u|", i); // print row numbering
+
+			for (unsigned j = 0; j < N; j++) {
+				double d = static_cast<double>(self(i, j));
+
+				// Matrix diagonal elements
+				if (N > 1 && M > 1 && i == j) {
+					// make diagonal elements bold (ANSI CSI n 1)
+					printf("\033[1m");
+				}
+
+				// avoid -0.0 for display
+				if (fabs(d - 0.0) < (double)eps) {
+					// print fixed width zero
+					printf(" 0       ");
+
+				} else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) {
+					printf("% .1e ", d);
+
+				} else {
+					printf("% 6.5f ", d);
+				}
+
+				// Matrix diagonal elements
+				if (N > 1 && M > 1 && i == j) {
+					// reset any formatting (ANSI CSI n 0)
+					printf("\033[0m");
+				}
+			}
+
+			printf("\n");
+		}
 	}
 
 	Matrix<Type, N, M> transpose() const

src/lib/matrix/test/MatrixAssignmentTest.cpp

@@ -276,14 +276,20 @@ TEST(MatrixAssignmentTest, Assignment)
 		}
 	}
 
+	char print_out[] = "  | 0      | 1      \n 0| 1.00000  1.2e+04\n 1| 1.2e+04  0.12346\n 2| 1.2e+10  1.2e+12\n";
+	printf("%s\n", print_out); // for debugging in case of failure
+
 	// check print()
 	// Redirect stdout
-	EXPECT_TRUE(freopen("testoutput.txt", "w", stdout) != NULL);
+	FILE *fp = freopen("testoutput.txt", "w", stdout);
+	EXPECT_NE(fp, nullptr);
+
 	// write
 	Comma.print();
-	fclose(stdout);
+	EXPECT_FALSE(fclose(fp)); // FIXME: this doesn't work as expected, further printf are not redirected to the console
+
 	// read
-	FILE *fp = fopen("testoutput.txt", "r");
+	fp = fopen("testoutput.txt", "r");
 	EXPECT_NE(fp, nullptr);
 	EXPECT_FALSE(fseek(fp, 0, SEEK_SET));
 
@@ -294,8 +300,8 @@ TEST(MatrixAssignmentTest, Assignment)
 			break;
 		}
 
-		printf("%d %d %d\n", static_cast<int>(i), output[i], c);
-		EXPECT_EQ(c, output[i]);
+		printf("%d %d %d\n", static_cast<int>(i), print_out[i], c);
+		EXPECT_EQ(c, print_out[i]);
 	}
 
 	EXPECT_FALSE(fclose(fp));