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.

ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart

@@ -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

ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d

@@ -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

ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d

@@ -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

ROMFS/px4fmu_common/init.d/rc.sc_apps

@@ -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 &
 

Tools/kconfig/allyesconfig.py

@@ -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():

Tools/module_config/generate_actuators_metadata.py

@@ -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']))
 

boards/matek/h743-slim/default.px4board

@@ -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

boards/modalai/voxl2/target/voxl-px4

@@ -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

boards/modalai/voxl2/target/voxl-px4-hitl-start

@@ -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

boards/modalai/voxl2/target/voxl-px4-start

@@ -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

boards/px4/sitl/default.px4board

@@ -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

cmake/metadata.cmake

@@ -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

msg/versioned/ActuatorMotors.msg

@@ -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,

msg/versioned/VehicleStatus.msg

@@ -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

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -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;

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -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{};

src/lib/collision_prevention/CollisionPrevention.cpp

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

src/lib/collision_prevention/CollisionPrevention.hpp

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

src/lib/collision_prevention/ObstacleMath.cpp

@@ -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

src/lib/collision_prevention/ObstacleMath.hpp

@@ -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

src/lib/collision_prevention/ObstacleMathTest.cpp

@@ -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)
 {

src/lib/mixer_module/CMakeLists.txt

@@ -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

src/lib/mixer_module/functions/FunctionThrusters.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;
-};

src/lib/mixer_module/mixer_module.hpp

@@ -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"
 

src/lib/mixer_module/output_functions.yaml

@@ -10,9 +10,6 @@ functions:
     Motor:
       start: 101
       count: 12
-    Thruster:
-      start: 501
-      count: 12
     Servo:
       start: 201
       count: 8

src/modules/commander/Commander.cpp

@@ -1572,6 +1572,42 @@ void Commander::handleCommandsFromModeExecutors()
 	}
 }
 
+unsigned Commander::handleCommandControlTest(const vehicle_command_s &cmd)
+{
+
+	// TODO: trigger this some different way using params for dev purposes...?
+	// TODO: decode from command: do we want to test roll/pitch/yaw(/collective tilt), or
+	// individual control surfaces?
+	// TODO define these somewhere else like all the cool kids do
+	static const int TEST_MODE_INDIVIDUAL = 0;
+	static const int TEST_MODE_RPY = 1;
+
+	int test_mode = TEST_MODE_RPY;
+
+	if (test_mode == TEST_MODE_INDIVIDUAL) {
+		// _user_mode_intention.change()
+		// _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_SERVO_TEST);
+		PX4_INFO("Not implemented");
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
+
+	} else if (test_mode == TEST_MODE_RPY) {
+
+		// this nice pattern stolen from handle_command
+		if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
+			return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+		} else {
+			printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
+			return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+		}
+
+	} else {
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+	}
+
+	return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+}
+
 unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
 {
 	if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
@@ -1835,6 +1871,34 @@ void Commander::run()
 			_status_changed = true;
 		}
 
+		if (_param_com_do_cs_check.get()) {
+
+			// directly modify user intention here.
+			// plan is for this to ultimately to be triggered by a mavlink command
+			// through Commander::handle_command
+
+			// this nice pattern stolen from handle_command
+			// if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK, ModeChangeSource::ModeExecutor, false)) {
+			_prev_nav_state = _vehicle_status.nav_state;
+			_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK);
+
+			// no error handling like this for now
+			// if (ret) {
+				// return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				// PX4_INFO("mode intention changed");
+
+			// } else {
+				// printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
+				// return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				// PX4_INFO("mode intention not changed");
+			// }
+
+		} 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();

src/modules/commander/Commander.hpp

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

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -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) {

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

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

src/modules/commander/ModeUtil/control_mode.cpp

@@ -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;

src/modules/commander/commander_params.c

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

src/modules/control_allocator/ControlAllocator.cpp

@@ -61,6 +61,8 @@ ControlAllocator::ControlAllocator() :
 	_actuator_servos_pub.advertise();
 	_actuator_servos_trim_pub.advertise();
 
+	_tiltrotor_extra_controls_pub.advertise();
+
 	for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
 		char buffer[17];
 		snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i);
@@ -346,6 +348,8 @@ ControlAllocator::Run()
 
 			_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};
 
 			// Check if the current flight phase is HOVER or FIXED_WING
@@ -437,8 +441,19 @@ ControlAllocator::Run()
 			}
 		}
 
+		if (_preflight_check_running) {
+			preflight_check_overwrite_torque_sp(c);
+		}
+
 		for (int i = 0; i < _num_control_allocation; ++i) {
 
+			ActuatorEffectivenessTiltrotorVTOL *casted = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL *>
+					(_actuator_effectiveness);
+
+			if (casted != nullptr) {
+				casted->_preflight_check_running = _preflight_check_running;
+			}
+
 			_control_allocation[i]->setControlSetpoint(c[i]);
 
 			// Do allocation
@@ -473,6 +488,82 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }
 
+// void ControlAllocator::test_individual_control_surfaces() {
+	// goal here: modify actuation at the servo level.
+
+	// in here: small state machine cycling through servos (or taking info
+	// from outside about which servo to actuate)
+	// if test running: if enough time passed: go to next thing
+	//                  if last thing: test = not running
+
+	// elsewhere (probably Run()...)
+	// set test running if right message received
+	// if test running,
+
+// }
+
+void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]) {
+
+	// cycle through roll, pitch, yaw, and for each one inject positive and
+	// negative torque setpoints.
+
+	// is this the proper way to do it?
+	// bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
+	bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
+
+	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;
+	}
+
+	int axis = _preflight_check_phase / 2;
+	int negative = _preflight_check_phase % 2;
+
+	float modified_tilt_control = 0.5f;
+
+	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;
+		}
+
+	} else {
+		// axis 4 = tiltrotor.
+		// collective tilt normalised control goes from 0 to 1.
+		modified_tilt_control = negative ? 0.f : 1.f;
+	}
+
+	tiltrotor_extra_controls_s tiltrotor_extra_controls;
+
+	if (!_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
+		// got no message, make up thrust setpoint
+		tiltrotor_extra_controls.collective_thrust_normalized_setpoint = 0.;
+	}
+
+	tiltrotor_extra_controls.collective_tilt_normalized_setpoint = modified_tilt_control;
+	tiltrotor_extra_controls.timestamp = hrt_absolute_time();
+	_tiltrotor_extra_controls_pub.publish(tiltrotor_extra_controls);
+
+	// PX4_INFO("_torque_sp: %f, %f, %f", (double) _torque_sp(0), (double) _torque_sp(1), (double) _torque_sp(2));
+
+}
+
 void
 ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
 {

src/modules/control_allocator/ControlAllocator.hpp

@@ -138,6 +138,8 @@ private:
 
 	void publish_actuator_controls();
 
+	void preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]);
+
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
 	int _num_control_allocation{0};
@@ -179,6 +181,7 @@ private:
 
 	uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1};  /**< vehicle torque setpoint subscription (2. instance) */
 	uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1};	 /**< vehicle thrust setpoint subscription (2. instance) */
+	uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};
 
 	// Outputs
 	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
@@ -187,6 +190,8 @@ private:
 	uORB::Publication<actuator_servos_s>	_actuator_servos_pub{ORB_ID(actuator_servos)};
 	uORB::Publication<actuator_servos_trim_s>	_actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)};
 
+	uORB::Publication<tiltrotor_extra_controls_s>	_tiltrotor_extra_controls_pub{ORB_ID(tiltrotor_extra_controls)};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
@@ -201,6 +206,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};

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -166,7 +166,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 				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) {
+					if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT || _preflight_check_running) {
 						actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
 
 					} else {

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -88,6 +88,8 @@ public:
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 
+	bool _preflight_check_running{false};
+
 protected:
 	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -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;

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -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();

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -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.

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

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

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -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

src/modules/spacecraft/Kconfig

@@ -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

src/modules/spacecraft/SpacecraftHandler.cpp

@@ -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();
 

src/modules/spacecraft/SpacecraftHandler.hpp

@@ -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 */
 
 };

src/modules/spacecraft/module.yaml

@@ -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:

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -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