Merge branch 'main' into pr-fw-auto-trim

.github/workflows/mavros_mission_tests.yml

@@ -88,7 +88,7 @@ jobs:
       run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
     - name: Upload px4 coredump
       if: failure()
-      uses: actions/upload-artifact@v2-preview
+      uses: actions/upload-artifact@v4
       with:
         name: coredump
         path: px4.core
@@ -103,21 +103,21 @@ jobs:
 
     - name: Upload px4 binary
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: binary
         path: build/px4_sitl_default/bin/px4
 
     - name: Store PX4 log
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: px4_log
         path: ~/.ros/log/*/*.ulg
 
     - name: Store ROS log
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: ros_log
         path: ~/.ros/**/rostest-*.log

.github/workflows/mavros_offboard_tests.yml

@@ -83,7 +83,7 @@ jobs:
       run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
     - name: Upload px4 coredump
       if: failure()
-      uses: actions/upload-artifact@v2-preview
+      uses: actions/upload-artifact@v4
       with:
         name: coredump
         path: px4.core
@@ -98,21 +98,21 @@ jobs:
 
     - name: Upload px4 binary
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: binary
         path: build/px4_sitl_default/bin/px4
 
     - name: Store PX4 log
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: px4_log
         path: ~/.ros/log/*/*.ulg
 
     - name: Store ROS log
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: ros_log
         path: ~/.ros/**/rostest-*.log

msg/BatteryStatus.msg

@@ -46,7 +46,7 @@ uint8 BATTERY_FAULT_CELL_FAIL= 2 		# One or more cells have failed
 uint8 BATTERY_FAULT_OVER_CURRENT = 3 		# Over-current
 uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 	# Over-temperature
 uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 	# Under-temperature fault
-uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 	# Vehicle voltage is not compatible with battery one
+uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 	# Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage).
 uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 	# Battery firmware is not compatible with current autopilot firmware
 uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 	# Battery model is not supported by the system
 uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 	# hardware problem

src/lib/events/enums.json

@@ -640,7 +640,7 @@
                         },
                         "6": {
                             "name": "incompatible_voltage",
-                            "description": "Vehicle voltage is not compatible with battery one"
+                            "description": "Voltage mismatch, use equally charged batteries"
                         },
                         "7": {
                             "name": "incompatible_firmware",

src/lib/matrix/matrix/Slice.hpp

@@ -116,6 +116,39 @@ class SliceT
 		return self;
 	}
 
+	template<size_t MM, size_t NN>
+	Matrix<Type, P, Q> operator-(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
+	{
+		return Matrix<Type, P, Q> {*this} - other;
+	}
+
+
+	Matrix<Type, P, Q> operator-(const Matrix<Type, P, Q> &other)
+	{
+		return Matrix<Type, P, Q> {*this} - other;
+	}
+
+	Matrix<Type, P, Q> operator-(const Type &other)
+	{
+		return Matrix<Type, P, Q> {*this} - other;
+	}
+
+	template<size_t MM, size_t NN>
+	Matrix<Type, P, Q> operator+(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
+	{
+		return Matrix<Type, P, Q> {*this} + other;
+	}
+
+	Matrix<Type, P, Q> operator+(const Matrix<Type, P, Q> &other)
+	{
+		return Matrix<Type, P, Q> {*this} + other;
+	}
+
+	Matrix<Type, P, Q> operator+(const Type &other)
+	{
+		return Matrix<Type, P, Q> {*this} + other;
+	}
+
 	// allow assigning vectors to a slice that are in the axis
 	template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
 	SliceT<MatrixT, Type, 1, Q, M, N> &operator=(const Vector<Type, Q> &other)
@@ -222,29 +255,19 @@ class SliceT
 		return self;
 	}
 
-	SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &other)
+	SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &scalar)
 	{
-		return operator*=(Type(1) / other);
+		return operator*=(Type(1) / scalar);
 	}
 
-	Matrix<Type, P, Q> operator*(const Type &other) const
+	Matrix<Type, P, Q> operator*(const Type &scalar) const
 	{
-		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
-		Matrix<Type, P, Q> res;
-
-		for (size_t i = 0; i < P; i++) {
-			for (size_t j = 0; j < Q; j++) {
-				res(i, j) = self(i, j) * other;
-			}
-		}
-
-		return res;
+		return Matrix<Type, P, Q> {*this} * scalar;
 	}
 
-	Matrix<Type, P, Q> operator/(const Type &other) const
+	Matrix<Type, P, Q> operator/(const Type &scalar) const
 	{
-		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
-		return self * (Type(1) / other);
+		return (*this) * (1 / scalar);
 	}
 
 	template<size_t R, size_t S>

src/lib/matrix/matrix/SquareMatrix.hpp

@@ -317,6 +317,7 @@ class SquareMatrix : public Matrix<Type, M, M>
 	}
 };
 
+using SquareMatrix2f = SquareMatrix<float, 2>;
 using SquareMatrix3f = SquareMatrix<float, 3>;
 using SquareMatrix3d = SquareMatrix<double, 3>;
 

src/lib/matrix/matrix/Vector2.hpp

@@ -102,7 +102,6 @@ class Vector2 : public Vector<Type, 2>
 
 };
 
-
 using Vector2f = Vector2<float>;
 using Vector2d = Vector2<double>;
 

src/lib/matrix/test/MatrixSliceTest.cpp

@@ -262,6 +262,79 @@ TEST(MatrixSliceTest, Slice)
 	float O_check_data_12 [4] = {2.5, 3, 4, 5};
 	EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
 }
+TEST(MatrixSliceTest, SliceAdditions)
+{
+	float data[9] = {0, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	SquareMatrix3f A{data};
+
+	float operand_data [4] = {2, 1,
+				  -3, -1
+				 };
+	const SquareMatrix2f operand(operand_data);
+
+	// 2x2 Slice + 2x2 Matrix
+	SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) + operand;
+	float res_1_check_data[4] = {6, 6,
+				     4, 7
+				    };
+	EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data)));
+
+	// 2x1 Slice + 2x1 Slice
+	Vector2f res_2 = A.slice<2, 1>(1, 1) + operand.slice<2, 1>(0, 0);
+	EXPECT_EQ(res_2, Vector2f(7, 5));
+
+	// 3x3 Slice + Scalar
+	SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) + (-1);
+	float res_3_check_data[9] = {-1, 1, 2,
+				     3, 4, 5,
+				     6, 7, 9
+				    };
+	EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data)));
+
+	// 3x1 Slice + 3 Vector
+	Vector3f res_4 = A.col(1) + Vector3f(1, -2, 3);
+	EXPECT_EQ(res_4, Vector3f(3, 3, 11));
+
+}
+TEST(MatrixSliceTest, SliceSubtractions)
+{
+	float data[9] = {0, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	SquareMatrix3f A{data};
+
+	float operand_data[4] = {2, 1,
+				 -3, -1
+				};
+	const SquareMatrix2f operand(operand_data);
+
+	// 2x2 Slice - 2x2 Matrix
+	SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) - operand;
+	float res_1_check_data[4] = {2, 4,
+				     10, 9
+				    };
+	EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data)));
+
+	// 2x1 Slice - 2x1 Slice
+	Vector2f res_2 = A.slice<2, 1>(1, 1) - operand.slice<2, 1>(0, 0);
+	EXPECT_EQ(res_2, Vector2f(3, 11));
+
+	// 3x3 Slice - Scalar
+	SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) - (-1);
+	float res_3_check_data[9] = {1, 3, 4,
+				     5, 6, 7,
+				     8, 9, 11
+				    };
+	EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data)));
+
+	// 3x1 Slice - 3 Vector
+	Vector3f res_4 = A.col(1) - Vector3f(1, -2, 3);
+	EXPECT_EQ(res_4, Vector3f(1, 7, 5));
+}
 
 TEST(MatrixSliceTest, XYAssignmentTest)
 {

src/lib/matrix/test/MatrixVector3Test.cpp

@@ -80,4 +80,13 @@ TEST(MatrixVector3Test, Vector3)
 	Vector3f m2(3.1f, 4.1f, 5.1f);
 	EXPECT_EQ(m2, m1 + 2.1f);
 	EXPECT_EQ(m2 - 2.1f, m1);
+
+	// Test Addition and Subtraction of Slices
+	Vector3f v1(3, 13, 0);
+	Vector3f v2(42, 6, 256);
+
+	EXPECT_EQ(v1.xy() - v2.xy(), Vector2f(-39, 7));
+	EXPECT_EQ(v1.xy() + v2.xy(), Vector2f(45, 19));
+	EXPECT_EQ(v1.xy() + 2.f, Vector2f(5, 15));
+	EXPECT_EQ(v1.xy() - 2.f, Vector2f(1, 11));
 }

src/modules/rover_differential/RoverDifferential.hpp

@@ -61,6 +61,14 @@
 
 using namespace time_literals;
 
+// Constants
+static constexpr float STICK_DEADZONE =
+	0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
+static constexpr float YAW_RATE_THRESHOLD =
+	0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero
+static constexpr float SPEED_THRESHOLD =
+	0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
+
 class RoverDifferential : public ModuleBase<RoverDifferential>, public ModuleParams,
 	public px4::ScheduledWorkItem
 {
@@ -120,16 +128,6 @@ class RoverDifferential : public ModuleBase<RoverDifferential>, public ModulePar
 	Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode
 	Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode
 
-	// Thresholds to avoid moving at rest due to measurement noise
-	static constexpr float YAW_RATE_THRESHOLD =
-		0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero
-	static constexpr float SPEED_THRESHOLD =
-		0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
-
-	// Stick input deadzone
-	static constexpr float STICK_DEADZONE =
-		0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
-
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RD_MAN_YAW_SCALE>) _param_rd_man_yaw_scale,
 		(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,

src/modules/simulation/gz_bridge/CMakeLists.txt

@@ -88,6 +88,7 @@ if(gz-transport_FOUND)
 		windy
 		baylands
   		lawn
+		aruco
 		rover
 		aruco
 	)