preflight check: enable mode via command

ActuatorEffectivenessTiltrotorVTOL: move comment
Commander: remove unneeded function
2026-06-09 16:10:04 +08:00 · 2025-02-20 14:37:42 +01:00 · 2025-02-20 11:44:35 +01:00 · 2025-02-20 10:10:52 +01:00 · 2025-02-20 09:12:15 +01:00 · 2025-02-20 09:11:35 +01:00
48 changed files with 564 additions and 792 deletions
@@ -13,7 +13,7 @@ param set-default CA_AIRFRAME 15
 param set-default MAV_TYPE 99

 param set-default CA_THRUSTER_CNT 12
-param set-default CA_R_REV 255
+param set-default CA_R_REV 0

 # param set-default FW_ARSP_MODE 1

@@ -129,18 +129,18 @@ param set-default CA_THRUSTER11_AX 0.0
 param set-default CA_THRUSTER11_AY 0.0
 param set-default CA_THRUSTER11_AZ -1.0

-param set-default PWM_MAIN_FUNC1 501
-param set-default PWM_MAIN_FUNC2 502
-param set-default PWM_MAIN_FUNC3 503
-param set-default PWM_MAIN_FUNC4 504
-param set-default PWM_MAIN_FUNC5 505
-param set-default PWM_MAIN_FUNC6 506
-param set-default PWM_MAIN_FUNC7 507
-param set-default PWM_MAIN_FUNC8 508
-param set-default PWM_MAIN_FUNC9 509
-param set-default PWM_MAIN_FUNC10 510
-param set-default PWM_MAIN_FUNC11 511
-param set-default PWM_MAIN_FUNC12 512
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 105
+param set-default PWM_MAIN_FUNC6 106
+param set-default PWM_MAIN_FUNC7 107
+param set-default PWM_MAIN_FUNC8 108
+param set-default PWM_MAIN_FUNC9 109
+param set-default PWM_MAIN_FUNC10 110
+param set-default PWM_MAIN_FUNC11 111
+param set-default PWM_MAIN_FUNC12 112

 # PWM Simulation
 param set PWM_SIM_PWM_MAX 10000
@@ -25,12 +25,12 @@ param set-default CA_AIRFRAME 14
 param set-default MAV_TYPE 99

 param set-default CA_THRUSTER_CNT 8
-param set-default CA_R_REV 255
+param set-default CA_R_REV 0

 # param set-default FW_ARSP_MODE 1

 # Auto to be provided by Custom Airframe
-param set-default CA_METHOD 3  # 0 is PseudoInverse, 3 is Metric
+param set-default CA_METHOD 0  # 0 is PseudoInverse, 3 is Metric

 # Set proper failsafes
 param set-default COM_ACT_FAIL_ACT 0
@@ -108,14 +108,14 @@ param set-default CA_THRUSTER7_AX 0.0
 param set-default CA_THRUSTER7_AY -1.0
 param set-default CA_THRUSTER7_AZ 0.0

-param set-default SIM_GZ_TH_FUNC1 501
-param set-default SIM_GZ_TH_FUNC2 502
-param set-default SIM_GZ_TH_FUNC3 503
-param set-default SIM_GZ_TH_FUNC4 504
-param set-default SIM_GZ_TH_FUNC5 505
-param set-default SIM_GZ_TH_FUNC6 506
-param set-default SIM_GZ_TH_FUNC7 507
-param set-default SIM_GZ_TH_FUNC8 508
+param set-default SIM_GZ_TH_FUNC1 101
+param set-default SIM_GZ_TH_FUNC2 102
+param set-default SIM_GZ_TH_FUNC3 103
+param set-default SIM_GZ_TH_FUNC4 104
+param set-default SIM_GZ_TH_FUNC5 105
+param set-default SIM_GZ_TH_FUNC6 106
+param set-default SIM_GZ_TH_FUNC7 107
+param set-default SIM_GZ_TH_FUNC8 108

 param set-default SIM_GZ_TH_MIN1 0
 param set-default SIM_GZ_TH_MIN2 0
@@ -10,11 +10,11 @@

 . ${R}etc/init.d/rc.sc_defaults

-param set-default CA_AIRFRAME 13
+param set-default CA_AIRFRAME 14
 param set-default MAV_TYPE 99

 param set-default CA_THRUSTER_CNT 8
-param set-default CA_R_REV 255
+param set-default CA_R_REV 0

 # Auto to be provided by Custom Airframe
 param set-default CA_METHOD 0
@@ -31,7 +31,6 @@ param set-default COM_POSCTL_NAVL 2
 param set EKF2_EV_CTRL 15
 param set EKF2_HGT_REF 3

-
 # disable attitude failure detection
 param set-default FD_FAIL_P 0
 param set-default FD_FAIL_R 0
@@ -5,6 +5,9 @@
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #

+# Start Spacecraft App
+spacecraft start
+
 # Estimator Group Selection
 # ekf2 start &

@@ -53,6 +53,7 @@ exception_list = [
    'SYSTEMCMDS_I2C_LAUNCHER', #  undefined reference to `system',
    'MODULES_MUORB_APPS', # Weird QURT/Posix package doesn't work on x86 px4 sitl
    'MODULES_SIMULATION_SIMULATOR_SIH', # Causes compile errors
+    'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code)
 ]

 exception_list_sitl = [
@@ -73,6 +74,7 @@ exception_list_sitl = [
    'SYSTEMCMDS_I2CDETECT', # Not supported in SITL
    'SYSTEMCMDS_DMESG', # Not supported in SITL
    'SYSTEMCMDS_USB_CONNECTED', # Not supported in SITL
+    'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code)
 ]

 def main():
@@ -364,8 +364,6 @@ def get_mixers(yaml_config, output_functions, verbose):
                    actuator['group-label'] = 'Motors'
                elif actuator_conf['actuator_type'] == 'servo':
                    actuator['group-label'] = 'Servos'
-                elif actuator_conf['actuator_type'] == 'thruster':
-                    actuator['group-label'] = 'Thrusters'
                else:
                    raise Exception('Missing group label for actuator type "{}"'.format(actuator_conf['actuator_type']))

@@ -5,6 +5,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"
+CONFIG_BOARD_PARAM_FILE="/fs/microsd/params"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
@@ -74,49 +74,49 @@ print_config_settings(){
 while getopts "bcdhfmorwz" flag
 do
    case "${flag}" in
-        b) 
+        b)
            echo "[INFO] Holybro GPS selected"
            GPS=HOLYBRO
            ;;
-        c) 
+        c)
            echo "[INFO] Wiping old config file"
            if [ -f "$CONFIG_FILE" ]; then
                rm -rf ${CONFIG_FILE}
            fi
            exit 0
            ;;
-        d) 
+        d)
            echo "[INFO] Disabling daemon mode"
            DAEMON_MODE=DISABLE
            ;;
-        h) 
+        h)
            print_usage
            ;;
-        f) 
+        f)
            echo "[INFO] Setting RC to FAKE_RC_INPUT"
            RC=FAKE_RC_INPUT
            ;;
-        m) 
+        m)
            echo "[INFO] Matek GPS selected"
            GPS=MATEK
            ;;
-        o) 
+        o)
            echo "[INFO] OSD module selected"
            OSD=ENABLE
            ;;
-        r) 
+        r)
            echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected"
            RC=CRSF_MAV
            ;;
-        w) 
+        w)
            echo "[INFO] TBS Crossfire RC receiver, raw selected"
            RC=CRSF_RAW
            ;;
-        z) 
+        z)
            echo "[INFO] Fake sensor calibration values selected"
            SENSOR_CAL=FAKE
            ;;
-        *) 
+        *)
            print_usage
            ;;
    esac
@@ -130,9 +130,9 @@ else
 fi

 if [ $SENSOR_CAL == "FAKE" ]; then
-   /bin/echo "[INFO] Setting up fake sensor calibration values"
-   px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config
-   /bin/sync
+    /bin/echo "[INFO] Setting up fake sensor calibration values"
+    px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config
+    /bin/sync
 fi

 print_config_settings
@@ -35,9 +35,9 @@ fi
 /bin/sleep 1

 if [ ! -f /data/px4/param/hitl_parameters ]; then
-   echo "[INFO] Setting default parameters for PX4 on voxl"
+    echo "[INFO] Setting default parameters for PX4 on voxl"
    . /etc/modalai/voxl-px4-hitl-set-default-parameters.config
-   /bin/sync
+    /bin/sync
 else
    param select /data/px4/param/hitl_parameters
    param load
@@ -13,7 +13,7 @@ echo "OSD: $OSD"
 echo "EXTRA STEPS:"
 for i in "${EXTRA_STEPS[@]}"
 do
-	echo -e "\t$i"
+    echo -e "\t$i"
 done
 echo -e "*************************\n"

@@ -83,17 +83,17 @@ qshell ist8310 start -R 10 -X -b 1

 # GPS and magnetometer
 if [ "$GPS" != "NONE" ]; then
-	# On M0052 the GPS driver runs on the apps processor
-	if [ $PLATFORM = "M0052" ]; then
-	    gps start -d /dev/ttyHS2
-	# On M0054 and M0104 the GPS driver runs on SLPI DSP
-	else
-	    qshell gps start -d 6
-	fi
+    # On M0052 the GPS driver runs on the apps processor
+    if [ $PLATFORM = "M0052" ]; then
+        gps start -d /dev/ttyHS2
+    # On M0054 and M0104 the GPS driver runs on SLPI DSP
+    else
+        qshell gps start -d 6
+    fi
 fi

 # Auto detect an ncp5623c i2c RGB LED. If one isn't connected this will
-# fail but not cause any harm. 
+# fail but not cause any harm.
 /bin/echo "Looking for ncp5623c RGB LED"
 qshell rgbled_ncp5623c start -X -b 1 -f 400 -a 56

@@ -107,17 +107,17 @@ param touch SYS_AUTOSTART
 # ESC driver
 if [ "$ESC" == "VOXL_ESC" ]; then
    /bin/echo "Starting VOXL ESC driver"
-	qshell voxl_esc start
+    qshell voxl_esc start
 elif [ "$ESC" == "VOXL2_IO_PWM_ESC" ]; then
-	if [ "$RC" == "M0065_SBUS" ]; then
-		/bin/echo "Starting VOXL IO for PWM ESC with SBUS RC"
-		qshell voxl2_io start
-	else
-		/bin/echo "Starting VOXL IO for PWM ESC without SBUS RC"
-		qshell voxl2_io start -e
-	fi
+    if [ "$RC" == "M0065_SBUS" ]; then
+        /bin/echo "Starting VOXL IO for PWM ESC with SBUS RC"
+        qshell voxl2_io start
+    else
+        /bin/echo "Starting VOXL IO for PWM ESC without SBUS RC"
+        qshell voxl2_io start -e
+    fi
 else
-	/bin/echo "No ESC type specified, not starting an ESC driver"
+    /bin/echo "No ESC type specified, not starting an ESC driver"
 fi


@@ -133,41 +133,41 @@ elif [ "$RC" == "CRSF_MAV" ]; then
    qshell mavlink_rc_in start -m -p 7 -b 115200
 elif [ "$RC" == "SPEKTRUM" ]; then
    /bin/echo "Starting Spektrum RC"
-	# On M0052 the RC driver runs on the apps processor
-	if [ $PLATFORM = "M0052" ]; then
+    # On M0052 the RC driver runs on the apps processor
+    if [ $PLATFORM = "M0052" ]; then
        rc_input start -d /dev/ttyHS1
-	# On M0054 and M0104 the RC driver runs on SLPI DSP
-	else
+    # On M0054 and M0104 the RC driver runs on SLPI DSP
+    else
        qshell spektrum_rc start
-	fi
+    fi
 elif [ "$RC" == "GHST" ]; then
    /bin/echo "Starting GHST RC driver"
    qshell ghst_rc start -d 7
 elif [ "$RC" == "M0065_SBUS" ]; then
-	if [ $PLATFORM = "M0052" ]; then
-		apps_sbus start
-	elif [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then
-    	/bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW"
-    	qshell dsp_sbus start
-		retVal=$?
-		if [ $retVal -ne 0 ]; then
-	    	/bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed"
-	    	/bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW"
-			qshell voxl2_io start -d -p 7
-		fi
-	else
-    	/bin/echo "M0065 SBUS RC driver already started with PWM ESC start"
-	fi
+    if [ $PLATFORM = "M0052" ]; then
+        apps_sbus start
+    elif [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then
+        /bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW"
+        qshell dsp_sbus start
+        retVal=$?
+        if [ $retVal -ne 0 ]; then
+            /bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed"
+            /bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW"
+            qshell voxl2_io start -d -p 7
+        fi
+    else
+        /bin/echo "M0065 SBUS RC driver already started with PWM ESC start"
+    fi
 fi

 if [ "$DISTANCE_SENSOR" == "LIGHTWARE_SF000" ]; then
-	# Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor
-	qshell lightware_laser_serial start -d 7
+    # Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor
+    qshell lightware_laser_serial start -d 7
 fi

 if [ "$POWER_MANAGER" == "VOXLPM" ]; then
-	# APM power monitor
-	qshell voxlpm start -X -b 2
+    # APM power monitor
+    qshell voxlpm start -X -b 2
 fi

 # Optional distance sensor on spare i2c
@@ -191,7 +191,7 @@ qshell load_mon start
 # is publishing input_rc topics. Otherwise for external RC
 # over Mavlink this isn't needed.
 if [ "$RC" != "EXTERNAL" ]; then
-	qshell rc_update start
+    qshell rc_update start
 fi

 qshell commander start
@@ -214,7 +214,7 @@ voxl_save_cal_params start
 # On M0052 there is only one IMU. So, PX4 needs to
 # publish IMU samples externally for VIO to use.
 if [ $PLATFORM = "M0052" ]; then
-	imu_server start
+    imu_server start
 fi

 # start the onboard fast link to connect to voxl-mavlink-server
@@ -250,5 +250,5 @@ fi
 # Start optional EXTRA_STEPS
 for i in "${EXTRA_STEPS[@]}"
 do
-	$i
+    $i
 done
@@ -80,3 +80,4 @@ CONFIG_EXAMPLES_HELLO=y
 CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
 CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
 CONFIG_EXAMPLES_WORK_ITEM=y
+CONFIG_MODULES_SPACECRAFT=n
@@ -53,6 +53,7 @@ file(GLOB_RECURSE yaml_config_files ${PX4_SOURCE_DIR}/src/modules/*.yaml
 # avoid param duplicates
 list(FILTER yaml_config_files EXCLUDE REGEX ".*/pwm_out_sim/")
 list(FILTER yaml_config_files EXCLUDE REGEX ".*/linux_pwm_out/")
+list(FILTER yaml_config_files EXCLUDE REGEX ".*/spacecraft/")

 add_custom_target(metadata_parameters
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs
@@ -8,7 +8,6 @@ uint64 timestamp_sample	    # the timestamp the data this control response is ba
 uint16 reversible_flags     # bitset which motors are configured to be reversible

 uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
-uint16 ACTUATOR_FUNCTION_THRUSTER1 = 501

 uint8 NUM_CONTROLS = 12
 float32[12] control # range: [-1, 1], where 1 means maximum positive thrust,
@@ -49,7 +49,7 @@ uint8 NAVIGATION_STATE_DESCEND = 12             # Descend mode (no position cont
 uint8 NAVIGATION_STATE_TERMINATION = 13         # Termination mode
 uint8 NAVIGATION_STATE_OFFBOARD = 14
 uint8 NAVIGATION_STATE_STAB = 15                # Stabilized mode
-uint8 NAVIGATION_STATE_FREE1 = 16
+uint8 NAVIGATION_STATE_CS_PREFLIGHT_CHECK = 16
 uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17        # Takeoff
 uint8 NAVIGATION_STATE_AUTO_LAND = 18           # Land
 uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow
@@ -595,35 +595,37 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8

 void SF45LaserSerial::sf45_process_replies()
 {
+	const int16_t YAW_THRESHOLD  = 32000;
+	const int16_t YAW_ADJUSTMENT = 65535;
+
 	switch (rx_field.msg_id) {
 	case SF_DISTANCE_DATA_CM: {
 			const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
 			int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));

 			// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
-			if (raw_yaw > 32000) {
-				raw_yaw = raw_yaw - 65535;
+			if (raw_yaw > YAW_THRESHOLD) {
+				raw_yaw -= YAW_ADJUSTMENT;
 			}

-			// The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle
+			// Adjust yaw for downward facing sensor
 			if (_orient_cfg == ROTATION_DOWNWARD_FACING) {
-				raw_yaw = raw_yaw * -1;
+				raw_yaw = -raw_yaw;
 			}

 			// SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}"
-			float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
+			float scaled_yaw_sensor_frame = raw_yaw * SF45_SCALE_FACTOR;
+			float scaled_yaw_frd          = ObstacleMath::wrap_360(scaled_yaw_sensor_frame + _obstacle_distance.angle_offset);
+			float distance_m              = raw_distance * SF45_SCALE_FACTOR;

-			// Adjust for sensor orientation
-			scaled_yaw = sf45_wrap_360(scaled_yaw + _obstacle_distance.angle_offset);
-
-			// Convert to meters for the debug message
-			float distance_m = raw_distance * SF45_SCALE_FACTOR;
+			// Update the current bin distance
 			_current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist;

-			uint8_t current_bin = sf45_convert_angle(scaled_yaw);
+			// Find bin index for the current sensor yaw angle (in sensor frame)
+			const int current_bin = ObstacleMath::get_bin_at_angle(_obstacle_distance.increment, scaled_yaw_sensor_frame);

 			if (current_bin != _previous_bin) {
-				PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin,
+				PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw_frd, current_bin,
 					  (double)distance_m);

 				if (_vehicle_attitude_sub.updated()) {
@@ -634,9 +636,10 @@ void SF45LaserSerial::sf45_process_replies()
 					}
 				}

+				// Scale distance with vehicle rotation
 				float current_bin_dist = static_cast<float>(_current_bin_dist);
-				float scaled_yaw_rad = math::radians(static_cast<float>(scaled_yaw));
-				ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_rad, _vehicle_attitude);
+				float scaled_yaw_frd_rad = math::radians(static_cast<float>(scaled_yaw_frd));
+				ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_frd_rad, _vehicle_attitude);
 				_current_bin_dist = static_cast<uint16_t>(current_bin_dist);

 				if (_current_bin_dist > _obstacle_distance.max_distance) {
@@ -702,21 +705,6 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 	}
 }

-
-uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
-{
-	uint8_t mapped_sector = 0;
-	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
-	mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment);
-
-	return mapped_sector;
-}
-
-float SF45LaserSerial::sf45_wrap_360(float f)
-{
-	return matrix::wrap(f, 0.f, 360.f);
-}
-
 uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val)
 {
 	uint32_t i;
@@ -74,10 +74,6 @@ enum SF45_PARSED_STATE {
 };

 enum SensorOrientation {	  // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
-	ROTATION_FORWARD_FACING = 0,  // MAV_SENSOR_ROTATION_NONE
-	ROTATION_RIGHT_FACING = 2,    // MAV_SENSOR_ROTATION_YAW_90
-	ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
-	ROTATION_LEFT_FACING = 6,     // MAV_SENSOR_ROTATION_YAW_270
 	ROTATION_UPWARD_FACING = 24,  // MAV_SENSOR_ROTATION_PITCH_90
 	ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270
 };
@@ -95,8 +91,6 @@ public:
 	void				sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
 	uint16_t			sf45_format_crc(uint16_t crc, uint8_t data_value);
 	void				sf45_process_replies();
-	uint8_t				sf45_convert_angle(const int16_t yaw);
-	float				sf45_wrap_360(float f);

 private:
 	obstacle_distance_s 			_obstacle_distance{};
@@ -236,14 +236,14 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
 		// corresponding data index (convert to world frame and shift by msg offset)
 		for (int i = 0; i < BIN_COUNT; i++) {
 			for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) {
-				float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
-								  - (float)_obstacle_map_body_frame.increment / 2.f);
-				float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
-								  + (float)_obstacle_map_body_frame.increment / 2.f);
-				float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg -
-								  obstacle.increment / 2.f);
-				float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg +
-								  obstacle.increment / 2.f);
+				float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment,
+							_obstacle_map_body_frame.angle_offset);
+				float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment,
+							_obstacle_map_body_frame.angle_offset);
+				float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment,
+							obstacle.angle_offset - vehicle_orientation_deg);
+				float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment,
+							obstacle.angle_offset - vehicle_orientation_deg);

 				// if a bin stretches over the 0/360 degree line, adjust the angles
 				if (bin_lower_angle > bin_upper_angle) {
@@ -277,12 +277,12 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
 		// corresponding data index (shift by msg offset)
 		for (int i = 0; i < BIN_COUNT; i++) {
 			for (int j = 0; j < 360 / obstacle.increment; j++) {
-				float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
-								  - (float)_obstacle_map_body_frame.increment / 2.f);
-				float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
-								  + (float)_obstacle_map_body_frame.increment / 2.f);
-				float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f);
-				float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f);
+				float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment,
+							_obstacle_map_body_frame.angle_offset);
+				float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment,
+							_obstacle_map_body_frame.angle_offset);
+				float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment, obstacle.angle_offset);
+				float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment, obstacle.angle_offset);

 				// if a bin stretches over the 0/360 degree line, adjust the angles
 				if (bin_lower_angle > bin_upper_angle) {
@@ -373,7 +373,7 @@ void
 CollisionPrevention::_transformSetpoint(const Vector2f &setpoint)
 {
 	const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw;
-	const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) -
+	const float sp_angle_with_offset_deg = ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) -
 					       _obstacle_map_body_frame.angle_offset);
 	_setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
 	// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
@@ -394,7 +394,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,

 	// discard values below min range
 	if (distance_reading > distance_sensor.min_distance) {
-		float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset);
+		float sensor_yaw_body_rad = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast<ObstacleMath::SensorOrientation>
+					    (distance_sensor.orientation), distance_sensor.q);
 		float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad));

 		// calculate the field of view boundary bin indices
@@ -408,7 +409,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 		uint16_t sensor_range = static_cast<uint16_t>(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm

 		for (int bin = lower_bound; bin <= upper_bound; ++bin) {
-			int wrapped_bin = _wrap_bin(bin);
+			int wrapped_bin = ObstacleMath::wrap_bin(bin, BIN_COUNT);

 			if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) {
 				_obstacle_map_body_frame.distances[wrapped_bin] = static_cast<uint16_t>(100.0f * distance_reading + 0.5f);
@@ -435,7 +436,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 		float mean_dist = 0;

 		for (int j = i - filter_size; j <= i + filter_size; j++) {
-			int bin = _wrap_bin(j);
+			int bin = ObstacleMath::wrap_bin(j, BIN_COUNT);

 			if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
 				mean_dist += _param_cp_dist.get() * 100.f;
@@ -445,7 +446,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 			}
 		}

-		const int bin = _wrap_bin(i);
+		const int bin = ObstacleMath::wrap_bin(i, BIN_COUNT);
 		mean_dist = mean_dist / (2.f * filter_size + 1.f);
 		const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original);
 		const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin];
@@ -465,52 +466,6 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 	}
 }

-float
-CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
-{
-	float offset = math::max(math::radians(angle_offset), 0.f);
-
-	switch (distance_sensor.orientation) {
-	case distance_sensor_s::ROTATION_YAW_0:
-		offset = 0.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_45:
-		offset = M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_90:
-		offset = M_PI_F / 2.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_135:
-		offset = 3.0f * M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_180:
-		offset = M_PI_F;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_225:
-		offset = -3.0f * M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_270:
-		offset = -M_PI_F / 2.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_315:
-		offset = -M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_CUSTOM:
-		offset = Eulerf(Quatf(distance_sensor.q)).psi();
-		break;
-	}
-
-	return offset;
-}
-
 float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
 {
 	float obstacle_distance = 0.f;
@@ -520,10 +475,10 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
 		Vector2f dir = direction / direction_norm;
 		const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw;
 		const float sp_angle_with_offset_deg =
-			_wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
-		int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
-		dir_index = math::constrain(dir_index, 0, BIN_COUNT - 1);
-		obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f;
+			ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
+
+		const int dir_index = ObstacleMath::get_bin_at_angle(BIN_SIZE, sp_angle_with_offset_deg);
+		obstacle_distance   = _obstacle_map_body_frame.distances[dir_index] * 0.01f;
 	}

 	return obstacle_distance;
@@ -632,19 +587,3 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter()
 	command.timestamp = getTime();
 	_vehicle_command_pub.publish(command);
 }
-
-float CollisionPrevention::_wrap_360(const float f)
-{
-	return wrap(f, 0.f, 360.f);
-}
-
-int CollisionPrevention::_wrap_bin(int i)
-{
-	i = i % BIN_COUNT;
-
-	while (i < 0) {
-		i += BIN_COUNT;
-	}
-
-	return i;
-}
@@ -204,13 +204,6 @@ private:
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual   /**< maximum velocity in manual flight mode*/
 	)

-	/**
-	 * Transforms the sensor orientation into a yaw in the local frame
-	 * @param distance_sensor, distance sensor message
-	 * @param angle_offset, sensor body frame offset
-	 */
-	float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const;
-
 	/**
 	 * Computes collision free setpoints
 	 * @param setpoint, setpoint before collision prevention intervention
@@ -238,6 +231,4 @@ private:
 	 */
 	void _publishVehicleCmdDoLoiter();

-	static float _wrap_360(const float f);
-	static int _wrap_bin(int i);
 };
@@ -57,13 +57,19 @@ int get_bin_at_angle(float bin_width, float angle)
 	return wrap_bin(bin_at_angle, 360 / bin_width);
 }

+float get_lower_bound_angle(int bin, float bin_width, float angle_offset)
+{
+	bin = wrap_bin(bin, 360 / bin_width);
+	return wrap_360(bin * bin_width + angle_offset - bin_width / 2.f);
+}
+
 int get_offset_bin_index(int bin, float bin_width, float angle_offset)
 {
 	int offset = get_bin_at_angle(bin_width, angle_offset);
 	return wrap_bin(bin - offset, 360 / bin_width);
 }

-float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4])
 {
 	float offset = 0.0f;

@@ -99,6 +105,13 @@ float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
 	case SensorOrientation::ROTATION_YAW_315:
 		offset = -M_PI_F / 4.0f;
 		break;
+
+	case SensorOrientation::ROTATION_CUSTOM:
+		if (q != nullptr) {
+			offset = Eulerf(Quatf(q)).psi();
+		}
+
+		break;
 	}

 	return offset;
@@ -109,4 +122,9 @@ int wrap_bin(int bin, int bin_count)
 	return (bin + bin_count) % bin_count;
 }

+float wrap_360(const float angle)
+{
+	return matrix::wrap(angle, 0.0f, 360.0f);
+}
+
 } // ObstacleMath
@@ -45,6 +45,7 @@ enum SensorOrientation {
 	ROTATION_YAW_225 = 5,	  // MAV_SENSOR_ROTATION_YAW_225
 	ROTATION_YAW_270 = 6,	  // MAV_SENSOR_ROTATION_YAW_270
 	ROTATION_YAW_315 = 7,	  // MAV_SENSOR_ROTATION_YAW_315
+	ROTATION_CUSTOM  = 100,	  // MAV_SENSOR_ROTATION_CUSTOM

 	ROTATION_FORWARD_FACING  = 0, // MAV_SENSOR_ROTATION_NONE
 	ROTATION_RIGHT_FACING    = 2, // MAV_SENSOR_ROTATION_YAW_90
@@ -56,7 +57,7 @@ enum SensorOrientation {
 * Converts a sensor orientation to a yaw offset
 * @param orientation sensor orientation
 */
-float sensor_orientation_to_yaw_offset(const SensorOrientation orientation);
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4] = nullptr);

 /**
 * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
@@ -73,6 +74,14 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons
 */
 int get_bin_at_angle(float bin_width, float angle);

+/**
+ * Returns lower bound angle of a bin
+ * @param bin bin index
+ * @param bin_width width of a bin in degrees
+ * @param angle_offset clockwise angle offset in degrees
+ */
+float get_lower_bound_angle(int bin, float bin_width, float angle_offset);
+
 /**
 * Returns bin index for the current bin after an angle offset
 * @param bin current bin index
@@ -88,4 +97,11 @@ int get_offset_bin_index(int bin, float bin_width, float angle_offset);
 */
 int wrap_bin(int bin, int bin_count);

+/**
+ * Wraps an angle to the range [0, 360)
+ * @param angle angle in degrees
+ */
+float wrap_360(const float angle);
+
+
 } // ObstacleMath
@@ -133,6 +133,22 @@ TEST(ObstacleMathTest, GetBinAtAngle)
 	EXPECT_EQ(bin_index, 18);
 }

+TEST(ObstacleMathTest, GetLowerBound)
+{
+	// GIVEN: an invalid bin index, non-integer bin width, and a negative non-integer angle offset
+	int      bin          = -1;
+	float    bin_width    =  7.5f;
+	float    angle_offset = -4.3f;
+
+	// WHEN: we calculate the lower bound angle of the bin
+	float lower_bound = ObstacleMath::get_lower_bound_angle(bin, bin_width, angle_offset);
+
+	// THEN: the lower bound angle should be correct. The bin index is wrapped to the end and
+	// the angle offset is applied in the counter-clockwise direction.
+	EXPECT_FLOAT_EQ(lower_bound, 344.45);
+
+}
+

 TEST(ObstacleMathTest, OffsetBinIndex)
 {
@@ -101,3 +101,8 @@ void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_mo
 		}
 	}
 }
+
+void ActuatorEffectiveness::setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust)
+{
+	// empty implementation to be overridden if needed.
+}
@@ -219,6 +219,18 @@ public:
 	 */
 	virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);

+	/**
+	 * If overridden by derived classes, optionally bypass the info usually
+	 * contained in tiltrotor_extra_controls -- normalised collective thrust
+	 * and tilt setpoints.
+	 *
+	 * Base class implementation is empty.
+	 *
+	 * @param bypass Flag indicating whether or not to use the other
+	 * arguments to bypass setpoints
+	 */
+	virtual void setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust);
+
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
 	uint32_t _stopped_motors_mask{0};
@@ -53,7 +53,6 @@ px4_add_library(mixer_module
 	functions/FunctionMotors.hpp
 	functions/FunctionParachute.hpp
 	functions/FunctionServos.hpp
-	functions/FunctionThrusters.hpp

 	actuator_test.cpp
 	actuator_test.hpp
@@ -1,87 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include "FunctionProviderBase.hpp"
-
-/**
- * Functions: Thruster1 ... ThrusterMax
- */
-class FunctionThrusters : public FunctionProviderBase
-{
-public:
-	static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::ThrusterMax - (int)OutputFunction::Thruster1 + 1,
-		      "Unexpected num thrusters");
-
-	static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1,
-		      "Unexpected thruster idx");
-
-	FunctionThrusters(const Context &context) :
-		_topic(&context.work_item, ORB_ID(actuator_motors)),
-		_thrust_factor(context.thrust_factor)
-	{
-		for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) {
-			_data.control[i] = NAN;
-		}
-	}
-
-	static FunctionProviderBase *allocate(const Context &context) { return new FunctionThrusters(context); }
-
-	void update() override
-	{
-		if (_topic.update(&_data)) {
-			updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
-		}
-	}
-
-	float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Thruster1]; }
-
-	bool allowPrearmControl() const override { return false; }
-
-	uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
-
-	bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; }
-
-	static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
-	{
-
-	}
-
-	bool reversible(OutputFunction func) const override { return false; }
-
-private:
-	uORB::SubscriptionCallbackWorkItem _topic;
-	actuator_motors_s _data{};
-	const float &_thrust_factor;
-};
@@ -44,7 +44,6 @@
 #include "functions/FunctionLandingGearWheel.hpp"
 #include "functions/FunctionManualRC.hpp"
 #include "functions/FunctionMotors.hpp"
-#include "functions/FunctionThrusters.hpp"
 #include "functions/FunctionParachute.hpp"
 #include "functions/FunctionServos.hpp"

@@ -10,9 +10,6 @@ functions:
    Motor:
      start: 101
      count: 12
-    Thruster:
-      start: 501
-      count: 12
    Servo:
      start: 201
      count: 8
@@ -408,7 +408,6 @@ int Commander::custom_command(int argc, char *argv[])
 			} else if (!strcmp(argv[1], "auto:land")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_LAND);
-
 			} else if (!strcmp(argv[1], "auto:precland")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND);
@@ -416,7 +415,6 @@ int Commander::custom_command(int argc, char *argv[])
 			} else if (!strcmp(argv[1], "ext1")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_EXTERNAL1);
-
 			} else {
 				PX4_ERR("argument %s unsupported.", argv[1]);
 			}
@@ -844,6 +842,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
 							break;

+						case PX4_CUSTOM_SUB_MODE_AUTO_CS_PREFLIGHT_CHECK:
+							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK;
+							break;
+
 						case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8:
 							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1);
 							break;
@@ -1835,6 +1837,22 @@ void Commander::run()
 			_status_changed = true;
 		}

+		// do the control surface preflight check only if pre-armed. this means we need:
+		// COM_PREARM_MODE = 1 (Safety Button) or 2 (Always).
+		if (_actuator_armed.prearmed) {
+			if (_param_com_do_cs_check.get()) {
+				if (_vehicle_status.nav_state != vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK) {
+					_prev_nav_state = _vehicle_status.nav_state;
+					_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK);
+				}
+
+			} else {
+				if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK) {
+					_user_mode_intention.change(_prev_nav_state);
+				}
+			}
+		}
+
 		modeManagementUpdate();

 		const hrt_abstime now = hrt_absolute_time();
@@ -157,6 +157,8 @@ private:

 	unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);

+	unsigned handleCommandControlTest(const vehicle_command_s &cmd);
+
 	void executeActionRequest(const action_request_s &action_request);

 	void printRejectMode(uint8_t nav_state);
@@ -284,6 +286,8 @@ private:

 	vehicle_land_detected_s	_vehicle_land_detected{};

+	uint8_t _prev_nav_state;
+
 	// commander publications
 	actuator_armed_s        _actuator_armed{};
 	vehicle_control_mode_s  _vehicle_control_mode{};
@@ -349,6 +353,7 @@ private:
 		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_com_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_com_takeoff_act,
+		(ParamBool<px4::params::COM_DO_CS_CHECK>)   _param_com_do_cs_check,
 		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max
 	)
 };
@@ -590,66 +590,6 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
 			}
 		}
 	}
-
-	const hrt_abstime now = hrt_absolute_time();
-
-	/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
-	* for a short time interval after takeoff.
-	* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
-	* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
-	* if this does not fix the issue we need to stop using a position controlled
-	* mode to prevent flyaway crashes.
-	*/
-
-	if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-
-		if (!context.isArmed()) {
-			_nav_test_failed = false;
-			_nav_test_passed = false;
-
-		} else {
-			if (!_nav_test_passed) {
-				// Both test ratios need to pass/fail together to change the nav test status
-				const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
-							     && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
-
-				const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
-
-				if (innovation_pass) {
-					_time_last_innov_pass = now;
-
-					// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
-					const bool sufficient_time = (context.status().takeoff_time != 0) && (now > context.status().takeoff_time + 30_s);
-					const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
-
-					// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
-					if ((now > _time_last_innov_fail + 10_s) && (sufficient_time || sufficient_speed)) {
-						_nav_test_passed = true;
-						_nav_test_failed = false;
-					}
-
-				} else if (innovation_fail) {
-					_time_last_innov_fail = now;
-
-					if (now > _time_last_innov_pass + 2_s) {
-						// if the innovation test has failed continuously, declare the nav as failed
-						_nav_test_failed = true;
-						/* EVENT
-						 * @description
-						 * Land and recalibrate the sensors.
-						 */
-						reporter.healthFailure(NavModes::All, health_component_t::local_position_estimate,
-								       events::ID("check_estimator_nav_failure"),
-								       events::Log::Emergency, "Navigation failure");
-
-						if (reporter.mavlink_log_pub()) {
-							mavlink_log_critical(reporter.mavlink_log_pub(), "Navigation failure! Land and recalibrate sensors\t");
-						}
-					}
-				}
-			}
-		}
-	}
 }

 void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const
@@ -739,8 +679,8 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 	// Check if quality checking of position accuracy and consistency is to be performed
 	const float lpos_eph_threshold = (_param_com_pos_fs_eph.get() < 0) ? INFINITY : _param_com_pos_fs_eph.get();

-	bool xy_valid = lpos.xy_valid && !_nav_test_failed;
-	bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
+	bool xy_valid = lpos.xy_valid;
+	bool v_xy_valid = lpos.v_xy_valid;

 	if (!context.isArmed()) {
 		if (pre_flt_fail_innov_heading || pre_flt_fail_innov_pos_horiz) {
@@ -100,12 +100,6 @@ private:
 	hrt_abstime	_last_lpos_relaxed_fail_time_us{0};	///< Last time that the relaxed local position validity recovery check failed (usec)
 	hrt_abstime	_last_lvel_fail_time_us{0};	///< Last time that the local velocity validity recovery check failed (usec)

-	// variables used to check for navigation failure after takeoff
-	hrt_abstime	_time_last_innov_pass{0};	///< last time velocity and position innovations passed
-	hrt_abstime	_time_last_innov_fail{0};	///< last time velocity and position innovations failed
-	bool		_nav_test_passed{false};	///< true if the post takeoff navigation test has passed
-	bool		_nav_test_failed{false};	///< true if the post takeoff navigation test has failed
-
 	bool _gps_was_fused{false};
 	bool _gnss_spoofed{false};

@@ -178,6 +178,21 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 		vehicle_control_mode.flag_control_allocation_enabled = true;
 		break;

+	// enabling literally all of these makes the allocator run nicely.
+	// enabling only allocator, it will not run.
+	// surely there is like one or two which we really need and the rest can be dropped...
+	case vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK:
+		vehicle_control_mode.flag_control_auto_enabled = true;
+		vehicle_control_mode.flag_control_position_enabled = true;
+		vehicle_control_mode.flag_control_velocity_enabled = true;
+		vehicle_control_mode.flag_control_altitude_enabled = true;
+		vehicle_control_mode.flag_control_climb_rate_enabled = true;
+		vehicle_control_mode.flag_control_attitude_enabled = true;
+		vehicle_control_mode.flag_control_rates_enabled = true;
+		vehicle_control_mode.flag_control_allocation_enabled = true;
+		break;
+
+
 	// vehicle_status_s::NAVIGATION_STATE_EXTERNALx: handled in ModeManagement
 	default:
 		break;
@@ -1029,3 +1029,6 @@ PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3);
 *
 */
 PARAM_DEFINE_INT32(COM_MODE_ARM_CHK, 0);
+
+// temporary, to test control surface preflight check.
+PARAM_DEFINE_INT32(COM_DO_CS_CHECK, 0);
@@ -65,6 +65,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
 	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
 	PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF,
+	PX4_CUSTOM_SUB_MODE_AUTO_CS_PREFLIGHT_CHECK,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL1,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL2,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL3,
@@ -157,6 +158,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;
 		break;

+	case vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_CS_PREFLIGHT_CHECK;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
@@ -345,6 +345,7 @@ ControlAllocator::Run()
 		if (_vehicle_status_sub.update(&vehicle_status)) {

 			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+			_preflight_check_running = vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK;

 			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};

@@ -437,12 +438,29 @@ ControlAllocator::Run()
 			}
 		}

+		if (_preflight_check_running) {
+			preflight_check_update_state();
+			preflight_check_overwrite_torque_sp(c);
+		}
+
 		for (int i = 0; i < _num_control_allocation; ++i) {

 			_control_allocation[i]->setControlSetpoint(c[i]);

 			// Do allocation
 			_control_allocation[i]->allocate();
+
+			bool is_tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
+
+			if (_preflight_check_running && is_tiltrotor) {
+				float preflight_check_tilt_sp = preflight_check_get_tilt_control();
+				_actuator_effectiveness->setBypassTiltrotorControls(true, preflight_check_tilt_sp, 0.0f);
+
+			} else {
+				_actuator_effectiveness->setBypassTiltrotorControls(false, 0.0f, 0.0f);
+
+			}
+
 			_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
 			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
 								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
@@ -473,6 +491,74 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }

+void ControlAllocator::preflight_check_update_state()
+{
+
+	// bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
+	bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
+
+	// cycle through roll, pitch, yaw, and for each one inject positive and
+	// negative torque setpoints.
+
+	int n_axes = 3;
+
+	if (tiltrotor) {
+		n_axes = 4;
+	}
+
+	int max_phase = 2 * n_axes;
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (now - _last_preflight_check_update >= 500_ms) {
+		_preflight_check_phase++;
+		_preflight_check_phase %= max_phase;  // or quit once we did the whole thing once?
+		_last_preflight_check_update = now;
+	}
+}
+
+void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES>
+		(&c)[ActuatorEffectiveness::MAX_NUM_MATRICES])
+{
+
+	int axis = _preflight_check_phase / 2;
+	int negative = _preflight_check_phase % 2;
+
+	if (axis < 3) {
+		c[0](0) = 0.;
+		c[0](1) = 0.;
+		c[0](2) = 0.;
+		c[0](axis) = negative ? -1.f : 1.f;
+
+		if (_num_control_allocation > 1) {
+			c[1](0) = 0.;
+			c[1](1) = 0.;
+			c[1](2) = 0.;
+			c[1](axis) = negative ? -1.f : 1.f;
+		}
+
+	}
+
+}
+
+float ControlAllocator::preflight_check_get_tilt_control()
+{
+
+	int axis = _preflight_check_phase / 2;
+	int negative = _preflight_check_phase % 2;
+
+	float modified_tilt_control = 0.5f;
+
+	if (axis == 3) {
+		// axis 3 = tiltrotor.
+		// collective tilt normalised control goes from 0 to 1.
+		modified_tilt_control = negative ? 0.f : 1.f;
+	}
+
+	return modified_tilt_control;
+
+}
+
 void
 ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
 {
@@ -138,6 +138,10 @@ private:

 	void publish_actuator_controls();

+	void preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]);
+	void preflight_check_update_state();
+	float preflight_check_get_tilt_control();
+
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
 	int _num_control_allocation{0};
@@ -201,6 +205,10 @@ private:
 	// For example, the system might report two motor failures, but only the first one is handled by CA
 	uint16_t _handled_motor_failure_bitmask{0};

+	bool _preflight_check_running{false};
+	int _preflight_check_phase{0};
+	hrt_abstime _last_preflight_check_update{0};
+
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */

 	bool _armed{false};
@@ -124,88 +124,121 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt

 }

+void ActuatorEffectivenessTiltrotorVTOL::setBypassTiltrotorControls(bool bypass, float collective_tilt,
+		float collective_thrust)
+{
+	// if _bypass_tiltrotor_controls (used for control surface preflight
+	// check), we alter the behaviour of processTiltrotorControls in these
+	// two ways:
+	// - collective tilt and thrust setpoints are NOT taken from uOrb
+	//   message, but from class member variable, which we can arbitrarily set
+	//   before calling this function using setBypassTiltrotorControls
+	// - collective tilt is added to actuator_sp even if
+	//   (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT)
+	//   evaluates to false
+
+	_bypass_tiltrotor_controls = bypass;
+	_collective_tilt_normalized_setpoint = collective_tilt;
+	_collective_thrust_normalized_setpoint = collective_thrust;
+}
+
+void ActuatorEffectivenessTiltrotorVTOL::processTiltrotorControls(ActuatorVector &actuator_sp,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+
+{
+	tiltrotor_extra_controls_s tiltrotor_extra_controls;
+
+	if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || _bypass_tiltrotor_controls) {
+
+		float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f;
+		float control_collective_thrust = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
+
+		if (_bypass_tiltrotor_controls) {
+			control_collective_tilt = _collective_tilt_normalized_setpoint * 2.f - 1.f;
+			control_collective_thrust = _collective_thrust_normalized_setpoint;
+		}
+
+		// set control_collective_tilt to exactly -1 or 1 if close to these end points
+		control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
+		control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;
+
+		// initialize _last_collective_tilt_control
+		if (!PX4_ISFINITE(_last_collective_tilt_control)) {
+			_last_collective_tilt_control = control_collective_tilt;
+
+		} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
+			_collective_tilt_updated = true;
+			_last_collective_tilt_control = control_collective_tilt;
+		}
+
+		// During transition to FF, only allow update thrust axis up to 45° as with a high tilt angle the effectiveness
+		// of the thrust axis in z is apporaching 0, and by that is increasing the motor output to max.
+		// Transition to HF: disable thrust axis tilting, and assume motors are vertical. This is to avoid
+		// a thrust spike when the transition is initiated (as then the tilt is fully forward).
+		if (_flight_phase == FlightPhase::TRANSITION_HF_TO_FF) {
+			_last_collective_tilt_control = math::constrain(_last_collective_tilt_control, -1.f, 0.f);
+
+		} else if (_flight_phase == FlightPhase::TRANSITION_FF_TO_HF) {
+			_last_collective_tilt_control = -1.f;
+		}
+
+		bool yaw_saturated_positive = true;
+		bool yaw_saturated_negative = true;
+
+		for (int i = 0; i < _tilts.count(); ++i) {
+			if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
+
+				// as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover)
+				if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT || _bypass_tiltrotor_controls) {
+					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
+
+				} else {
+					actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position
+				}
+			}
+
+			// custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit
+			if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) {
+
+				if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
+					yaw_saturated_positive = false;
+				}
+
+				if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
+					yaw_saturated_negative = false;
+				}
+
+			} else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) {
+				if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
+					yaw_saturated_negative = false;
+				}
+
+				if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
+					yaw_saturated_positive = false;
+				}
+			}
+		}
+
+		_yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative;
+		_yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive;
+
+		// in FW directly use throttle sp
+		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
+			for (int i = 0; i < _first_tilt_idx; ++i) {
+				actuator_sp(i) = control_collective_thrust;
+			}
+		}
+	}
+}
+
 void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
-	// apply tilt
 	if (matrix_index == 0) {
-		tiltrotor_extra_controls_s tiltrotor_extra_controls;

-		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
-			float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f;
-
-			// set control_collective_tilt to exactly -1 or 1 if close to these end points
-			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
-			control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;
-
-			// initialize _last_collective_tilt_control
-			if (!PX4_ISFINITE(_last_collective_tilt_control)) {
-				_last_collective_tilt_control = control_collective_tilt;
-
-			} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
-				_collective_tilt_updated = true;
-				_last_collective_tilt_control = control_collective_tilt;
-			}
-
-			// During transition to FF, only allow update thrust axis up to 45° as with a high tilt angle the effectiveness
-			// of the thrust axis in z is apporaching 0, and by that is increasing the motor output to max.
-			// Transition to HF: disable thrust axis tilting, and assume motors are vertical. This is to avoid
-			// a thrust spike when the transition is initiated (as then the tilt is fully forward).
-			if (_flight_phase == FlightPhase::TRANSITION_HF_TO_FF) {
-				_last_collective_tilt_control = math::constrain(_last_collective_tilt_control, -1.f, 0.f);
-
-			} else if (_flight_phase == FlightPhase::TRANSITION_FF_TO_HF) {
-				_last_collective_tilt_control = -1.f;
-			}
-
-			bool yaw_saturated_positive = true;
-			bool yaw_saturated_negative = true;
-
-			for (int i = 0; i < _tilts.count(); ++i) {
-				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
-
-					// as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover)
-					if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) {
-						actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
-
-					} else {
-						actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position
-					}
-				}
-
-				// custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit
-				if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) {
-
-					if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
-						yaw_saturated_positive = false;
-					}
-
-					if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
-						yaw_saturated_negative = false;
-					}
-
-				} else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) {
-					if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
-						yaw_saturated_negative = false;
-					}
-
-					if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
-						yaw_saturated_positive = false;
-					}
-				}
-			}
-
-			_yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative;
-			_yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive;
-
-			// in FW directly use throttle sp
-			if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
-				for (int i = 0; i < _first_tilt_idx; ++i) {
-					actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
-				}
-			}
-		}
+		processTiltrotorControls(actuator_sp, actuator_min, actuator_max);

 		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
 			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);
@@ -88,6 +88,13 @@ public:

 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;

+	void setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust) override;
+
+	void processTiltrotorControls(ActuatorVector &actuator_sp,
+				      const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+				      const matrix::Vector<float, NUM_ACTUATORS> &actuator_max);
+
+
 protected:
 	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
@@ -114,6 +121,10 @@ protected:

 	uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};

+	bool _bypass_tiltrotor_controls{false};
+	float _collective_tilt_normalized_setpoint{0.5f};
+	float _collective_thrust_normalized_setpoint{0.0f};
+
 private:

 	void updateParams() override;
@@ -374,11 +374,9 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)

 	// Save current setpoints for the next FlightTask
 	trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint;
-	ekf_reset_counters_s last_reset_counters{};

 	if (isAnyTaskActive()) {
 		last_setpoint = _current_task.task->getTrajectorySetpoint();
-		last_reset_counters = _current_task.task->getResetCounters();
 	}

 	if (_initTask(new_task_index)) {
@@ -399,7 +397,6 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 		return FlightTaskError::ActivationFailed;
 	}

-	_current_task.task->setResetCounters(last_reset_counters);
 	_command_failed = false;

 	return FlightTaskError::NoError;
@@ -11,6 +11,7 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	_resetSetpoints();
 	_setDefaultConstraints();
+	initEkfResetCounters();
 	_time_stamp_activate = hrt_absolute_time();
 	_gear = empty_landing_gear_default_keep;
 	return true;
@@ -24,6 +25,16 @@ void FlightTask::reActivate()
 	activate(setpoint_preserve_vertical);
 }

+void FlightTask::initEkfResetCounters()
+{
+	_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
+	_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
+	_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
+	_reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter;
+	_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
+	_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
+}
+
 bool FlightTask::updateInitialize()
 {
 	_time_stamp_current = hrt_absolute_time();
@@ -55,15 +55,6 @@
 #include <uORB/topics/home_position.h>
 #include <lib/geo/geo.h>

-struct ekf_reset_counters_s {
-	uint8_t xy;
-	uint8_t vxy;
-	uint8_t z;
-	uint8_t vz;
-	uint8_t heading;
-	uint8_t hagl;
-};
-
 class FlightTask : public ModuleParams
 {
 public:
@@ -88,6 +79,8 @@ public:
 	 */
 	virtual void reActivate();

+	virtual void initEkfResetCounters();
+
 	/**
 	 * To be called to adopt parameters from an arrived vehicle command
 	 * @param command received command message containing the parameters
@@ -115,9 +108,6 @@ public:
 	 */
 	const trajectory_setpoint_s getTrajectorySetpoint();

-	const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
-	void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
-
 	/**
 	 * Get vehicle constraints.
 	 * The constraints can vary with task.
@@ -236,7 +226,14 @@ protected:
 	float _yaw_setpoint{};
 	float _yawspeed_setpoint{};

-	ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets
+	struct ekf_reset_counters_s {
+		uint8_t xy;
+		uint8_t vxy;
+		uint8_t z;
+		uint8_t vz;
+		uint8_t heading;
+		uint8_t hagl;
+	} _reset_counters{}; ///< Counters for estimator local position resets

 	/**
 	 * Vehicle constraints.
@@ -200,7 +200,7 @@ void VehicleAirData::Run()
 					}

 					if (estimator_status_flags_updated && _selected_sensor_sub_index >= 0 && _selected_sensor_sub_index == uorb_index
-					    && estimator_status_flags.cs_baro_fault) {
+					    && estimator_status_flags.cs_baro_fault && !_last_status_baro_fault) {
 						_priority[uorb_index] = 1; // 1 is min priority while still being enabled
 					}

@@ -225,6 +225,10 @@ void VehicleAirData::Run()
 		}
 	}

+	if (estimator_status_flags_updated) {
+		_last_status_baro_fault = estimator_status_flags.cs_baro_fault;
+	}
+
 	// check for the current best sensor
 	int best_index = 0;
 	_voter.get_best(time_now_us, &best_index);
@@ -126,6 +126,8 @@ private:

 	float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature

+	bool _last_status_baro_fault{false};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
 		(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate
@@ -1,5 +1,5 @@
 menuconfig MODULES_SPACECRAFT
-	bool "spacecraft"
+	bool "SPACECRAFT"
 	default n
 	---help---
 		Enable support for spacecraft
@@ -9,4 +9,4 @@ menuconfig USER_SPACECRAFT
 	default y
 	depends on BOARD_PROTECTED && MODULES_SPACECRAFT
 	---help---
-		Put spacecraft in userspace memory
+		Put SPACECRAFT in userspace memory
@@ -41,261 +41,9 @@

 #include "SpacecraftHandler.hpp"

-
-SpacecraftHandler::SpacecraftHandler() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
-{
-	updateParams();
-	_rover_differential_setpoint_pub.advertise();
-}
-
-bool SpacecraftHandler::init()
-{
-	ScheduleOnInterval(10_ms); // 100 Hz
-	return true;
-}
-
-void SpacecraftHandler::updateParams()
-{
-	ModuleParams::updateParams();
-	_max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
-}
-
-void SpacecraftHandler::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-		return;
-	}
-
-	updateSubscriptions();
-
-	// Generate and publish attitude, rate and speed setpoints
-	hrt_abstime timestamp = hrt_absolute_time();
-
-	switch (_nav_state) {
-	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
-			manual_control_setpoint_s manual_control_setpoint{};
-
-			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				_thrust_setpoint(0) = math::constrain((manual_control_setpoint.pitch * _manual_force_max), -1.f, 1.f);
-				_thrust_setpoint(1) = math::constrain((manual_control_setpoint.roll * _manual_force_max), -1.f, 1.f);
-				_thrust_setpoint(2) = 0.0;
-
-				_torque_setpoint(0) = _torque_setpoint(1) = 0.0;
-				_torque_setpoint(2) = math::constrain((manual_control_setpoint.yaw * _manual_torque_max), -1.f, 1.f);
-				// publish thrust and torque setpoints
-				vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
-				vehicle_torque_setpoint_s vehicle_torque_setpoint{};
-
-				_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
-				_torque_setpoint.copyTo(vehicle_torque_setpoint.xyz);
-
-				vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
-				vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
-
-				vehicle_torque_setpoint.timestamp = hrt_absolute_time();
-				vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
-
-				_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
-				_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
-
-				updateActuatorControlsStatus(vehicle_torque_setpoint, dt);
-			}
-		} break;
-
-	case vehicle_status_s::NAVIGATION_STATE_ACRO: {
-			manual_control_setpoint_s manual_control_setpoint{};
-
-			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				rover_differential_setpoint_s rover_differential_setpoint{};
-				rover_differential_setpoint.timestamp = timestamp;
-				rover_differential_setpoint.forward_speed_setpoint = NAN;
-				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
-				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
-						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
-				rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
-				rover_differential_setpoint.yaw_setpoint = NAN;
-				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
-			}
-
-		} break;
-
-	case vehicle_status_s::NAVIGATION_STATE_STAB: {
-			manual_control_setpoint_s manual_control_setpoint{};
-
-			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				rover_differential_setpoint_s rover_differential_setpoint{};
-				rover_differential_setpoint.timestamp = timestamp;
-				rover_differential_setpoint.forward_speed_setpoint = NAN;
-				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
-				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
-						STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
-				rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
-				rover_differential_setpoint.yaw_setpoint = NAN;
-
-				if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
-				    || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control
-					_yaw_ctl = false;
-
-
-				} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
-					if (!_yaw_ctl) {
-						_stab_desired_yaw = _vehicle_yaw;
-						_yaw_ctl = true;
-					}
-
-					rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw;
-					rover_differential_setpoint.yaw_rate_setpoint = NAN;
-
-				}
-
-				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
-			}
-
-		} break;
-
-	case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
-			manual_control_setpoint_s manual_control_setpoint{};
-
-			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				rover_differential_setpoint_s rover_differential_setpoint{};
-				rover_differential_setpoint.timestamp = timestamp;
-				rover_differential_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-						-1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get());
-				rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
-				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
-						STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
-				rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
-				rover_differential_setpoint.yaw_setpoint = NAN;
-
-				if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
-				    || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
-					_yaw_ctl = false;
-
-
-				} else { // Course control if the yaw rate input is zero (keep driving on a straight line)
-					if (!_yaw_ctl) {
-						_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
-						_pos_ctl_start_position_ned = _curr_pos_ned;
-						_yaw_ctl = true;
-					}
-
-					// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
-					const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
-										2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
-					const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(
-							rover_differential_setpoint.forward_speed_setpoint) *
-									     vector_scaling * _pos_ctl_course_direction;
-					// Calculate yaw setpoint
-					const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned,
-								   _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed));
-					rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ?
-							yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards
-					rover_differential_setpoint.yaw_rate_setpoint = NAN;
-
-				}
-
-				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
-			}
-
-		} break;
-
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-		_rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state);
-		break;
-
-	default: // Unimplemented nav states will stop the rover
-		_rover_differential_control.resetControllers();
-		_yaw_ctl = false;
-		rover_differential_setpoint_s rover_differential_setpoint{};
-		rover_differential_setpoint.forward_speed_setpoint = NAN;
-		rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f;
-		rover_differential_setpoint.yaw_rate_setpoint = NAN;
-		rover_differential_setpoint.speed_diff_setpoint_normalized = 0.f;
-		rover_differential_setpoint.yaw_setpoint = NAN;
-		_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
-		break;
-	}
-
-	if (!_armed) { // Reset when disarmed
-		_rover_differential_control.resetControllers();
-		_yaw_ctl = false;
-	}
-
-	_rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed);
-
-}
-
-void SpacecraftHandler::updateSubscriptions()
-{
-
-	if (_parameter_update_sub.updated()) {
-		parameter_update_s parameter_update;
-		_parameter_update_sub.copy(&parameter_update);
-		updateParams();
-	}
-
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status{};
-		_vehicle_status_sub.copy(&vehicle_status);
-
-		if (vehicle_status.nav_state != _nav_state) { // Reset on mode change
-			_rover_differential_control.resetControllers();
-			_yaw_ctl = false;
-		}
-
-		_nav_state = vehicle_status.nav_state;
-		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
-	}
-
-	if (_vehicle_angular_velocity_sub.updated()) {
-		vehicle_angular_velocity_s vehicle_angular_velocity{};
-		_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
-		_vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f;
-	}
-
-	if (_vehicle_attitude_sub.updated()) {
-		vehicle_attitude_s vehicle_attitude{};
-		_vehicle_attitude_sub.copy(&vehicle_attitude);
-		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
-		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
-	}
-
-	if (_vehicle_local_position_sub.updated()) {
-		vehicle_local_position_s vehicle_local_position{};
-		_vehicle_local_position_sub.copy(&vehicle_local_position);
-		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
-		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-		_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
-	}
-}
-
 int SpacecraftHandler::task_spawn(int argc, char *argv[])
 {
-	SpacecraftHandler *instance = new SpacecraftHandler();
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
+	return 0;
 }

 int SpacecraftHandler::print_status()
@@ -325,7 +73,7 @@ int SpacecraftHandler::print_usage(const char *reason)
 	)DESCR_STR"
 	);

-	PRINT_MODULE_USAGE_NAME("sc_control_allocator", "controller");
+	PRINT_MODULE_USAGE_NAME("spacecraft", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

@@ -32,7 +32,7 @@
 ****************************************************************************/

 /**
- * @file SpacecraftHandler.hpp
+ * @file ControlAllocator.hpp
 *
 * Control allocator.
 *
@@ -62,20 +62,13 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/failure_detector_status.h>

-// 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 SpacecraftHandler : public ModuleBase<SpacecraftHandler>, public ModuleParams, public px4::ScheduledWorkItem
 {
-
 public:
+
 	SpacecraftHandler();
-	~SpacecraftHandler() override = default;
+
+	virtual ~SpacecraftHandler();

 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -86,50 +79,9 @@ public:
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);

-	bool init();
-
-protected:
-	void updateParams() override;
-
-private:
-	void Run() override;
-
-	/**
-	 * @brief Update uORB subscriptions.
-	 */
-	void updateSubscriptions();
-
-	void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt);
-
-	// uORB Subscriptions
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-
-	// uORB Publications
-	uORB::Publication<rover_differential_setpoint_s> _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)};
-
-	// Instances
-	SpacecraftHandlerGuidance _rover_differential_guidance{this};
-	SpacecraftHandlerControl _rover_differential_control{this};
-	PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
-
-	// Variables
-	Vector2f _curr_pos_ned{};
-	matrix::Quatf _vehicle_attitude_quaternion{};
-	float _vehicle_yaw_rate{0.f};
-	float _vehicle_forward_speed{0.f};
-	float _vehicle_yaw{0.f};
-	float _max_yaw_rate{0.f};
-	int _nav_state{0};
-	bool _armed{false};
-	bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode
-	float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode
-	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
+	/** @see ModuleBase::print_status() */
+	int print_status() override;

+private:		/**< loop duration performance counter */

 };
@@ -8,8 +8,35 @@ module_name: Control Allocation
 parameters:
    - group: Geometry
      definitions:
+        CA_AIRFRAME:
+            description:
+                short: Airframe selection
+                long: |
+                  Defines which mixer implementation to use.
+                  Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators.

-        SC_CA_METHOD:
+                  'Custom' should only be used if noting else can be used.
+            type: enum
+            values:
+                0: Multirotor
+                1: Fixed-wing
+                2: Standard VTOL
+                3: Tiltrotor VTOL
+                4: Tailsitter VTOL
+                5: Rover (Ackermann)
+                6: Rover (Differential)
+                7: Motors (6DOF)
+                8: Multirotor with Tilt
+                9: Custom
+                10: Helicopter (tail ESC)
+                11: Helicopter (tail Servo)
+                12: Helicopter (Coaxial)
+                13: Rover (Mecanum)
+                14: Spacecraft 2D
+                15: Spacecraft 3D
+            default: 14
+
+        CA_METHOD:
            description:
                short: Control allocation method
                long: |
@@ -22,6 +49,27 @@ parameters:
                2: Automatic
            default: 1

+        CA_R_REV:
+            description:
+                short: Bidirectional/Reversible motors
+                long: |
+                  Configure motors to be bidirectional/reversible. Note that the output driver needs to support this as well.
+            type: bitmask
+            bit:
+                0: Motor 1
+                1: Motor 2
+                2: Motor 3
+                3: Motor 4
+                4: Motor 5
+                5: Motor 6
+                6: Motor 7
+                7: Motor 8
+                8: Motor 9
+                9: Motor 10
+                10: Motor 11
+                11: Motor 12
+            default: 0
+
        # (SC) Thrusters
        CA_THRUSTER_CNT:
            description:
@@ -127,8 +175,8 @@ parameters:
 # Mixer
 mixer:
    actuator_types:
-        thruster:
-            functions: 'Thruster'
+        motor:
+            functions: 'Motor'
            actuator_testing_values:
                min: 0
                max: 1
@@ -145,7 +193,7 @@ mixer:
            14: # Spacecraft 2D
                title: 'Spacecraft 2D'
                actuators:
-                  - actuator_type: 'thruster'
+                  - actuator_type: 'motor'
                    count: 'CA_THRUSTER_CNT'  # count would be too long for 16 max size
                    per_item_parameters:
                        standard:
@@ -167,7 +215,7 @@ mixer:
            15: # Sapcecraft 3D
                title: 'Spacecraft 3D'
                actuators:
-                  - actuator_type: 'thruster'
+                  - actuator_type: 'motor'
                    count: 'CA_THRUSTER_CNT'
                    per_item_parameters:
                        standard:
@@ -74,6 +74,9 @@ publications:
  - topic: /fmu/out/vehicle_trajectory_waypoint_desired
    type: px4_msgs::msg::VehicleTrajectoryWaypoint

+  - topic: /fmu/out/airspeed_validated
+    type: px4_msgs::msg::AirspeedValidated
+
 # Create uORB::Publication
 subscriptions:
  - topic: /fmu/in/register_ext_component_request
Author	SHA1	Message	Date
Balduin	93c0d19712	preflight check: enable mode via command	2025-02-20 14:37:42 +01:00
Balduin	276a7a2ab2	ActuatorEffectivenessTiltrotorVTOL: move comment	2025-02-20 11:44:35 +01:00
Balduin	6613272a70	Commander: remove unneeded function The preflight check can be enabled by switching to nav_state vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK using VEHICLE_CMD_SET_NAV_STATE. Thus we can ditch this function and forgo adding an extra vehicle command.	2025-02-20 10:10:52 +01:00
Balduin	6e6df2e47e	preflight check: format	2025-02-20 09:12:15 +01:00
Balduin	9654ac6b20	ActuatorEffectivenessTiltrotorVTOL: refactor put everything related to tiltrotor extra controls into own function to clean up updateSetpoint	2025-02-20 09:11:35 +01:00
Balduin	009c300216	preflight check: replace argument with setter this allows us to have only the setter in the base class (ActuatorEffectiveness) with an empty implementation. Derived classes can implement another implementation, or stay completely unchanged w.r.t. previously. This may finally be the desired clean-ish OOP-ish solution to this.	2025-02-20 08:50:50 +01:00
Balduin	10bcc52f13	preflight check: only change nav_mode once	2025-02-19 16:30:01 +01:00
Balduin	dea58db757	preflight check: only conduct if pre-armed	2025-02-19 16:30:01 +01:00
Balduin	6e5f3a6099	preflight check: only conduct if not armed	2025-02-18 16:59:08 +01:00
Balduin	c28d015d9c	preflight check: replace uOrb msg with argument Previously, the approach to modify collective tilt control was to send the corresponding tiltrotor_extra_control uOrb message from ControlAllocator, which then influences ActuatorEffectivenessTiltrotorVTOL with minimal changes. This was a bit hacky and introduced potentially conflicting uOrb messages. So, with this new approach we pass the same information via argument. Specifically, the class ActuatorEffectiveness now declares updateSetpoint with an extra argument, preflight_check_running. It is only used in ActuatorEffectivenessTiltrotorVTOL, but has to be included as a "dummy" in all other classes inheriting from ActuatorEffectiveness. The argument can be used to bypass the collective tilt/thrust setpoints, instead replacing them with values from public class member variables which can be set from outside just before calling updateSetpoints. Also, slight refactor in ControlAllocator by splitting up the functions related to the preflight check into smaller parts	2025-02-18 15:57:56 +01:00
Balduin	96fc4a0673	preflight check: modify actuator_sp if running this adds a new flag to ActuatorEffectivenessTiltrotorVTOL: _preflight_check_running. We set it from ControlAllocator, and if true we always add collective tilt to the actuator setpoint.	2025-02-18 13:03:45 +01:00
Balduin	122643af25	preflight check: fix range	2025-02-18 13:03:38 +01:00
Balduin	71afdb5680	preflight check: restore old nav_state only once previously it would always change _user_mode_intention to _prev_nav_state forever. now we change it only if we are in preflight check mode to not interfere with anything else later.	2025-02-18 11:08:12 +01:00
Balduin	0253f9798e	preflight check: send tilt control always even if we are not currently testing the tilt axis. then we send zero tilt rather than nothing.	2025-02-18 11:08:06 +01:00
Balduin	ad42a5b2f9	preflight check: more self explanatory code	2025-02-17 17:46:39 +01:00
Balduin	761810884e	preflight check: always send tiltrotor controls	2025-02-17 17:24:21 +01:00
Balduin	2a779663f6	preflight check: tilt collective actuation we "intercept" the tiltrotor_extra_controls message, which is read again right after in ActuatorEffectivenessTiltrotorVTOL::updateSetpoint.	2025-02-17 11:40:41 +01:00
Balduin	7816fd14b7	preflight check: restore prev nav state	2025-02-14 13:37:31 +01:00
Balduin	64d18f0a64	preflight check: first working version modifies torque setpoints directly via the matrix c.	2025-02-14 12:17:10 +01:00
Balduin	a3cca099a8	preflight check: set vehicle_control_mode flags not sure if all of these are needed, but with all of them the allocator runs, and with only flag_control_allocation_enabled it does not.	2025-02-14 10:59:42 +01:00
Balduin	01a65bfcf6	preflight check: base business logic - currently triggered via param COM_DO_CS_CHECK. ultimately this will be replaced by a mavlink cmd - new VehicleStatus.nav_state, NAVIGATION_STATE_CS_PREFLIGHT_CHECK. this is how commander tells ControlAllocator to conduct the check - (todo) within ControlAllocator we overwrite torque setpoints to do the check. additionally we can also control servos directly from there, in a different mode if that is needed.	2025-02-14 09:45:53 +01:00
Bertug Dilman	ce64263ce7	publish validated airspeed topic (#24302 ) * publish validated airspeed topic * fix typo	2025-02-07 14:44:48 +01:00
mahimayoga	69d95a6664	sf45: separate sensor yaw variable into FRD and sensor frames for clarity. Obstacle map is created in sensor frame, but scaling for vehicle orientation is done in vehicle FRD frame.	2025-02-07 13:28:17 +01:00
mahimayoga	093b379b6b	sf45/collision-prevention: replace repeated code with ObstacleMath library functions.	2025-02-07 13:28:17 +01:00
Pedro Roque	e7e76e2e21	Spacecraft build and bare control allocator (#24221 )	2025-02-06 23:54:24 -05:00
Marco Hauswirth	de1ade8eb8	sensors/vehicle_air_data: only trigger the sensor fallback when the baro_fault flag switches from 0 -> 1	2025-02-06 23:47:32 -05:00
Alexander Lerach	fd175d619c	boards/modalai/voxl2/target: remove trailing spaces, consistent tabs/spaces in files	2025-02-06 23:43:34 -05:00
bresch	8d296a50f9	FlightTask: properly initialize EKF reset counters This fixes a race condition when switching from a flight mode that is not a flight task (e.g.: stabilized). In this case, the reset counters were initialized to 0 and deltas were applied to the first setpoints if the EKF had any of its reset counters different from 0.	2025-02-05 13:21:32 -05:00
Junwoo Hwang	de650cab9e	boards/matek/h743-slim: change parameter path to MicroSD	2025-02-05 13:09:02 -05:00
Daniel Agar	bd2a009217	commander: remove MC nav test	2025-02-05 11:58:04 -05:00