control_allocator: add parameter based ActuatorEffectivenessCustom

- this allows creating a custom actuator effectiveness entirely from parameters of the form CA_ACTn_TRQ_{R,P,Y} and CA_ACTn_THR_{X,Y,Z}
control_allocator: push VTOL flight phase into ActuatorEffectiveness
2026-06-03 02:50:06 +08:00 · 2021-10-27 14:25:03 -04:00 · 2021-10-27 13:42:22 -04:00 · 2021-10-27 12:48:42 -04:00 · 2021-10-27 10:07:01 -04:00 · 2021-10-27 15:23:23 +02:00
48 changed files with 678 additions and 508 deletions
@@ -10,6 +10,7 @@ on:
 jobs:
  enumerate_targets:
    runs-on: ubuntu-latest
+    container: px4io/px4-dev-base-focal:2021-09-08
    outputs:
      matrix: ${{ steps.set-matrix.outputs.matrix }}
    steps:
@@ -128,6 +128,9 @@ config BOARD_KEYSTORE

 menu "Serial ports"

+    config BOARD_SERIAL_URT6
+        string "URT6 tty port"
+
    config BOARD_SERIAL_GPS1
        string "GPS1 tty port"

@@ -158,8 +161,14 @@ menu "Serial ports"
    config BOARD_SERIAL_TEL5
        string "TEL5 tty port"

+    config BOARD_SERIAL_RC
+        string "RC tty port"
+
    config BOARD_SERIAL_WIFI
        string "WIFI tty port"
+
+    config BOARD_SERIAL_PPB
+        string "PPB (Pixhawk Payload Bus) tty port"
 endmenu

 menu "drivers"
@@ -110,10 +110,16 @@ def main(kconfig_file, config1, config2):

    for line in f:
        match = unset_match(line)
-        #pprint.pprint(match)
+        #pprint.pprint(line)
        if match is not None:
            sym_name = match.group(1)
            kconf.syms[sym_name].unset_value()
+
+            for default, cond in kconf.syms[sym_name].orig_defaults:
+                if(cond.str_value == 'y'):
+                    # Default is y, our diff is unset thus we've set it to no
+                    kconf.syms[sym_name].set_value(0)
+
    f.close()

    # Print warnings for symbols whose actual value doesn't match the assigned
@@ -5,6 +5,7 @@ CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_DSHOT=y
@@ -3,6 +3,7 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_DSHOT=y
@@ -52,6 +53,7 @@ CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
@@ -34,8 +34,8 @@
 #include <px4_arch/io_timer_hw_description.h>

 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
-	initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream1, DMA::Channel3}),
 	initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
+	initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream1, DMA::Channel3}),
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
@@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_COMMON_BAROMETERS=y
@@ -210,6 +210,9 @@ if(EXISTS ${BOARD_DEFCONFIG})

 	# Serial ports
 	set(board_serial_ports)
+	if(SERIAL_URT6)
+		list(APPEND board_serial_ports URT6:${SERIAL_URT6})
+	endif()
 	if(SERIAL_GPS1)
        list(APPEND board_serial_ports GPS1:${SERIAL_GPS1})
 	endif()
@@ -240,9 +243,15 @@ if(EXISTS ${BOARD_DEFCONFIG})
 	if(SERIAL_TEL5)
        list(APPEND board_serial_ports TEL5:${SERIAL_TEL5})
 	endif()
+	if(SERIAL_RC)
+		list(APPEND board_serial_ports RC:${SERIAL_RC})
+	endif()
 	if(SERIAL_WIFI)
        list(APPEND board_serial_ports WIFI:${SERIAL_WIFI})
 	endif()
+	if(SERIAL_PPB)
+        list(APPEND board_serial_ports PPB:${SERIAL_PPB})
+	endif()


 	# ROMFS
@@ -138,7 +138,6 @@ set(msg_files
 	sensor_combined.msg
 	sensor_correction.msg
 	sensor_gps.msg
-	sensor_gps_heading.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg
 	sensor_gyro_fifo.msg
@@ -1,10 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-uint64 timestamp_sample
-
-uint32 device_id                # unique device ID for the sensor that does not change between power cycles
-
-float32 heading
-float32 heading_accuracy                 # [1e-5 deg] Accuracy of the heading of the relative position vector
-
-float32[3] relative_position    # relative position vector
-float32[3] relative_position_accuracy
@@ -43,7 +43,6 @@
 #include <px4_platform_common/defines.h>
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/sensor_gps.h>
-#include <uORB/topics/sensor_gps_heading.h>

 #define GPS_INFO(...) PX4_INFO(__VA_ARGS__)
 #define GPS_WARN(...) PX4_WARN(__VA_ARGS__)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2019 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
@@ -63,7 +63,6 @@
 #include <uORB/topics/gps_dump.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>
-#include <uORB/topics/sensor_gps_heading.h>

 #ifndef CONSTRAINED_FLASH
 # include "devices/src/ashtech.h"
@@ -181,11 +180,7 @@ private:
 	sensor_gps_s			_report_gps_pos{};				///< uORB topic for gps position
 	satellite_info_s		*_p_report_sat_info{nullptr};			///< pointer to uORB topic for satellite info

-	sensor_gps_heading_s            _sensor_gps_heading{};
-	hrt_abstime                     _sensor_gps_heading_timestamp_last{0};
-
 	uORB::PublicationMulti<sensor_gps_s>	_report_gps_pos_pub{ORB_ID(sensor_gps)};	///< uORB pub for gps position
-	uORB::PublicationMulti<sensor_gps_heading_s> _sensor_gps_heading_pub{ORB_ID(sensor_gps_heading)};
 	uORB::PublicationMulti<satellite_info_s>	_report_sat_info_pub{ORB_ID(satellite_info)};		///< uORB pub for satellite info

 	float				_rate{0.0f};					///< position update rate
@@ -780,7 +775,7 @@ GPS::run()

 		/* FALLTHROUGH */
 		case gps_driver_mode_t::UBX:
-			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, &_sensor_gps_heading, _p_report_sat_info,
+			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
 						   gps_ubx_dynmodel, heading_offset, ubx_mode);
 			set_device_type(DRV_GPS_DEVTYPE_UBX);
 			break;
@@ -1104,15 +1099,6 @@ GPS::publish()
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
 		_report_gps_pos.heading = NAN;
 		_is_gps_main_advertised.store(true);
-
-		if (_sensor_gps_heading.timestamp_sample > _sensor_gps_heading_timestamp_last) {
-
-			_sensor_gps_heading.device_id = get_device_id();
-			_sensor_gps_heading.timestamp = hrt_absolute_time();
-			_sensor_gps_heading_pub.publish(_sensor_gps_heading);
-
-			_sensor_gps_heading_timestamp_last = _sensor_gps_heading.timestamp_sample;
-		}
 	}
 }

@@ -1205,7 +1205,7 @@ int uavcan_main(int argc, char *argv[])
 	}

 	if (!std::strcmp(argv[1], "hardpoint")) {
-		if (!std::strcmp(argv[2], "set") && argc > 4) {
+		if (argc > 4 && !std::strcmp(argv[2], "set")) {
 			const int hardpoint_id = atoi(argv[3]);
 			const int command = atoi(argv[4]);

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file NodeClient.hpp
+ *
+ * Defines basic implementation of UAVCAN PNP for dynamic Node ID
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+
+#include <px4_platform_common/defines.h>
+#include <drivers/drv_hrt.h>
+
+#include "CanardInterface.hpp"
+
+#include <uavcan/node/ID_1_0.h>
+#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
+#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
+
+#include "Services/AccessRequest.hpp"
+#include "Services/ListRequest.hpp"
+
+#define PNP1_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
+#define PNP1_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_
+#define PNP2_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
+#define PNP2_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_
+
+class NodeClient : public UavcanBaseSubscriber
+{
+public:
+	NodeClient(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0),
+		_canard_instance(ins) { };
+
+	void subscribe() override
+	{
+
+		canardRxSubscribe(&_canard_instance,
+				  CanardTransferKindMessage,
+				  (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID),  // The fixed Subject-ID
+				  (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
+				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+				  &_subj_sub._canard_sub);
+	}
+
+	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
+	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
+
+	void callback(const CanardTransfer &receive); // NodeIDAllocation callback
+
+	void update();
+
+private:
+
+	CanardInstance &_canard_instance;
+	CanardTransferID _node_id_alloc_transfer_id{0};
+
+	hrt_abstime _nodealloc_request_last{0};
+};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2021 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
@@ -276,7 +276,9 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
 	float tangent_vel = xtrack_vel_center * loiter_direction;

 	/* prevent PD output from turning the wrong way when in circle mode */
-	if (tangent_vel < 0.0f && _circle_mode) {
+	const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction
+
+	if (tangent_vel < -l1_op_tan_vel && _circle_mode) {
 		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f);
 	}

@@ -34,7 +34,6 @@
 add_custom_target(world_magnetic_model_update
 	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py > ${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp
 	DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py
-	BYPRODUCTS ${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	USES_TERMINAL
 	COMMENT "Updating world magnetic model from NOAA (${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp)"
@@ -43,7 +42,6 @@ add_custom_target(world_magnetic_model_update
 add_custom_target(world_magnetic_model_tests_update
 	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py > ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
 	DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py
-	BYPRODUCTS ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	USES_TERMINAL
 	COMMENT "Updating world magnetic model unit tests from NOAA (${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp)"
@@ -49,8 +49,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 {
 	bool success = true; // start with a pass and change to a fail if any test fails

-	int32_t mag_strength_check_enabled = 1;
-	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);
+	int32_t mag_strength_check = 1;
+	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check);

 	float hgt_test_ratio_limit = 1.f;
 	param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
@@ -107,13 +107,20 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 		goto out;
 	}

-	if ((mag_strength_check_enabled == 1) && status.pre_flt_fail_mag_field_disturbed) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: strong magnetic interference detected");
-		}
+	if ((mag_strength_check >= 1) && status.pre_flt_fail_mag_field_disturbed) {
+		const char *message = "Preflight%s: Strong magnetic interference detected";

-		success = false;
-		goto out;
+		if (mag_strength_check == 1) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, message, " Fail");
+			}
+
+			success = false;
+			goto out;
+
+		} else if (report_fail) {
+			mavlink_log_warning(mavlink_log_pub, message, "");
+		}
 	}

 	// check vertical position innovation test ratio
@@ -620,10 +620,13 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45);
 /**
 * Enable mag strength preflight check
 *
- * Deny arming if the estimator detects a strong magnetic
+ * Check if the estimator detects a strong magnetic
 * disturbance (check enabled by EKF2_MAG_CHECK)
 *
- * @boolean
+ * @value 0 Disabled
+ * @value 1 Deny arming
+ * @value 2 Warning only
+ *
 * @group Commander
 */
 PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 1);
@@ -827,6 +827,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
 		}

+		break;
+
 	default:
 		break;
 	}
@@ -44,7 +44,6 @@
 #include <ControlAllocation/ControlAllocation.hpp>

 #include <matrix/matrix/math.hpp>
-#include <uORB/topics/vehicle_actuator_setpoint.h>

 class ActuatorEffectiveness
 {
@@ -55,23 +54,6 @@ public:
 	static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
 	static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;

-	enum class FlightPhase {
-		HOVER_FLIGHT = 0,
-		FORWARD_FLIGHT = 1,
-		TRANSITION_HF_TO_FF = 2,
-		TRANSITION_FF_TO_HF = 3
-	};
-
-	/**
-	 * Set the current flight phase
-	 *
-	 * @param Flight phase
-	 */
-	virtual void setFlightPhase(const FlightPhase &flight_phase)
-	{
-		_flight_phase = flight_phase;
-	}
-
 	/**
 	 * Get the control effectiveness matrix if updated
 	 *
@@ -84,20 +66,7 @@ public:
 	 *
 	 * @return Actuator trims
 	 */
-	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
-	{
-		return _trim;
-	}
-
-	/**
-	 * Get the current flight phase
-	 *
-	 * @return Flight phase
-	 */
-	const FlightPhase &getFlightPhase() const
-	{
-		return _flight_phase;
-	}
+	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const { return _trim; }

 	/**
 	 * Get the number of actuators
@@ -106,5 +75,4 @@ public:

 protected:
 	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
-	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
 };
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+#include "ActuatorEffectivenessCustom.hpp"
+
+ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent):
+	ModuleParams(parent)
+{
+}
+
+bool ActuatorEffectivenessCustom::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
+		bool force)
+{
+	if (_updated || force) {
+		_updated = false;
+
+		int num_actuators = 0;
+
+		for (int n = 0; n < NUM_ACTUATORS; n++) {
+			// CA_ACTn_TRQ_R
+			// CA_ACTn_TRQ_P
+			// CA_ACTn_TRQ_Y
+			char torque_str[3][17];
+			sprintf(torque_str[0], "CA_ACT%u_TRQ_R", n);
+			sprintf(torque_str[1], "CA_ACT%u_TRQ_P", n);
+			sprintf(torque_str[2], "CA_ACT%u_TRQ_Y", n);
+
+			// CA_ACTn_THR_X
+			// CA_ACTn_THR_Y
+			// CA_ACTn_THR_Z
+			char thrust_str[3][17];
+			sprintf(thrust_str[0], "CA_ACT%u_THR_X", n);
+			sprintf(thrust_str[1], "CA_ACT%u_THR_Y", n);
+			sprintf(thrust_str[2], "CA_ACT%u_THR_Z", n);
+
+			for (int i = 0; i < 3; i++) {
+				// CA_ACTn_TRQ_{R,P,Y}
+				param_get(param_find(torque_str[i]), &_matrix(n, i));
+
+				// CA_ACTn_THR_{X,Y,Z}
+				param_get(param_find(thrust_str[i]), &_matrix(n, i + 3));
+			}
+
+			if (_matrix.row(n).longerThan(0.f)) {
+				num_actuators++;
+			}
+		}
+
+		_num_actuators = num_actuators;
+		matrix = _matrix;
+		return true;
+	}
+
+	return false;
+}
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ActuatorEffectivenessCustom.hpp
+ *
+ * Actuator effectiveness computed from rotors position and orientation
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+
+#include <px4_platform_common/module_params.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+
+using namespace time_literals;
+
+class ActuatorEffectivenessCustom: public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	ActuatorEffectivenessCustom(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessCustom() = default;
+
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
+
+	int numActuators() const override { return _num_actuators; }
+private:
+
+	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _matrix{};
+
+	int _num_actuators{0};
+
+	bool _updated{true};
+};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -54,78 +54,33 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NU
 		_updated = false;

 		// Get multirotor geometry
-		MultirotorGeometry geometry = {};
-		geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
-		geometry.rotors[0].position_y = _param_ca_mc_r0_py.get();
-		geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get();
-		geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get();
-		geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get();
-		geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get();
-		geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get();
-		geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get();
+		MultirotorGeometry geometry{};

-		geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
-		geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
-		geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get();
-		geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get();
-		geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get();
-		geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get();
-		geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get();
-		geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get();
+		for (int n = 0; n < NUM_ROTORS_MAX; n++) {
+			char buffer[17];

-		geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
-		geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
-		geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get();
-		geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get();
-		geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get();
-		geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get();
-		geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get();
-		geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get();
+			for (int i = 0; i < 3; i++) {
+				char axis_char = 'X' + i;

-		geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
-		geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
-		geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
-		geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get();
-		geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get();
-		geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get();
-		geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get();
-		geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get();
+				// CA_MC_Rn_P{X,Y,Z}
+				sprintf(buffer, "CA_MC_R%u_P%c", n, axis_char);
+				param_get(param_find(buffer), &geometry.rotors[n].position(i));

-		geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
-		geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
-		geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
-		geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get();
-		geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get();
-		geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get();
-		geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get();
-		geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get();
+				// CA_MC_Rn_A{X,Y,Z}
+				sprintf(buffer, "CA_MC_R%u_A%c", n, axis_char);
+				param_get(param_find(buffer), &geometry.rotors[n].axis(i));
+			}

-		geometry.rotors[5].position_x = _param_ca_mc_r5_px.get();
-		geometry.rotors[5].position_y = _param_ca_mc_r5_py.get();
-		geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get();
-		geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get();
-		geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get();
-		geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get();
-		geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get();
-		geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get();
+			geometry.rotors[n].axis.normalize();

-		geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
-		geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
-		geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
-		geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get();
-		geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get();
-		geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get();
-		geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get();
-		geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get();
+			// CA_MC_Rn_CT
+			sprintf(buffer, "CA_MC_R%u_CT", n);
+			param_get(param_find(buffer), &geometry.rotors[n].thrust_coef);

-		geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
-		geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
-		geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
-		geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get();
-		geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get();
-		geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get();
-		geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
-		geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
+			// CA_MC_Rn_KM
+			sprintf(buffer, "CA_MC_R%u_KM", n);
+			param_get(param_find(buffer), &geometry.rotors[n].moment_ratio);
+		}

 		_num_actuators = computeEffectivenessMatrix(geometry, matrix);
 		return true;
@@ -145,11 +100,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {

 		// Get rotor axis
-		matrix::Vector3f axis(
-			geometry.rotors[i].axis_x,
-			geometry.rotors[i].axis_y,
-			geometry.rotors[i].axis_z
-		);
+		matrix::Vector3f axis{geometry.rotors[i].axis};

 		// Normalize axis
 		float axis_norm = axis.norm();
@@ -163,11 +114,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		}

 		// Get rotor position
-		matrix::Vector3f position(
-			geometry.rotors[i].position_x,
-			geometry.rotors[i].position_y,
-			geometry.rotors[i].position_z
-		);
+		const matrix::Vector3f position{geometry.rotors[i].position};

 		// Get coefficients
 		float ct = geometry.rotors[i].thrust_coef;
@@ -178,10 +125,10 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		}

 		// Compute thrust generated by this rotor
-		matrix::Vector3f thrust = ct * axis;
+		const matrix::Vector3f thrust{ct * axis};

 		// Compute moment generated by this rotor
-		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
+		const matrix::Vector3f moment{ct * position.cross(axis) - ct *km * axis};

 		// Fill corresponding items in effectiveness matrix
 		for (size_t j = 0; j < 3; j++) {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -57,20 +57,16 @@ public:

 	static constexpr int NUM_ROTORS_MAX = 8;

-	typedef struct {
-		float position_x;
-		float position_y;
-		float position_z;
-		float axis_x;
-		float axis_y;
-		float axis_z;
-		float thrust_coef;
-		float moment_ratio;
-	} RotorGeometry;
+	struct RotorGeometry {
+		matrix::Vector3f position{};
+		matrix::Vector3f axis{};
+		float thrust_coef{0.f};
+		float moment_ratio{0.f};
+	};

-	typedef struct {
+	struct MultirotorGeometry {
 		RotorGeometry rotors[NUM_ROTORS_MAX];
-	} MultirotorGeometry;
+	};

 	static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
 					      matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
@@ -81,78 +77,4 @@ public:
 private:
 	bool _updated{true};
 	int _num_actuators{0};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::CA_MC_R0_PX>) _param_ca_mc_r0_px,
-		(ParamFloat<px4::params::CA_MC_R0_PY>) _param_ca_mc_r0_py,
-		(ParamFloat<px4::params::CA_MC_R0_PZ>) _param_ca_mc_r0_pz,
-		(ParamFloat<px4::params::CA_MC_R0_AX>) _param_ca_mc_r0_ax,
-		(ParamFloat<px4::params::CA_MC_R0_AY>) _param_ca_mc_r0_ay,
-		(ParamFloat<px4::params::CA_MC_R0_AZ>) _param_ca_mc_r0_az,
-		(ParamFloat<px4::params::CA_MC_R0_CT>) _param_ca_mc_r0_ct,
-		(ParamFloat<px4::params::CA_MC_R0_KM>) _param_ca_mc_r0_km,
-
-		(ParamFloat<px4::params::CA_MC_R1_PX>) _param_ca_mc_r1_px,
-		(ParamFloat<px4::params::CA_MC_R1_PY>) _param_ca_mc_r1_py,
-		(ParamFloat<px4::params::CA_MC_R1_PZ>) _param_ca_mc_r1_pz,
-		(ParamFloat<px4::params::CA_MC_R1_AX>) _param_ca_mc_r1_ax,
-		(ParamFloat<px4::params::CA_MC_R1_AY>) _param_ca_mc_r1_ay,
-		(ParamFloat<px4::params::CA_MC_R1_AZ>) _param_ca_mc_r1_az,
-		(ParamFloat<px4::params::CA_MC_R1_CT>) _param_ca_mc_r1_ct,
-		(ParamFloat<px4::params::CA_MC_R1_KM>) _param_ca_mc_r1_km,
-
-		(ParamFloat<px4::params::CA_MC_R2_PX>) _param_ca_mc_r2_px,
-		(ParamFloat<px4::params::CA_MC_R2_PY>) _param_ca_mc_r2_py,
-		(ParamFloat<px4::params::CA_MC_R2_PZ>) _param_ca_mc_r2_pz,
-		(ParamFloat<px4::params::CA_MC_R2_AX>) _param_ca_mc_r2_ax,
-		(ParamFloat<px4::params::CA_MC_R2_AY>) _param_ca_mc_r2_ay,
-		(ParamFloat<px4::params::CA_MC_R2_AZ>) _param_ca_mc_r2_az,
-		(ParamFloat<px4::params::CA_MC_R2_CT>) _param_ca_mc_r2_ct,
-		(ParamFloat<px4::params::CA_MC_R2_KM>) _param_ca_mc_r2_km,
-
-		(ParamFloat<px4::params::CA_MC_R3_PX>) _param_ca_mc_r3_px,
-		(ParamFloat<px4::params::CA_MC_R3_PY>) _param_ca_mc_r3_py,
-		(ParamFloat<px4::params::CA_MC_R3_PZ>) _param_ca_mc_r3_pz,
-		(ParamFloat<px4::params::CA_MC_R3_AX>) _param_ca_mc_r3_ax,
-		(ParamFloat<px4::params::CA_MC_R3_AY>) _param_ca_mc_r3_ay,
-		(ParamFloat<px4::params::CA_MC_R3_AZ>) _param_ca_mc_r3_az,
-		(ParamFloat<px4::params::CA_MC_R3_CT>) _param_ca_mc_r3_ct,
-		(ParamFloat<px4::params::CA_MC_R3_KM>) _param_ca_mc_r3_km,
-
-		(ParamFloat<px4::params::CA_MC_R4_PX>) _param_ca_mc_r4_px,
-		(ParamFloat<px4::params::CA_MC_R4_PY>) _param_ca_mc_r4_py,
-		(ParamFloat<px4::params::CA_MC_R4_PZ>) _param_ca_mc_r4_pz,
-		(ParamFloat<px4::params::CA_MC_R4_AX>) _param_ca_mc_r4_ax,
-		(ParamFloat<px4::params::CA_MC_R4_AY>) _param_ca_mc_r4_ay,
-		(ParamFloat<px4::params::CA_MC_R4_AZ>) _param_ca_mc_r4_az,
-		(ParamFloat<px4::params::CA_MC_R4_CT>) _param_ca_mc_r4_ct,
-		(ParamFloat<px4::params::CA_MC_R4_KM>) _param_ca_mc_r4_km,
-
-		(ParamFloat<px4::params::CA_MC_R5_PX>) _param_ca_mc_r5_px,
-		(ParamFloat<px4::params::CA_MC_R5_PY>) _param_ca_mc_r5_py,
-		(ParamFloat<px4::params::CA_MC_R5_PZ>) _param_ca_mc_r5_pz,
-		(ParamFloat<px4::params::CA_MC_R5_AX>) _param_ca_mc_r5_ax,
-		(ParamFloat<px4::params::CA_MC_R5_AY>) _param_ca_mc_r5_ay,
-		(ParamFloat<px4::params::CA_MC_R5_AZ>) _param_ca_mc_r5_az,
-		(ParamFloat<px4::params::CA_MC_R5_CT>) _param_ca_mc_r5_ct,
-		(ParamFloat<px4::params::CA_MC_R5_KM>) _param_ca_mc_r5_km,
-
-		(ParamFloat<px4::params::CA_MC_R6_PX>) _param_ca_mc_r6_px,
-		(ParamFloat<px4::params::CA_MC_R6_PY>) _param_ca_mc_r6_py,
-		(ParamFloat<px4::params::CA_MC_R6_PZ>) _param_ca_mc_r6_pz,
-		(ParamFloat<px4::params::CA_MC_R6_AX>) _param_ca_mc_r6_ax,
-		(ParamFloat<px4::params::CA_MC_R6_AY>) _param_ca_mc_r6_ay,
-		(ParamFloat<px4::params::CA_MC_R6_AZ>) _param_ca_mc_r6_az,
-		(ParamFloat<px4::params::CA_MC_R6_CT>) _param_ca_mc_r6_ct,
-		(ParamFloat<px4::params::CA_MC_R6_KM>) _param_ca_mc_r6_km,
-
-		(ParamFloat<px4::params::CA_MC_R7_PX>) _param_ca_mc_r7_px,
-		(ParamFloat<px4::params::CA_MC_R7_PY>) _param_ca_mc_r7_py,
-		(ParamFloat<px4::params::CA_MC_R7_PZ>) _param_ca_mc_r7_pz,
-		(ParamFloat<px4::params::CA_MC_R7_AX>) _param_ca_mc_r7_ax,
-		(ParamFloat<px4::params::CA_MC_R7_AY>) _param_ca_mc_r7_ay,
-		(ParamFloat<px4::params::CA_MC_R7_AZ>) _param_ca_mc_r7_az,
-		(ParamFloat<px4::params::CA_MC_R7_CT>) _param_ca_mc_r7_ct,
-		(ParamFloat<px4::params::CA_MC_R7_KM>) _param_ca_mc_r7_km
-	)
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -41,15 +41,40 @@

 #include "ActuatorEffectivenessStandardVTOL.hpp"

-ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
-{
-	setFlightPhase(FlightPhase::HOVER_FLIGHT);
-}
-
 bool
 ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
 		bool force)
 {
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+
+		FlightPhase flight_phase{FlightPhase::HOVER_FLIGHT};
+
+		// Check if the current flight phase is HOVER or FIXED_WING
+		if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			flight_phase = FlightPhase::HOVER_FLIGHT;
+
+		} else {
+			flight_phase = FlightPhase::FORWARD_FLIGHT;
+		}
+
+		// Special cases for VTOL in transition
+		if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
+			if (vehicle_status.in_transition_to_fw) {
+				flight_phase = FlightPhase::TRANSITION_HF_TO_FF;
+
+			} else {
+				flight_phase = FlightPhase::TRANSITION_FF_TO_HF;
+			}
+		}
+
+		if (flight_phase != _flight_phase) {
+			_flight_phase = flight_phase;
+			_updated = true;
+		}
+	}
+
 	if (!(_updated || force)) {
 		return false;
 	}
@@ -99,11 +124,3 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float,
 	_updated = false;
 	return true;
 }
-
-void
-ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
-{
-	ActuatorEffectiveness::setFlightPhase(flight_phase);
-	_updated = true;
-
-}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -43,22 +43,30 @@

 #include "ActuatorEffectiveness.hpp"

+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+
 class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessStandardVTOL();
+	ActuatorEffectivenessStandardVTOL() = default;
 	virtual ~ActuatorEffectivenessStandardVTOL() = default;

 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;

-	/**
-	 * Set the current flight phase
-	 *
-	 * @param Flight phase
-	 */
-	void setFlightPhase(const FlightPhase &flight_phase) override;
-
 	int numActuators() const override { return 7; }
 protected:
+
+	enum class FlightPhase {
+		HOVER_FLIGHT = 0,
+		FORWARD_FLIGHT = 1,
+		TRANSITION_HF_TO_FF = 2,
+		TRANSITION_FF_TO_HF = 3
+	};
+
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
 	bool _updated{true};
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -41,14 +41,40 @@

 #include "ActuatorEffectivenessTiltrotorVTOL.hpp"

-ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
-{
-	setFlightPhase(FlightPhase::HOVER_FLIGHT);
-}
 bool
 ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
 		bool force)
 {
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+
+		FlightPhase flight_phase{FlightPhase::HOVER_FLIGHT};
+
+		// Check if the current flight phase is HOVER or FIXED_WING
+		if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			flight_phase = FlightPhase::HOVER_FLIGHT;
+
+		} else {
+			flight_phase = FlightPhase::FORWARD_FLIGHT;
+		}
+
+		// Special cases for VTOL in transition
+		if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
+			if (vehicle_status.in_transition_to_fw) {
+				flight_phase = FlightPhase::TRANSITION_HF_TO_FF;
+
+			} else {
+				flight_phase = FlightPhase::TRANSITION_FF_TO_HF;
+			}
+		}
+
+		if (flight_phase != _flight_phase) {
+			_flight_phase = flight_phase;
+			_updated = true;
+		}
+	}
+
 	if (!(_updated || force)) {
 		return false;
 	}
@@ -106,11 +132,3 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float,
 	_updated = false;
 	return true;
 }
-
-void
-ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
-{
-	ActuatorEffectiveness::setFlightPhase(flight_phase);
-
-	_updated = true;
-}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -43,22 +43,30 @@

 #include "ActuatorEffectiveness.hpp"

+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+
 class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessTiltrotorVTOL();
+	ActuatorEffectivenessTiltrotorVTOL() = default;
 	virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;

 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;

-	/**
-	 * Set the current flight phase
-	 *
-	 * @param Flight phase
-	 */
-	void setFlightPhase(const FlightPhase &flight_phase) override;
-
 	int numActuators() const override { return 10; }
 protected:
+
+	enum class FlightPhase {
+		HOVER_FLIGHT = 0,
+		FORWARD_FLIGHT = 1,
+		TRANSITION_HF_TO_FF = 2,
+		TRANSITION_FF_TO_HF = 3
+	};
+
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
 	bool _updated{true};
 };
@@ -1,52 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2019 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.
-#
-############################################################################
-
-px4_add_library(ActuatorEffectiveness
-	ActuatorEffectiveness.hpp
-	ActuatorEffectivenessMultirotor.cpp
-	ActuatorEffectivenessMultirotor.hpp
-	ActuatorEffectivenessStandardVTOL.cpp
-	ActuatorEffectivenessStandardVTOL.hpp
-	ActuatorEffectivenessTiltrotorVTOL.cpp
-	ActuatorEffectivenessTiltrotorVTOL.hpp
-)
-
-target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-target_link_libraries(ActuatorEffectiveness
-	PRIVATE
-		mathlib
-		ControlAllocation
-)
-
-# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)
@@ -31,22 +31,45 @@
 #
 ############################################################################

-include_directories(${CMAKE_CURRENT_SOURCE_DIR})
-add_subdirectory(ActuatorEffectiveness)
-add_subdirectory(ControlAllocation)
-
 px4_add_module(
 	MODULE modules__control_allocator
 	MAIN control_allocator
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
+	INCLUDES
+		${CMAKE_CURRENT_SOURCE_DIR}
 	SRCS
+		ActuatorEffectiveness/ActuatorEffectiveness.hpp
+		ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp
+		ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp
+		ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp
+		ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp
+		ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
+		ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
+		ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+		ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
+
+		ControlAllocation/ControlAllocation.cpp
+		ControlAllocation/ControlAllocation.hpp
+		ControlAllocation/ControlAllocationPseudoInverse.cpp
+		ControlAllocation/ControlAllocationPseudoInverse.hpp
+		ControlAllocation/ControlAllocationSequentialDesaturation.cpp
+		ControlAllocation/ControlAllocationSequentialDesaturation.hpp
+
 		ControlAllocator.cpp
 		ControlAllocator.hpp
 	DEPENDS
 		mathlib
-		ActuatorEffectiveness
-		ControlAllocation
-		mixer
 		px4_work_queue
+	MODULE_CONFIG
+		module.yaml
 )
+
+px4_add_library(ControlAllocation_testing
+	ControlAllocation/ControlAllocation.cpp
+	ControlAllocation/ControlAllocationPseudoInverse.cpp
+)
+target_link_libraries(ControlAllocation_testing PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC ControlAllocation/ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation_testing)
+# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)
@@ -1,46 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2019 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.
-#
-############################################################################
-
-px4_add_library(ControlAllocation
-	ControlAllocation.cpp
-	ControlAllocation.hpp
-	ControlAllocationPseudoInverse.cpp
-	ControlAllocationPseudoInverse.hpp
-	ControlAllocationSequentialDesaturation.cpp
-	ControlAllocationSequentialDesaturation.hpp
-)
-target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-target_link_libraries(ControlAllocation PRIVATE mathlib)
-
-px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)
@@ -70,7 +70,6 @@
 #pragma once

 #include <matrix/matrix/math.hpp>
-#include <uORB/topics/vehicle_actuator_setpoint.h>

 class ControlAllocation
 {
@@ -81,7 +80,7 @@ public:
 	static constexpr uint8_t NUM_ACTUATORS = 16;
 	static constexpr uint8_t NUM_AXES = 6;

-	typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
+	using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;

 	enum ControlAxis {
 		ROLL = 0,
@@ -40,7 +40,8 @@
 */

 #include <gtest/gtest.h>
-#include <ControlAllocationPseudoInverse.hpp>
+
+#include "ControlAllocationPseudoInverse.hpp"

 using namespace matrix;

@@ -212,6 +212,10 @@ ControlAllocator::update_effectiveness_source()
 			tmp = new ActuatorEffectivenessTiltrotorVTOL();
 			break;

+		case EffectivenessSource::CUSTOM:
+			tmp = new ActuatorEffectivenessCustom(this);
+			break;
+
 		default:
 			PX4_ERR("Unknown airframe");
 			break;
@@ -260,34 +264,6 @@ ControlAllocator::Run()
 		return;
 	}

-	vehicle_status_s vehicle_status;
-
-	if (_vehicle_status_sub.update(&vehicle_status)) {
-
-		ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
-
-		// Check if the current flight phase is HOVER or FIXED_WING
-		if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT;
-
-		} else {
-			flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT;
-		}
-
-		// Special cases for VTOL in transition
-		if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
-			if (vehicle_status.in_transition_to_fw) {
-				flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF;
-
-			} else {
-				flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF;
-			}
-		}
-
-		// Forward to effectiveness source
-		_actuator_effectiveness->setFlightPhase(flight_phase);
-	}
-
 	// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
 	const hrt_abstime now = hrt_absolute_time();
 	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
@@ -512,6 +488,10 @@ int ControlAllocator::print_status()
 	case EffectivenessSource::TILTROTOR_VTOL:
 		PX4_INFO("EffectivenessSource: Tiltrotor VTOL");
 		break;
+
+	case EffectivenessSource::CUSTOM:
+		PX4_INFO("EffectivenessSource: Custom");
+		break;
 	}

 	// Print current effectiveness matrix
@@ -41,14 +41,15 @@

 #pragma once

-#include <ActuatorEffectiveness.hpp>
-#include <ActuatorEffectivenessMultirotor.hpp>
-#include <ActuatorEffectivenessStandardVTOL.hpp>
-#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectiveness.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp>

-#include <ControlAllocation.hpp>
-#include <ControlAllocationPseudoInverse.hpp>
-#include <ControlAllocationSequentialDesaturation.hpp>
+#include <ControlAllocation/ControlAllocation.hpp>
+#include <ControlAllocation/ControlAllocationPseudoInverse.hpp>
+#include <ControlAllocation/ControlAllocationSequentialDesaturation.hpp>

 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
@@ -68,7 +69,6 @@
 #include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_actuator_setpoint.h>
-#include <uORB/topics/vehicle_status.h>

 class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::WorkItem
 {
@@ -124,10 +124,11 @@ private:
 	ControlAllocation *_control_allocation{nullptr}; 	///< class for control allocation calculations

 	enum class EffectivenessSource {
-		NONE = -1,
-		MULTIROTOR = 0,
-		STANDARD_VTOL = 1,
+		NONE           = -1,
+		MULTIROTOR     = 0,
+		STANDARD_VTOL  = 1,
 		TILTROTOR_VTOL = 2,
+		CUSTOM         = 3,
 	};

 	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
@@ -147,7 +148,6 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};				/**< airspeed subscription */
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

 	matrix::Vector3f _torque_sp;
 	matrix::Vector3f _thrust_sp;
@@ -50,6 +50,7 @@
 * @value 0 Multirotor
 * @value 1 Standard VTOL (WIP)
 * @value 2 Tiltrotor VTOL (WIP)
+ * @value 3 Custom (CA_ACTn* torque and thrust parameters)
 * @group Control Allocation
 */
 PARAM_DEFINE_INT32(CA_AIRFRAME, 0);
@@ -0,0 +1,72 @@
+__max_num_config_instances: &max_num_config_instances 16
+
+module_name: control_allocator
+
+parameters:
+    - group: Control Allocator
+      definitions:
+        CA_ACT${i}_TRQ_R:
+            description:
+                short: actuator ${i} roll
+                long: Actuator ${i} roll torque
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
+
+        CA_ACT${i}_TRQ_P:
+            description:
+                short: actuator ${i} pitch
+                long: Actuator ${i} pitch torque
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
+
+        CA_ACT${i}_TRQ_Y:
+            description:
+                short: actuator ${i} yaw
+                long: Actuator ${i} yaw torque
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
+
+        CA_ACT${i}_THR_X:
+            description:
+                short: actuator ${i} thrust x
+                long: Actuator ${i} thrust x
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
+
+        CA_ACT${i}_THR_Y:
+            description:
+                short: actuator ${i} thrust y
+                long: Actuator ${i} thrust y
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
+
+        CA_ACT${i}_THR_Z:
+            description:
+                short: actuator ${i} thrust y
+                long: Actuator ${i} thrust y
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
@@ -134,7 +134,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 }

 void
-FixedwingAttitudeControl::vehicle_manual_poll()
+FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 {
 	const bool is_tailsitter_transition = _is_tailsitter && _vehicle_status.in_transition_mode;
 	const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
@@ -156,7 +156,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 					_att_sp.pitch_body = constrain(_att_sp.pitch_body,
 								       -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));

-					_att_sp.yaw_body = 0.0f;
+					_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
 					_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);

 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
@@ -357,7 +357,7 @@ void FixedwingAttitudeControl::Run()
 		_vehicle_status_sub.update(&_vehicle_status);

 		vehicle_control_mode_poll();
-		vehicle_manual_poll();
+		vehicle_manual_poll(euler_angles.psi());
 		vehicle_land_detected_poll();

 		// the position controller will not emit attitude setpoints in some modes
@@ -233,7 +233,7 @@ private:
 	int		parameters_update();

 	void		vehicle_control_mode_poll();
-	void		vehicle_manual_poll();
+	void		vehicle_manual_poll(const float yaw_body);
 	void		vehicle_attitude_setpoint_poll();
 	void		vehicle_rates_setpoint_poll();
 	void		vehicle_land_detected_poll();
@@ -686,14 +686,14 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
 		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
 			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _current_altitude;
-			_hdg_hold_yaw = _yaw;
+			_hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw
 			_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
 			_yaw_lock_engaged = false;

 			/* reset setpoints from other modes (auto) otherwise we won't
 			 * level out without new manual input */
 			_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
-			_att_sp.yaw_body = 0;
+			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 		}

 		_control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -920,7 +920,6 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 			// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER
 			if ((dist >= 0.f)
-			    && (dist_z > 2.f * _param_fw_clmbout_diff.get())
 			    && (dist_xy > 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) {
 				// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
 				position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
@@ -1048,7 +1047,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve

 	_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed));
 	_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	_att_sp.yaw_body = _l1_control.nav_bearing();
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

 	tecs_update_pitch_throttle(now, position_sp_alt,
 				   calculate_target_airspeed(mission_airspeed, ground_speed),
@@ -1152,7 +1151,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect
 	_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed));

 	_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	_att_sp.yaw_body = _l1_control.nav_bearing();
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

 	float alt_sp = pos_sp_curr.alt;

@@ -1267,7 +1266,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo

 		// assign values
 		_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
-		_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
+		_att_sp.yaw_body = _runway_takeoff.getYaw(_yaw);
 		_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
 		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());

@@ -1308,7 +1307,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo

 			_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
-			_att_sp.yaw_body = _l1_control.nav_bearing();
+			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

 			/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
 			 * full throttle, otherwise we use idle throttle */
@@ -1465,7 +1464,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 	}

 	_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	_att_sp.yaw_body = _l1_control.nav_bearing();
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

 	if (_land_noreturn_horizontal) {
 		/* limit roll motion to prevent wings from touching the ground first */
@@ -1682,7 +1681,7 @@ FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2
 				   _manual_height_rate_setpoint_m_s);

 	_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
-	_att_sp.yaw_body = 0;
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

 	/* Copy thrust and pitch values from tecs */
 	if (_landed) {
@@ -1776,7 +1775,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);

 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
-			_att_sp.yaw_body = _l1_control.nav_bearing();
+			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

 			if (in_takeoff_situation()) {
 				/* limit roll motion to ensure enough lift */
@@ -1801,7 +1800,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 		}

 		_att_sp.roll_body = roll_sp_new;
-		_att_sp.yaw_body = 0;
+		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 	}

 	/* Copy thrust and pitch values from tecs */
@@ -154,7 +154,6 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("sensor_accel", 1000, 4);
 	add_topic_multi("sensor_baro", 1000, 4);
 	add_topic_multi("sensor_gps", 1000, 2);
-	add_topic_multi("sensor_gps_heading", 1000, 1);
 	add_topic_multi("sensor_gyro", 1000, 4);
 	add_topic_multi("sensor_mag", 1000, 4);
 	add_topic_multi("vehicle_imu", 500, 4);
@@ -100,11 +100,22 @@ Loiter::set_loiter_position()

 	_loiter_pos_set = true;

-	// set current mission item to loiter
-	set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	if (_navigator->get_land_detected()->landed) {
+		_mission_item.nav_cmd = NAV_CMD_IDLE;
+
+	} else {
+		if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+			setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
+
+		} else {
+			setLoiterItemFromCurrentPosition(&_mission_item);
+		}
+
+	}

 	// convert mission item to current setpoint
-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->current.velocity_valid = false;
 	pos_sp_triplet->previous.valid = false;
 	mission_apply_limitation(_mission_item);
@@ -683,11 +683,22 @@ Mission::set_mission_items()

 		_mission_type = MISSION_TYPE_NONE;

-		/* set loiter mission item and ensure that there is a minimum clearance from home */
-		set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+		if (_navigator->get_land_detected()->landed) {
+			_mission_item.nav_cmd = NAV_CMD_IDLE;
+
+		} else {
+			if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+				setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
+
+			} else {
+				setLoiterItemFromCurrentPosition(&_mission_item);
+			}
+
+		}

 		/* update position setpoint triplet  */
-		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->previous.valid = false;
 		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
@@ -375,6 +375,9 @@ MissionBlock::is_mission_item_reached()
 			struct position_setpoint_s *curr_sp_new = &_navigator->get_position_setpoint_triplet()->current;
 			const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;

+			const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
+							next_sp.lon);
+
 			/* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set,
 			   or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */
 			const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
@@ -383,11 +386,10 @@ MissionBlock::is_mission_item_reached()
 							  curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER &&
 							  (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT);

-			if (enforce_exit_heading) {
+			// can only enforce exit heading if next waypoint is not within loiter radius of current waypoint
+			const bool exit_heading_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius;

-
-				const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
-								next_sp.lon);
+			if (enforce_exit_heading && exit_heading_is_reachable) {

 				float yaw_err = 0.0f;

@@ -439,7 +441,9 @@ MissionBlock::is_mission_item_reached()
 					bearing += inner_angle;
 				}

-				// Replace current setpoint lat/lon with tangent coordinate
+				// set typ to position, will get set to loiter in the fw position controller once close
+				// and replace current setpoint lat/lon with tangent coordinate
+				curr_sp.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 				waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon,
 								   bearing, fabsf(curr_sp.loiter_radius),
 								   &curr_sp.lat, &curr_sp.lon);
@@ -640,42 +644,40 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 }

 void
-MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
+MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item)
 {
-	if (_navigator->get_land_detected()->landed) {
-		/* landed, don't takeoff, but switch to IDLE mode */
-		item->nav_cmd = NAV_CMD_IDLE;
+	setLoiterItemCommonFields(item);

-	} else {
-		item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+	const position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

-		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	item->lat = pos_sp_triplet->current.lat;
+	item->lon = pos_sp_triplet->current.lon;
+	item->altitude = pos_sp_triplet->current.alt;
+	item->loiter_radius = pos_sp_triplet->current.loiter_radius;
+}

-		if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
-			/* use current position setpoint */
-			item->lat = pos_sp_triplet->current.lat;
-			item->lon = pos_sp_triplet->current.lon;
-			item->altitude = pos_sp_triplet->current.alt;
+void
+MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
+{
+	setLoiterItemCommonFields(item);

-		} else {
-			/* use current position and use return altitude as clearance */
-			item->lat = _navigator->get_global_position()->lat;
-			item->lon = _navigator->get_global_position()->lon;
-			item->altitude = _navigator->get_global_position()->alt;
+	item->lat = _navigator->get_global_position()->lat;
+	item->lon = _navigator->get_global_position()->lon;
+	item->altitude = _navigator->get_global_position()->alt;
+	item->loiter_radius = _navigator->get_loiter_radius();
+}

-			if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
-				item->altitude = _navigator->get_home_position()->alt + min_clearance;
-			}
-		}
+void
+MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item)
+{
+	item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;

-		item->altitude_is_relative = false;
-		item->yaw = NAN;
-		item->loiter_radius = _navigator->get_loiter_radius();
-		item->acceptance_radius = _navigator->get_acceptance_radius();
-		item->time_inside = 0.0f;
-		item->autocontinue = false;
-		item->origin = ORIGIN_ONBOARD;
-	}
+	item->altitude_is_relative = false;
+	item->acceptance_radius = _navigator->get_acceptance_radius();
+	item->yaw = NAN;
+	item->time_inside = 0.0f;
+	item->autocontinue = false;
+	item->origin = ORIGIN_ONBOARD;
 }

 void
@@ -108,10 +108,11 @@ protected:
 	 */
 	bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp);

-	/**
-	 * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
-	 */
-	void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
+	void setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item);
+
+	void setLoiterItemFromCurrentPosition(struct mission_item_s *item);
+
+	void setLoiterItemCommonFields(struct mission_item_s *item);

 	/**
 	 * Set a takeoff mission item
@@ -306,8 +306,8 @@ Navigator::run()

 					} else if (PX4_ISFINITE(cmd.param7)) {
 						// Received only a request to change altitude, thus we keep the setpoint
-						rep->current.lat = curr->current.lat;
-						rep->current.lon = curr->current.lon;
+						rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat;
+						rep->current.lon = PX4_ISFINITE(curr->current.lon) ? curr->current.lon : get_global_position()->lon;
 						rep->current.alt = cmd.param7;
 						only_alt_change_requested = true;

@@ -343,16 +343,23 @@ Navigator::run()
 						}
 					}

-					if (only_alt_change_requested && PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > 0) {
-						rep->current.loiter_radius = curr->current.loiter_radius;
-						rep->current.loiter_direction = curr->current.loiter_direction;
+					if (only_alt_change_requested) {
+						if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > 0) {
+							rep->current.loiter_radius = curr->current.loiter_radius;

-					} else {
-						rep->current.loiter_radius = get_loiter_radius();
-						rep->current.loiter_direction = 1;
+
+						} else {
+							rep->current.loiter_radius = get_loiter_radius();
+						}
+
+						if (curr->current.loiter_direction == 1 || curr->current.loiter_direction == -1) {
+							rep->current.loiter_direction = curr->current.loiter_direction;
+
+						} else {
+							rep->current.loiter_direction = 1;
+						}
 					}

-					rep->previous.valid = true;
 					rep->previous.timestamp = hrt_absolute_time();

 					rep->current.valid = true;
@@ -66,11 +66,25 @@ Takeoff::on_active()
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();

+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
 		// set loiter item so position controllers stop doing takeoff logic
-		set_loiter_item(&_mission_item);
-		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+		if (_navigator->get_land_detected()->landed) {
+			_mission_item.nav_cmd = NAV_CMD_IDLE;
+
+		} else {
+			if (pos_sp_triplet->current.valid) {
+				setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
+
+			} else {
+				setLoiterItemFromCurrentPosition(&_mission_item);
+			}
+		}
+
 		mission_apply_limitation(_mission_item);
+
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
 		_navigator->set_position_setpoint_triplet_updated();
 	}
 }
Author	SHA1	Message	Date
Daniel Agar	a88c6434b6	control_allocator: add parameter based ActuatorEffectivenessCustom - this allows creating a custom actuator effectiveness entirely from parameters of the form CA_ACTn_TRQ_{R,P,Y} and CA_ACTn_THR_{X,Y,Z}	2021-10-27 14:25:03 -04:00
Daniel Agar	43d7d83a4b	control_allocator: push VTOL flight phase into ActuatorEffectiveness	2021-10-27 13:42:22 -04:00
Daniel Agar	eb362f2f63	control_allocator: replace ModuleParams with generated parameter strings as needed	2021-10-27 12:48:42 -04:00
Peter van der Perk	51abb804ac	UAVCANv1 Fix NodeClient header and Kconfig merge logic	2021-10-27 10:07:01 -04:00
Beat Küng	0decdb1c7b	github action: run ./Tools/generate_board_targets_json.py in container As it requires kconfiglib	2021-10-27 15:23:23 +02:00
Silvan Fuhrer	e715e6c245	Fixed-wing position control: set yaw_sp to yaw_current instead of nav_bearing when not controlled Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 14:35:00 +03:00
Silvan Fuhrer	b53808d11b	fixed-wing: set yaw_sp to yaw_current instead of 0 when not controlled Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 14:35:00 +03:00
Silvan Fuhrer	da4d6dc657	L1: increase the max allowed tangential velocity in the opposite direction to 2m/s There is logic in L1 that prevents the vehicle from trying to achieve an impossible loiter entry (e.g. due to wind). That check makes the vehicle track the loiter center if the tangential velocity is in the wrong direction while loitering. After the vehicle flies through the center, it can then turn the other way around to join the loiter. This check is though too sensitive if it purely checks for the wrong direction, and it can end in delayed loiter entry for no reason. This commit increases the threshold to 2m/s of tangential velocity in the wrong direction to trigger the check. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 12:32:58 +03:00
RomanBapst	eee5f501cd	navigator: fix flyaway when altitude change is commanded without a valid triplet Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-27 11:01:13 +03:00
RomanBapst	bf6a47ba6a	navigator: cleanup of set_loiter_item Unwraps the set_loiter_item() to solve the issue where the altitdue setpoint in a MC takeoff wasn't correctly used. Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-27 11:01:13 +03:00
Silvan Fuhrer	cb78ba34d7	Mission: for tangential loiter exit, set current position setpoint typ to position Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 11:01:13 +03:00
Silvan Fuhrer	4b21c0c49e	Fw Pos C: always reset pos_sp type from LOITER to POSITION if far away Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 11:01:13 +03:00
RomanBapst	d678e792cc	mission_block: don't require an exiting heading when loitering if the next waypoint is within the loiter radius of the current waypoint Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-27 11:01:13 +03:00
Michael Schaeuble	5e1f62e9d0	Add option to warn the pilot in case of strong magnetic interference but still allow arming. This PR changes the COM_ARM_MAG_STR parameter to accept values. If the parameter is set to 2, the check is performed and a warning is logged but the vehicle can still arm.	2021-10-27 09:59:18 +02:00
ponomarevda	2b6bd452df	fix hardpoint hardfault by checking argc before std::strcmp	2021-10-27 08:11:23 +02:00
Beat Küng	de488f0f40	omnibus/f4sd: add topic listener & change timer order So it matches the usage in the channel definition order	2021-10-27 08:03:55 +02:00
Beat Küng	8476875b4d	Kconfig: add missing serial ports	2021-10-27 08:03:55 +02:00
Beat Küng	48344c6e2a	state_machine_helper: add missing 'break' (no behavior change)	2021-10-27 08:03:55 +02:00
Daniel Agar	6d0c6bb6ce	lib/world_magnetic_model: cmake remove helper target BYPRODUCTS - otherwise ninja will try to rebuild these	2021-10-26 18:52:12 -04:00