Rebasing branch 9 18 24

.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

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -21,6 +21,7 @@ param set-default RD_YAW_RATE_P 0.1
 param set-default RD_YAW_RATE_I 0
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0
+param set-default RD_MAX_SPEED 5
 param set-default RD_MAX_THR_SPD 7
 param set-default RD_SPEED_P 1
 param set-default RD_SPEED_I 0

ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil

@@ -43,6 +43,10 @@ param set-default CA_SV_CS1_TRQ_P 0.3
 param set-default CA_SV_CS1_TRQ_Y -0.3
 param set-default CA_SV_CS1_TYPE 6
 
+param set-default FW_AIRSPD_MAX 12
+param set-default FW_AIRSPD_MIN 7
+param set-default FW_AIRSPD_TRIM 10
+
 param set-default HIL_ACT_FUNC1 101
 param set-default HIL_ACT_FUNC2 102
 param set-default HIL_ACT_FUNC5 202
@@ -62,6 +66,8 @@ param set-default CBRK_SUPPLY_CHK 894281
 # - without safety switch
 param set-default CBRK_IO_SAFETY 22027
 
+param set-default SENS_DPRES_OFF 0.001
+
 param set SIH_T_MAX 2.0
 param set SIH_Q_MAX 0.0165
 param set SIH_MASS 0.2
@@ -75,3 +81,5 @@ param set SIH_L_ROLL 0.145
 
 # sih as tailsitter
 param set SIH_VEHICLE_TYPE 2
+
+param set-default VT_ARSP_TRANS 6

ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover

@@ -31,6 +31,7 @@ param set-default RD_YAW_RATE_P 0.1
 param set-default RD_YAW_RATE_I 0
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0
+param set-default RD_MAX_SPEED 1.8
 param set-default RD_MAX_THR_SPD 2
 param set-default RD_SPEED_P 0.5
 param set-default RD_SPEED_I 0.1

boards/cuav/x7pro/default.px4board

@@ -39,7 +39,6 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -70,7 +69,6 @@ CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
-CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y

msg/CMakeLists.txt

@@ -73,6 +73,7 @@ set(msg_files
 	DebugVect.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
+	DistanceSensorModeChangeRequest.msg
 	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg

msg/DistanceSensorModeChangeRequest.msg

@@ -0,0 +1,5 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 request_on_off 			# request to disable/enable the distance sensor
+uint8 REQUEST_OFF = 0
+uint8 REQUEST_ON  = 1

msg/TecsStatus.msg

@@ -27,3 +27,4 @@ float32 pitch_sp_rad			# Current pitch setpoint [rad]
 float32 throttle_trim			# estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions
 
 float32 underspeed_ratio		# 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0.
+float32 fast_descend_ratio 		#  value indicating if fast descend mode is enabled with ramp up and ramp down [0-1]

src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp

@@ -55,6 +55,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/distance_sensor_mode_change_request.h>
 
 using namespace time_literals;
 
@@ -143,6 +144,8 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver<LightwareLaser>,
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN};
+	uORB::Subscription _dist_sense_mode_change_sub{ORB_ID(distance_sensor_mode_change_request)};
+	typeof(px4::msg::DistanceSensorModeChangeRequest::request_on_off) _req_mode{px4::msg::DistanceSensorModeChangeRequest::REQUEST_OFF};
 	bool _restriction{false};
 	bool _auto_restriction{false};
 	bool _prev_restriction{false};
@@ -412,6 +415,17 @@ void LightwareLaser::start()
 
 int LightwareLaser::updateRestriction()
 {
+	if (_dist_sense_mode_change_sub.updated()) {
+		distance_sensor_mode_change_request_s dist_sense_mode_change;
+
+		if (_dist_sense_mode_change_sub.copy(&dist_sense_mode_change)) {
+			_req_mode = dist_sense_mode_change.request_on_off;
+
+		} else {
+			_req_mode = distance_sensor_mode_change_request_s::REQUEST_OFF;
+		}
+	}
+
 	px4::msg::VehicleStatus vehicle_status;
 
 	if (_vehicle_status_sub.update(&vehicle_status)) {
@@ -452,7 +466,7 @@ int LightwareLaser::updateRestriction()
 		break;
 
 	case 2:
-		_restriction = _auto_restriction;
+		_restriction = _auto_restriction && _req_mode != distance_sensor_mode_change_request_s::REQUEST_ON;
 		break;
 	}
 

src/drivers/distance_sensor/lightware_laser_i2c/parameters.c

@@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
  *
  * @value 0 Disabled
  * @value 1 Enabled
- * @value 2 Disabled during VTOL fast forward flight
+ * @value 2 Enabled in VTOL MC mode, listen to request from system in FW mode
  *
  * @min 0
  * @max 2

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));
 }