gps: ublox sensor_gps_heading message

.github/workflows/deploy_all.yml

@@ -10,7 +10,6 @@ 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:

Kconfig

@@ -128,9 +128,6 @@ config BOARD_KEYSTORE
 
 menu "Serial ports"
 
-    config BOARD_SERIAL_URT6
-        string "URT6 tty port"
-
     config BOARD_SERIAL_GPS1
         string "GPS1 tty port"
 
@@ -161,14 +158,8 @@ 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"

Tools/kconfig/merge_config.py

@@ -110,16 +110,10 @@ def main(kconfig_file, config1, config2):
 
     for line in f:
         match = unset_match(line)
-        #pprint.pprint(line)
+        #pprint.pprint(match)
         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

boards/holybro/kakutef7/default.px4board

@@ -5,7 +5,6 @@ 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

boards/omnibus/f4sd/default.px4board

@@ -3,7 +3,6 @@ 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
@@ -53,7 +52,6 @@ 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

boards/omnibus/f4sd/src/timer_config.cpp

@@ -34,8 +34,8 @@
 #include <px4_arch/io_timer_hw_description.h>
 
 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
-	initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
 	initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream1, DMA::Channel3}),
+	initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
 };
 
 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {

boards/px4/fmu-v5x/default.px4board

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

cmake/kconfig.cmake

@@ -210,9 +210,6 @@ 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()
@@ -243,15 +240,9 @@ 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

msg/CMakeLists.txt

@@ -138,6 +138,7 @@ 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

msg/sensor_gps_heading.msg

@@ -0,0 +1,10 @@
+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

src/drivers/gps/definitions.h

@@ -43,6 +43,7 @@
 #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__)

src/drivers/gps/gps.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 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
@@ -63,6 +63,7 @@
 #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"
@@ -180,7 +181,11 @@ 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
@@ -775,7 +780,7 @@ GPS::run()
 
 		/* FALLTHROUGH */
 		case gps_driver_mode_t::UBX:
-			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
+			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, &_sensor_gps_heading, _p_report_sat_info,
 						   gps_ubx_dynmodel, heading_offset, ubx_mode);
 			set_device_type(DRV_GPS_DEVTYPE_UBX);
 			break;
@@ -1099,6 +1104,15 @@ 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;
+		}
 	}
 }
 

src/drivers/uavcan/uavcan_main.cpp

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

src/drivers/uavcan_v1/NodeClient.hpp

@@ -1,92 +0,0 @@
-/****************************************************************************
- *
- *   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};
-};

src/lib/l1/ECL_L1_Pos_Controller.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2020 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,9 +276,7 @@ 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 */
-	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) {
+	if (tangent_vel < 0.0f && _circle_mode) {
 		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f);
 	}
 

src/lib/world_magnetic_model/CMakeLists.txt

@@ -34,6 +34,7 @@
 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)"
@@ -42,6 +43,7 @@ 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)"

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.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 = 1;
-	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check);
+	int32_t mag_strength_check_enabled = 1;
+	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);
 
 	float hgt_test_ratio_limit = 1.f;
 	param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
@@ -107,20 +107,13 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 		goto out;
 	}
 
-	if ((mag_strength_check >= 1) && status.pre_flt_fail_mag_field_disturbed) {
-		const char *message = "Preflight%s: Strong magnetic interference detected";
-
-		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, "");
+	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");
 		}
+
+		success = false;
+		goto out;
 	}
 
 	// check vertical position innovation test ratio

src/modules/commander/commander_params.c

@@ -620,13 +620,10 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45);
 /**
  * Enable mag strength preflight check
  *
- * Check if the estimator detects a strong magnetic
+ * Deny arming if the estimator detects a strong magnetic
  * disturbance (check enabled by EKF2_MAG_CHECK)
  *
- * @value 0 Disabled
- * @value 1 Deny arming
- * @value 2 Warning only
- *
+ * @boolean
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 1);

src/modules/commander/state_machine_helper.cpp

@@ -827,8 +827,6 @@ 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;
 	}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -44,6 +44,7 @@
 #include <ControlAllocation/ControlAllocation.hpp>
 
 #include <matrix/matrix/math.hpp>
+#include <uORB/topics/vehicle_actuator_setpoint.h>
 
 class ActuatorEffectiveness
 {
@@ -54,6 +55,23 @@ 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
 	 *
@@ -66,7 +84,20 @@ public:
 	 *
 	 * @return Actuator trims
 	 */
-	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const { return _trim; }
+	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;
+	}
 
 	/**
 	 * Get the number of actuators
@@ -75,4 +106,5 @@ public:
 
 protected:
 	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp

@@ -1,85 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -1,68 +0,0 @@
-/****************************************************************************
- *
- *   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};
-};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 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,33 +54,78 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NU
 		_updated = false;
 
 		// Get multirotor geometry
-		MultirotorGeometry 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();
 
-		for (int n = 0; n < NUM_ROTORS_MAX; n++) {
-			char buffer[17];
+		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 i = 0; i < 3; i++) {
-				char axis_char = 'X' + i;
+		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();
 
-				// 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[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_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[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();
 
-			geometry.rotors[n].axis.normalize();
+		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();
 
-			// CA_MC_Rn_CT
-			sprintf(buffer, "CA_MC_R%u_CT", n);
-			param_get(param_find(buffer), &geometry.rotors[n].thrust_coef);
+		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_KM
-			sprintf(buffer, "CA_MC_R%u_KM", n);
-			param_get(param_find(buffer), &geometry.rotors[n].moment_ratio);
-		}
+		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();
 
 		_num_actuators = computeEffectivenessMatrix(geometry, matrix);
 		return true;
@@ -100,7 +145,11 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
 
 		// Get rotor axis
-		matrix::Vector3f axis{geometry.rotors[i].axis};
+		matrix::Vector3f axis(
+			geometry.rotors[i].axis_x,
+			geometry.rotors[i].axis_y,
+			geometry.rotors[i].axis_z
+		);
 
 		// Normalize axis
 		float axis_norm = axis.norm();
@@ -114,7 +163,11 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		}
 
 		// Get rotor position
-		const matrix::Vector3f position{geometry.rotors[i].position};
+		matrix::Vector3f position(
+			geometry.rotors[i].position_x,
+			geometry.rotors[i].position_y,
+			geometry.rotors[i].position_z
+		);
 
 		// Get coefficients
 		float ct = geometry.rotors[i].thrust_coef;
@@ -125,10 +178,10 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		}
 
 		// Compute thrust generated by this rotor
-		const matrix::Vector3f thrust{ct * axis};
+		matrix::Vector3f thrust = ct * axis;
 
 		// Compute moment generated by this rotor
-		const matrix::Vector3f moment{ct * position.cross(axis) - ct *km * axis};
+		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
 
 		// Fill corresponding items in effectiveness matrix
 		for (size_t j = 0; j < 3; j++) {

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 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,16 +57,20 @@ public:
 
 	static constexpr int NUM_ROTORS_MAX = 8;
 
-	struct RotorGeometry {
-		matrix::Vector3f position{};
-		matrix::Vector3f axis{};
-		float thrust_coef{0.f};
-		float moment_ratio{0.f};
-	};
+	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 MultirotorGeometry {
+	typedef struct {
 		RotorGeometry rotors[NUM_ROTORS_MAX];
-	};
+	} MultirotorGeometry;
 
 	static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
 					      matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
@@ -77,4 +81,78 @@ 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
+	)
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 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,40 +41,15 @@
 
 #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;
 	}
@@ -124,3 +99,11 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float,
 	_updated = false;
 	return true;
 }
+
+void
+ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
+{
+	ActuatorEffectiveness::setFlightPhase(flight_phase);
+	_updated = true;
+
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 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,30 +43,22 @@
 
 #include "ActuatorEffectiveness.hpp"
 
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_status.h>
-
 class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessStandardVTOL() = default;
+	ActuatorEffectivenessStandardVTOL();
 	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};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 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,40 +41,14 @@
 
 #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;
 	}
@@ -132,3 +106,11 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float,
 	_updated = false;
 	return true;
 }
+
+void
+ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
+{
+	ActuatorEffectiveness::setFlightPhase(flight_phase);
+
+	_updated = true;
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 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,30 +43,22 @@
 
 #include "ActuatorEffectiveness.hpp"
 
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_status.h>
-
 class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessTiltrotorVTOL() = default;
+	ActuatorEffectivenessTiltrotorVTOL();
 	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};
 };

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -0,0 +1,52 @@
+############################################################################
+#
+#   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)

src/modules/control_allocator/CMakeLists.txt

@@ -31,45 +31,22 @@
 #
 ############################################################################
 
+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)

src/modules/control_allocator/ControlAllocation/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   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)

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -70,6 +70,7 @@
 #pragma once
 
 #include <matrix/matrix/math.hpp>
+#include <uORB/topics/vehicle_actuator_setpoint.h>
 
 class ControlAllocation
 {
@@ -80,7 +81,7 @@ public:
 	static constexpr uint8_t NUM_ACTUATORS = 16;
 	static constexpr uint8_t NUM_AXES = 6;
 
-	using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;
+	typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
 
 	enum ControlAxis {
 		ROLL = 0,

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp

@@ -40,8 +40,7 @@
  */
 
 #include <gtest/gtest.h>
-
-#include "ControlAllocationPseudoInverse.hpp"
+#include <ControlAllocationPseudoInverse.hpp>
 
 using namespace matrix;
 

src/modules/control_allocator/ControlAllocator.cpp

@@ -212,10 +212,6 @@ ControlAllocator::update_effectiveness_source()
 			tmp = new ActuatorEffectivenessTiltrotorVTOL();
 			break;
 
-		case EffectivenessSource::CUSTOM:
-			tmp = new ActuatorEffectivenessCustom(this);
-			break;
-
 		default:
 			PX4_ERR("Unknown airframe");
 			break;
@@ -264,6 +260,34 @@ 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);
@@ -488,10 +512,6 @@ 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

src/modules/control_allocator/ControlAllocator.hpp

@@ -41,15 +41,14 @@
 
 #pragma once
 
-#include <ActuatorEffectiveness/ActuatorEffectiveness.hpp>
-#include <ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp>
-#include <ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp>
-#include <ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp>
-#include <ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp>
+#include <ActuatorEffectiveness.hpp>
+#include <ActuatorEffectivenessMultirotor.hpp>
+#include <ActuatorEffectivenessStandardVTOL.hpp>
+#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
 
-#include <ControlAllocation/ControlAllocation.hpp>
-#include <ControlAllocation/ControlAllocationPseudoInverse.hpp>
-#include <ControlAllocation/ControlAllocationSequentialDesaturation.hpp>
+#include <ControlAllocation.hpp>
+#include <ControlAllocationPseudoInverse.hpp>
+#include <ControlAllocationSequentialDesaturation.hpp>
 
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
@@ -69,6 +68,7 @@
 #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,11 +124,10 @@ 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};
@@ -148,6 +147,7 @@ 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;

src/modules/control_allocator/control_allocator_params.c

@@ -50,7 +50,6 @@
  * @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);

src/modules/control_allocator/module.yaml

@@ -1,72 +0,0 @@
-__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

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -134,7 +134,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 }
 
 void
-FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
+FixedwingAttitudeControl::vehicle_manual_poll()
 {
 	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(const float yaw_body)
 					_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 = yaw_body; // yaw is not controlled, so set setpoint to current yaw
+					_att_sp.yaw_body = 0.0f;
 					_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(euler_angles.psi());
+		vehicle_manual_poll();
 		vehicle_land_detected_poll();
 
 		// the position controller will not emit attitude setpoints in some modes

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -233,7 +233,7 @@ private:
 	int		parameters_update();
 
 	void		vehicle_control_mode_poll();
-	void		vehicle_manual_poll(const float yaw_body);
+	void		vehicle_manual_poll();
 	void		vehicle_attitude_setpoint_poll();
 	void		vehicle_rates_setpoint_poll();
 	void		vehicle_land_detected_poll();

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -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; // yaw is not controlled, so set setpoint to current yaw
+			_hdg_hold_yaw = _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 = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			_att_sp.yaw_body = 0;
 		}
 
 		_control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -920,6 +920,7 @@ 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;
@@ -1047,7 +1048,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 = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	_att_sp.yaw_body = _l1_control.nav_bearing();
 
 	tecs_update_pitch_throttle(now, position_sp_alt,
 				   calculate_target_airspeed(mission_airspeed, ground_speed),
@@ -1151,7 +1152,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 = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	_att_sp.yaw_body = _l1_control.nav_bearing();
 
 	float alt_sp = pos_sp_curr.alt;
 
@@ -1266,7 +1267,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(_yaw);
+		_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
 		_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
 		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
 
@@ -1307,7 +1308,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 = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
 			/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
 			 * full throttle, otherwise we use idle throttle */
@@ -1464,7 +1465,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 	}
 
 	_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	_att_sp.yaw_body = _l1_control.nav_bearing();
 
 	if (_land_noreturn_horizontal) {
 		/* limit roll motion to prevent wings from touching the ground first */
@@ -1681,7 +1682,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 = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	_att_sp.yaw_body = 0;
 
 	/* Copy thrust and pitch values from tecs */
 	if (_landed) {
@@ -1775,7 +1776,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 = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
 			if (in_takeoff_situation()) {
 				/* limit roll motion to ensure enough lift */
@@ -1800,7 +1801,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 		}
 
 		_att_sp.roll_body = roll_sp_new;
-		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+		_att_sp.yaw_body = 0;
 	}
 
 	/* Copy thrust and pitch values from tecs */

src/modules/logger/logged_topics.cpp

@@ -154,6 +154,7 @@ 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);

src/modules/navigator/loiter.cpp

@@ -100,22 +100,11 @@ Loiter::set_loiter_position()
 
 	_loiter_pos_set = true;
 
-	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);
-		}
-
-	}
+	// set current mission item to loiter
+	set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());
 
 	// 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);

src/modules/navigator/mission.cpp

@@ -683,22 +683,11 @@ Mission::set_mission_items()
 
 		_mission_type = MISSION_TYPE_NONE;
 
-		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);
-			}
-
-		}
+		/* set loiter mission item and ensure that there is a minimum clearance from home */
+		set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());
 
 		/* 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);

src/modules/navigator/mission_block.cpp

@@ -375,9 +375,6 @@ 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
@@ -386,10 +383,11 @@ 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);
 
-			// 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;
+			if (enforce_exit_heading) {
 
-			if (enforce_exit_heading && exit_heading_is_reachable) {
+
+				const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
+								next_sp.lon);
 
 				float yaw_err = 0.0f;
 
@@ -441,9 +439,7 @@ MissionBlock::is_mission_item_reached()
 					bearing += inner_angle;
 				}
 
-				// 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;
+				// Replace current setpoint lat/lon with tangent coordinate
 				waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon,
 								   bearing, fabsf(curr_sp.loiter_radius),
 								   &curr_sp.lat, &curr_sp.lon);
@@ -644,40 +640,42 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 }
 
 void
-MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item)
+MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 {
-	setLoiterItemCommonFields(item);
+	if (_navigator->get_land_detected()->landed) {
+		/* landed, don't takeoff, but switch to IDLE mode */
+		item->nav_cmd = NAV_CMD_IDLE;
 
-	const position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	} else {
+		item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
 
-	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;
-}
+		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-void
-MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
-{
-	setLoiterItemCommonFields(item);
+		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;
 
-	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();
-}
+		} 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;
 
-void
-MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item)
-{
-	item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+			if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
+				item->altitude = _navigator->get_home_position()->alt + min_clearance;
+			}
+		}
 
-	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;
+		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;
+	}
 }
 
 void

src/modules/navigator/mission_block.h

@@ -108,11 +108,10 @@ protected:
 	 */
 	bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp);
 
-	void setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item);
-
-	void setLoiterItemFromCurrentPosition(struct mission_item_s *item);
-
-	void setLoiterItemCommonFields(struct mission_item_s *item);
+	/**
+	 * 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);
 
 	/**
 	 * Set a takeoff mission item

src/modules/navigator/navigator_main.cpp

@@ -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 = 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.lat = curr->current.lat;
+						rep->current.lon = curr->current.lon;
 						rep->current.alt = cmd.param7;
 						only_alt_change_requested = true;
 
@@ -343,23 +343,16 @@ Navigator::run()
 						}
 					}
 
-					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;
+					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;
 
-
-						} 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;
-						}
+					} else {
+						rep->current.loiter_radius = get_loiter_radius();
+						rep->current.loiter_direction = 1;
 					}
 
+					rep->previous.valid = true;
 					rep->previous.timestamp = hrt_absolute_time();
 
 					rep->current.valid = true;

src/modules/navigator/takeoff.cpp

@@ -66,25 +66,11 @@ 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
-		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);
-			}
-		}
-
+		set_loiter_item(&_mission_item);
+		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		mission_apply_limitation(_mission_item);
-
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
-
 		_navigator->set_position_setpoint_triplet_updated();
 	}
 }