initial control allocation support

- control allocation module with multirotor, VTOL standard, and tiltrotor support
 - angular_velocity_controller
 - See https://github.com/PX4/PX4-Autopilot/pull/13351 for details

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Roman Bapst <bapstroman@gmail.com>

ROMFS/px4fmu_common/init.d-posix/10017_iris_ctrlalloc

@@ -0,0 +1,52 @@
+#!/bin/sh
+#
+# @name 3DR Iris Quadrotor SITL
+#
+# @type Quadrotor Wide
+#
+# @maintainer Julian Oes <julian@oes.ch>
+#
+
+sh /etc/init.d/rc.mc_defaults
+sh /etc/init.d/rc.ctrlalloc
+
+if [ $AUTOCNF = yes ]
+then
+	param set MPC_USE_HTE 0
+
+	param set VM_MASS 1.5
+	param set VM_INERTIA_XX 0.03
+	param set VM_INERTIA_YY 0.03
+	param set VM_INERTIA_ZZ 0.05
+
+	param set CA_AIRFRAME 0
+	param set CA_METHOD 1
+	param set CA_ACT0_MIN 0.0
+	param set CA_ACT1_MIN 0.0
+	param set CA_ACT2_MIN 0.0
+	param set CA_ACT3_MIN 0.0
+	param set CA_ACT0_MAX 1.0
+	param set CA_ACT1_MAX 1.0
+	param set CA_ACT2_MAX 1.0
+	param set CA_ACT3_MAX 1.0
+
+	param set CA_MC_R0_PX 0.1515
+	param set CA_MC_R0_PY 0.245
+	param set CA_MC_R0_CT 6.5
+	param set CA_MC_R0_KM 0.05
+	param set CA_MC_R1_PX -0.1515
+	param set CA_MC_R1_PY -0.1875
+	param set CA_MC_R1_CT 6.5
+	param set CA_MC_R1_KM 0.05
+	param set CA_MC_R2_PX 0.1515
+	param set CA_MC_R2_PY -0.245
+	param set CA_MC_R2_CT 6.5
+	param set CA_MC_R2_KM -0.05
+	param set CA_MC_R3_PX -0.1515
+	param set CA_MC_R3_PY 0.1875
+	param set CA_MC_R3_CT 6.5
+	param set CA_MC_R3_KM -0.05
+
+fi
+
+set MIXER direct

ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc

@@ -0,0 +1,75 @@
+#!/bin/sh
+#
+# @name Typhoon H480 SITL
+#
+# @type Hexarotor x
+#
+
+sh /etc/init.d/rc.mc_defaults
+sh /etc/init.d/rc.ctrlalloc
+
+if [ $AUTOCNF = yes ]
+then
+	param set MPC_XY_VEL_I_ACC 4
+	param set MPC_XY_VEL_P_ACC 3
+
+	param set RTL_DESCEND_ALT 10
+	param set RTL_LAND_DELAY 0
+
+	param set TRIG_INTERFACE 3
+	param set TRIG_MODE 4
+	param set MNT_MODE_IN 0
+	param set MAV_PROTO_VER 2
+
+	param set MPC_USE_HTE 0
+
+	param set VM_MASS 2.66
+	param set VM_INERTIA_XX 0.06
+	param set VM_INERTIA_YY 0.06
+	param set VM_INERTIA_ZZ 0.10
+
+	param set CA_AIRFRAME 0
+	param set CA_METHOD 1
+	param set CA_ACT0_MIN 0.0
+	param set CA_ACT1_MIN 0.0
+	param set CA_ACT2_MIN 0.0
+	param set CA_ACT3_MIN 0.0
+	param set CA_ACT4_MIN 0.0
+	param set CA_ACT5_MIN 0.0
+	param set CA_ACT0_MAX 1.0
+	param set CA_ACT1_MAX 1.0
+	param set CA_ACT2_MAX 1.0
+	param set CA_ACT3_MAX 1.0
+	param set CA_ACT4_MAX 1.0
+	param set CA_ACT5_MAX 1.0
+
+	param set CA_MC_R0_PX 0.0
+	param set CA_MC_R0_PY 1.0
+	param set CA_MC_R0_CT 9.5
+	param set CA_MC_R0_KM -0.05
+	param set CA_MC_R1_PX 0.0
+	param set CA_MC_R1_PY -1.0
+	param set CA_MC_R1_CT 9.5
+	param set CA_MC_R1_KM 0.05
+	param set CA_MC_R2_PX 0.866025
+	param set CA_MC_R2_PY -0.5
+	param set CA_MC_R2_CT 9.5
+	param set CA_MC_R2_KM -0.05
+	param set CA_MC_R3_PX -0.866025
+	param set CA_MC_R3_PY 0.5
+	param set CA_MC_R3_CT 9.5
+	param set CA_MC_R3_KM 0.05
+	param set CA_MC_R4_PX 0.866025
+	param set CA_MC_R4_PY 0.5
+	param set CA_MC_R4_CT 9.5
+	param set CA_MC_R4_KM 0.05
+	param set CA_MC_R5_PX -0.866025
+	param set CA_MC_R5_PY -0.5
+	param set CA_MC_R5_CT 9.5
+	param set CA_MC_R5_KM -0.05
+fi
+
+set MAV_TYPE 13
+
+# set MIXER hexa_x
+set MIXER direct

ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc.post

@@ -0,0 +1,10 @@
+
+mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix
+
+mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530
+
+# shellcheck disable=SC2154
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
+# shellcheck disable=SC2154
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -54,4 +54,5 @@ px4_add_romfs_files(
 	rc.vehicle_setup
 	rc.vtol_apps
 	rc.vtol_defaults
+	rc.ctrlalloc
 )

ROMFS/px4fmu_common/init.d/airframes/4017_s500_ctrlalloc

@@ -0,0 +1,57 @@
+#!/bin/sh
+#
+# @name S500 with control allocation
+#
+# @type Quadrotor x
+# @class Copter
+#
+# @maintainer Silvan Fuhrer
+#
+
+sh /etc/init.d/rc.mc_defaults
+sh /etc/init.d/rc.ctrlalloc
+
+if [ $AUTOCNF = yes ]
+then
+	param set MPC_USE_HTE 0
+
+	param set VM_MASS 1.5
+	param set VM_INERTIA_XX 0.03
+	param set VM_INERTIA_YY 0.03
+	param set VM_INERTIA_ZZ 0.05
+
+	param set CA_AIRFRAME 0
+	param set CA_METHOD 1
+	param set CA_ACT0_MIN 0.0
+	param set CA_ACT1_MIN 0.0
+	param set CA_ACT2_MIN 0.0
+	param set CA_ACT3_MIN 0.0
+	param set CA_ACT0_MAX 1.0
+	param set CA_ACT1_MAX 1.0
+	param set CA_ACT2_MAX 1.0
+	param set CA_ACT3_MAX 1.0
+
+	param set CA_MC_R0_PX 0.177
+	param set CA_MC_R0_PY 0.177
+	param set CA_MC_R0_CT 6.5
+	param set CA_MC_R0_KM 0.05
+	param set CA_MC_R1_PX -0.177
+	param set CA_MC_R1_PY -0.177
+	param set CA_MC_R1_CT 6.5
+	param set CA_MC_R1_KM 0.05
+	param set CA_MC_R2_PX 0.177
+	param set CA_MC_R2_PY -0.177
+	param set CA_MC_R2_CT 6.5
+	param set CA_MC_R2_KM -0.05
+	param set CA_MC_R3_PX -0.177
+	param set CA_MC_R3_PY 0.177
+	param set CA_MC_R3_CT 6.5
+	param set CA_MC_R3_KM -0.05
+
+fi
+
+set MIXER direct
+set PWM_OUT 1234
+
+set MIXER_AUX direct_aux
+set PWM_AUX_OUT 1234

ROMFS/px4fmu_common/init.d/airframes/6002_hexa_x_ctrlalloc

@@ -0,0 +1,75 @@
+#!/bin/sh
+#
+# @name Hex X with control allocation
+#
+# @type Hexarotor x
+# @class Copter
+#
+# @maintainer Silvan Fuhrer
+#
+
+sh /etc/init.d/rc.mc_defaults
+sh /etc/init.d/rc.ctrlalloc
+
+if [ $AUTOCNF = yes ]
+then
+	param set MPC_USE_HTE 0
+
+	param set VM_MASS 1.5
+	param set VM_INERTIA_XX 0.03
+	param set VM_INERTIA_YY 0.03
+	param set VM_INERTIA_ZZ 0.05
+
+	param set CA_AIRFRAME 0
+	param set CA_METHOD 1
+	param set CA_ACT0_MIN 0.0
+	param set CA_ACT1_MIN 0.0
+	param set CA_ACT2_MIN 0.0
+	param set CA_ACT3_MIN 0.0
+	param set CA_ACT4_MIN 0.0
+	param set CA_ACT5_MIN 0.0
+
+	param set CA_ACT0_MAX 1.0
+	param set CA_ACT1_MAX 1.0
+	param set CA_ACT2_MAX 1.0
+	param set CA_ACT3_MAX 1.0
+	param set CA_ACT4_MAX 1.0
+	param set CA_ACT5_MAX 1.0
+
+	param set CA_MC_R0_PX 0.0
+	param set CA_MC_R0_PY 0.275
+	param set CA_MC_R0_CT 6.5
+	param set CA_MC_R0_KM -0.05
+
+	param set CA_MC_R1_PX 0.0
+	param set CA_MC_R1_PY -0.275
+	param set CA_MC_R1_CT 6.5
+	param set CA_MC_R1_KM 0.05
+
+	param set CA_MC_R2_PX 0.238
+	param set CA_MC_R2_PY -0.1375
+	param set CA_MC_R2_CT 6.5
+	param set CA_MC_R2_KM -0.05
+
+	param set CA_MC_R3_PX -0.238
+	param set CA_MC_R3_PY 0.1375
+	param set CA_MC_R3_CT 6.5
+	param set CA_MC_R3_KM 0.05
+
+	param set CA_MC_R4_PX 0.238
+	param set CA_MC_R4_PY 0.1375
+	param set CA_MC_R4_CT 6.5
+	param set CA_MC_R4_KM 0.05
+
+	param set CA_MC_R5_PX -0.238
+	param set CA_MC_R5_PY -0.1375
+	param set CA_MC_R5_CT 6.5
+	param set CA_MC_R5_KM -0.05
+
+fi
+
+set MIXER direct
+set PWM_OUT 123456
+
+set MIXER_AUX direct_aux
+set PWM_AUX_OUT 123456

ROMFS/px4fmu_common/init.d/rc.ctrlalloc

@@ -0,0 +1,26 @@
+#!/bin/sh
+#
+# Standard apps for new control allocation and controllers
+#
+# NOTE: Script variables are declared/initialized/unset in the rcS script.
+#
+
+#
+# Start angular velocity controller
+#
+angular_velocity_controller start
+mc_rate_control stop
+
+#
+# Start Control Allocator
+#
+control_allocator start
+
+#
+# Disable hover thrust estimator and prearming
+# These features are currently incompatible with control allocation
+#
+# TODO: fix
+#
+param set MPC_USE_HTE 0
+param set COM_PREARM_MODE 0

ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt

@@ -43,4 +43,5 @@ px4_add_romfs_files(
 	tiltrotor_sitl.main.mix
 	uuv_x_sitl.main.mix
 	vectored6dof_sitl.main.mix
+	tiltrotor_sitl_direct.main.mix
 )

ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix

@@ -0,0 +1,14 @@
+Mixer for quad tiltrotor (x motor configuration)
+================================================
+
+A: 0
+A: 1
+A: 2
+A: 3
+A: 4
+A: 5
+A: 6
+A: 7
+A: 8
+A: 9
+A: 10

ROMFS/px4fmu_common/mixers/CMakeLists.txt

@@ -47,6 +47,7 @@ px4_add_romfs_files(
 	coax.main.mix
 	delta.main.mix
 	deltaquad.main.mix
+	direct.main.mix
 	dodeca_bottom_cox.aux.mix
 	dodeca_top_cox.main.mix
 	firefly6.aux.mix

ROMFS/px4fmu_common/mixers/direct.main.mix

@@ -0,0 +1,10 @@
+# Direct mixer
+
+A: 0
+A: 1
+A: 2
+A: 3
+A: 4
+A: 5
+A: 6
+A: 7

Tools/px_romfs_pruner.py

@@ -97,7 +97,7 @@ def main():
                     # handle mixer files differently than startup files
                     if file_path.endswith(".mix"):
                         if line.startswith(("Z:", "M:", "R: ", "O:", "S:",
-                                            "H:", "T:", "P:")):
+                                            "H:", "T:", "P:", "A:")):
                             # reduce multiple consecutive spaces into a
                             # single space
                             line_reduced = re.sub(' +', ' ', line)

boards/px4/fmu-v3/ctrlalloc.cmake

@@ -0,0 +1,134 @@
+
+# FMUv3 is FMUv2 with access to the full 2MB flash
+
+px4_add_board(
+	PLATFORM nuttx
+	VENDOR px4
+	MODEL fmu-v3
+	LABEL ctrlalloc
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m4
+	ROMFSROOT px4fmu_common
+	IO px4_io-v2_default
+	TESTING
+	UAVCAN_INTERFACES 2
+	SERIAL_PORTS
+		GPS1:/dev/ttyS3
+		TEL1:/dev/ttyS1
+		TEL2:/dev/ttyS2
+		TEL4:/dev/ttyS6
+	DRIVERS
+		adc
+		barometer # all available barometer drivers
+		batt_smbus
+		camera_capture
+		camera_trigger
+		differential_pressure # all available differential pressure drivers
+		distance_sensor # all available distance sensor drivers
+		dshot
+		gps
+		#heater
+		#imu # all available imu drivers
+		imu/adis16448
+		imu/adis16477
+		imu/adis16497
+		imu/l3gd20
+		imu/lsm303d
+		imu/invensense/icm20608g
+		imu/invensense/mpu6000
+		imu/invensense/mpu9250
+		imu/icm20948
+		irlock
+		lights/blinkm
+		lights/rgbled
+		lights/rgbled_ncp5623c
+		#lights/rgbled_pwm
+		magnetometer # all available magnetometer drivers
+		mkblctrl
+		#optical_flow # all available optical flow drivers
+		optical_flow/px4flow
+		#osd
+		pca9685
+		#power_monitor/ina226
+		#protocol_splitter
+		pwm_input
+		pwm_out_sim
+		pwm_out
+		px4io
+		roboclaw
+		tap_esc
+		telemetry # all available telemetry drivers
+		test_ppm
+		tone_alarm
+		uavcan
+	MODULES
+		airspeed_selector
+		angular_velocity_controller
+		attitude_estimator_q
+		battery_status
+		camera_feedback
+		commander
+		control_allocator
+		dataman
+		ekf2
+		esc_battery
+		events
+		fw_att_control
+		fw_pos_control_l1
+		land_detector
+		landing_target_estimator
+		load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_att_control
+		mc_hover_thrust_estimator
+		mc_pos_control
+		mc_rate_control
+		#micrortps_bridge
+		navigator
+		rc_update
+		rover_pos_control
+		sensors
+		sih
+		temperature_compensation
+		vmount
+		vtol_att_control
+	SYSTEMCMDS
+		bl_update
+		#dmesg
+		dumpfile
+		esc_calib
+		gpio
+		hardfault_log
+		i2cdetect
+		led_control
+		mixer
+		motor_ramp
+		motor_test
+		mtd
+		nshterm
+		param
+		perf
+		pwm
+		reboot
+		reflect
+		sd_bench
+		tests # tests and test runner
+		top
+		topic_listener
+		tune_control
+		usb_connected
+		ver
+		work_queue
+	EXAMPLES
+		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		hello
+		hwtest # Hardware test
+		#matlab_csv_serial
+		px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		rover_steering_control # Rover example app
+		uuv_example_app
+		work_item
+	)

boards/px4/fmu-v4/ctrlalloc.cmake

@@ -0,0 +1,128 @@
+
+px4_add_board(
+	PLATFORM nuttx
+	VENDOR px4
+	MODEL fmu-v4
+	LABEL default
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m4
+	ROMFSROOT px4fmu_common
+	TESTING
+	UAVCAN_INTERFACES 1
+	SERIAL_PORTS
+		GPS1:/dev/ttyS3
+		TEL1:/dev/ttyS1
+		TEL2:/dev/ttyS2
+	DRIVERS
+		adc
+		barometer # all available barometer drivers
+		batt_smbus
+		camera_capture
+		camera_trigger
+		differential_pressure # all available differential pressure drivers
+		distance_sensor # all available distance sensor drivers
+		dshot
+		gps
+		heater
+		#imu # all available imu drivers
+		imu/adis16448
+		imu/adis16477
+		imu/adis16497
+		imu/invensense/icm20602
+		imu/invensense/icm20608g
+		imu/invensense/icm40609d
+		imu/invensense/mpu6500
+		imu/invensense/mpu9250
+		irlock
+		lights/blinkm
+		lights/rgbled
+		lights/rgbled_ncp5623c
+		magnetometer # all available magnetometer drivers
+		mkblctrl
+		optical_flow # all available optical flow drivers
+		#osd
+		pca9685
+		#protocol_splitter
+		pwm_input
+		pwm_out_sim
+		pwm_out
+		rc_input
+		roboclaw
+		safety_button
+		tap_esc
+		telemetry # all available telemetry drivers
+		test_ppm
+		tone_alarm
+		uavcan
+	MODULES
+		airspeed_selector
+		angular_velocity_controller
+		attitude_estimator_q
+		battery_status
+		camera_feedback
+		commander
+		control_allocator
+		dataman
+		ekf2
+		esc_battery
+		events
+		fw_att_control
+		fw_pos_control_l1
+		land_detector
+		landing_target_estimator
+		load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_att_control
+		mc_hover_thrust_estimator
+		mc_pos_control
+		mc_rate_control
+		#micrortps_bridge
+		navigator
+		rc_update
+		rover_pos_control
+		sensors
+		sih
+		temperature_compensation
+		uuv_att_control
+		vmount
+		vtol_att_control
+	SYSTEMCMDS
+		bl_update
+		#dmesg
+		dumpfile
+		esc_calib
+		gpio
+		hardfault_log
+		i2cdetect
+		led_control
+		mixer
+		motor_ramp
+		motor_test
+		mtd
+		nshterm
+		param
+		perf
+		pwm
+		reboot
+		reflect
+		sd_bench
+		tests # tests and test runner
+		top
+		topic_listener
+		tune_control
+		usb_connected
+		ver
+		work_queue
+	EXAMPLES
+		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		hello
+		hwtest # Hardware test
+		#matlab_csv_serial
+		px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		rover_steering_control # Rover example app
+		uuv_example_app
+		work_item
+	)

boards/px4/fmu-v5/ctrlalloc.cmake

@@ -0,0 +1,132 @@
+
+px4_add_board(
+	PLATFORM nuttx
+	VENDOR px4
+	MODEL fmu-v5
+	LABEL default
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m7
+	ROMFSROOT px4fmu_common
+	IO px4_io-v2_default
+	TESTING
+	UAVCAN_INTERFACES 2
+	SERIAL_PORTS
+		GPS1:/dev/ttyS0
+		TEL1:/dev/ttyS1
+		TEL2:/dev/ttyS2
+		TEL4:/dev/ttyS3
+	DRIVERS
+		adc
+		barometer # all available barometer drivers
+		batt_smbus
+		camera_capture
+		camera_trigger
+		differential_pressure # all available differential pressure drivers
+		distance_sensor # all available distance sensor drivers
+		dshot
+		gps
+		#heater
+		#imu # all available imu drivers
+		imu/adis16448
+		imu/adis16477
+		imu/adis16497
+		imu/bmi055
+		imu/invensense/icm20602
+		imu/invensense/icm20689
+		#imu/mpu6000 # legacy icm20602/icm20689 driver
+		irlock
+		lights/blinkm
+		lights/rgbled
+		lights/rgbled_ncp5623c
+		lights/rgbled_pwm
+		magnetometer # all available magnetometer drivers
+		mkblctrl
+		optical_flow # all available optical flow drivers
+		#osd
+		pca9685
+		power_monitor/ina226
+		#protocol_splitter
+		pwm_input
+		pwm_out_sim
+		pwm_out
+		px4io
+		rc_input
+		roboclaw
+		rpm
+		safety_button
+		tap_esc
+		telemetry # all available telemetry drivers
+		test_ppm
+		tone_alarm
+		uavcan
+	MODULES
+		airspeed_selector
+		angular_velocity_controller
+		attitude_estimator_q
+		battery_status
+		camera_feedback
+		commander
+		control_allocator
+		dataman
+		ekf2
+		esc_battery
+		events
+		fw_att_control
+		fw_pos_control_l1
+		land_detector
+		landing_target_estimator
+		load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_att_control
+		mc_hover_thrust_estimator
+		mc_pos_control
+		mc_rate_control
+		#micrortps_bridge
+		navigator
+		rc_update
+		rover_pos_control
+		sensors
+		sih
+		temperature_compensation
+		vmount
+		vtol_att_control
+	SYSTEMCMDS
+		bl_update
+		dmesg
+		dumpfile
+		esc_calib
+		gpio
+		hardfault_log
+		i2cdetect
+		led_control
+		mixer
+		motor_ramp
+		motor_test
+		mtd
+		nshterm
+		param
+		perf
+		pwm
+		reboot
+		reflect
+		sd_bench
+		tests # tests and test runner
+		top
+		topic_listener
+		tune_control
+		usb_connected
+		ver
+		work_queue
+	EXAMPLES
+		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		hello
+		hwtest # Hardware test
+		#matlab_csv_serial
+		px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		rover_steering_control # Rover example app
+		uuv_example_app
+		work_item
+	)

boards/px4/sitl/ctrlalloc.cmake

@@ -0,0 +1,108 @@
+
+px4_add_board(
+	PLATFORM posix
+	VENDOR px4
+	MODEL sitl
+	LABEL default
+	TESTING
+	DRIVERS
+		#barometer # all available barometer drivers
+		#batt_smbus
+		camera_capture
+		camera_trigger
+		#differential_pressure # all available differential pressure drivers
+		#distance_sensor # all available distance sensor drivers
+		gps
+		#imu # all available imu drivers
+		#magnetometer # all available magnetometer drivers
+		pwm_out_sim
+		rpm/rpm_simulator
+		#telemetry # all available telemetry drivers
+		tone_alarm
+		#uavcan
+	MODULES
+		airspeed_selector
+		angular_velocity_controller
+		attitude_estimator_q
+		camera_feedback
+		commander
+		control_allocator
+		dataman
+		ekf2
+		events
+		fw_att_control
+		fw_pos_control_l1
+		land_detector
+		landing_target_estimator
+		#load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_att_control
+		mc_hover_thrust_estimator
+		mc_pos_control
+		mc_rate_control
+		navigator
+		rc_update
+		replay
+		rover_pos_control
+		sensors
+		#sih
+		simulator
+		temperature_compensation
+		vmount
+		vtol_att_control
+		uuv_att_control
+
+	SYSTEMCMDS
+		#dumpfile
+		dyn
+		esc_calib
+		led_control
+		mixer
+		motor_ramp
+		motor_test
+		#mtd
+		#nshterm
+		param
+		perf
+		pwm
+		sd_bench
+		shutdown
+		tests # tests and test runner
+		#top
+		topic_listener
+		tune_control
+		ver
+		work_queue
+	EXAMPLES
+		dyn_hello # dynamically loading modules example
+		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		hello
+		#hwtest # Hardware test
+		#matlab_csv_serial
+		px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		rover_steering_control # Rover example app
+		uuv_example_app
+		work_item
+	)
+
+set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl")
+set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none")
+
+set(config_sitl_debugger disable CACHE STRING "debugger for sitl")
+set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb")
+
+# If the environment variable 'replay' is defined, we are building with replay
+# support. In this case, we enable the orb publisher rules.
+set(REPLAY_FILE "$ENV{replay}")
+if(REPLAY_FILE)
+	message(STATUS "Building with uorb publisher rules support")
+	add_definitions(-DORB_USE_PUBLISHER_RULES)
+
+	message(STATUS "Building without lockstep for replay")
+	set(ENABLE_LOCKSTEP_SCHEDULER no)
+else()
+	set(ENABLE_LOCKSTEP_SCHEDULER yes)
+endif()

boards/px4/sitl/test.cmake

@@ -28,6 +28,7 @@ px4_add_board(
 		attitude_estimator_q
 		camera_feedback
 		commander
+		control_allocator
 		dataman
 		ekf2
 		events

msg/CMakeLists.txt

@@ -48,6 +48,7 @@ set(msg_files
 	collision_constraints.msg
 	collision_report.msg
 	commander_state.msg
+	control_allocator_status.msg
 	cpuload.msg
 	differential_pressure.msg
 	distance_sensor.msg
@@ -140,8 +141,10 @@ set(msg_files
 	ulog_stream.msg
 	ulog_stream_ack.msg
 	vehicle_acceleration.msg
+	vehicle_actuator_setpoint.msg
 	vehicle_air_data.msg
 	vehicle_angular_acceleration.msg
+	vehicle_angular_acceleration_setpoint.msg
 	vehicle_angular_velocity.msg
 	vehicle_attitude.msg
 	vehicle_attitude_setpoint.msg
@@ -162,6 +165,8 @@ set(msg_files
 	vehicle_roi.msg
 	vehicle_status.msg
 	vehicle_status_flags.msg
+	vehicle_thrust_setpoint.msg
+	vehicle_torque_setpoint.msg
 	vehicle_trajectory_bezier.msg
 	vehicle_trajectory_waypoint.msg
 	vtol_vehicle_status.msg

msg/actuator_controls.msg

@@ -1,6 +1,6 @@
 uint64 timestamp			# time since system start (microseconds)
 uint8 NUM_ACTUATOR_CONTROLS = 8
-uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 6
 uint8 INDEX_ROLL = 0
 uint8 INDEX_PITCH = 1
 uint8 INDEX_YAW = 2
@@ -16,10 +16,13 @@ uint8 GROUP_INDEX_ATTITUDE = 0
 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
 uint8 GROUP_INDEX_GIMBAL = 2
 uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3
+uint8 GROUP_INDEX_ALLOCATED_PART1 = 4
+uint8 GROUP_INDEX_ALLOCATED_PART2 = 5
 uint8 GROUP_INDEX_PAYLOAD = 6
 
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[8] control
 
 # TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
+# TOPICS actuator_controls_4 actuator_controls_5
 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc

msg/control_allocator_status.msg

@@ -0,0 +1,21 @@
+uint64 timestamp                        # time since system start (microseconds)
+
+uint8 torque_setpoint_achieved          # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
+float32[3] allocated_torque             # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved.
+float32[3] unallocated_torque           # Unallocated torque. Equal to 0 if the setpoint was achieved.
+                                        # Computed as: unallocated_torque = torque_setpoint - allocated_torque
+
+uint8 thrust_setpoint_achieved          # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
+float32[3] allocated_thrust             # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved.
+float32[3] unallocated_thrust           # Unallocated thrust. Equal to 0 if the setpoint was achieved.
+                                        # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust
+
+int8 ACTUATOR_SATURATION_OK        =  0 # The actuator is not saturated
+int8 ACTUATOR_SATURATION_UPPER_DYN =  1 # The actuator is saturated (with a value <= the desired value) because it cannot increase its value faster
+int8 ACTUATOR_SATURATION_UPPER     =  2 # The actuator is saturated (with a value <= the desired value) because it has reached its maximum value
+int8 ACTUATOR_SATURATION_LOWER_DYN = -1 # The actuator is saturated (with a value >= the desired value) because it cannot decrease its value faster
+int8 ACTUATOR_SATURATION_LOWER     = -2 # The actuator is saturated (with a value >= the desired value) because it has reached its minimum value
+
+int8[16] actuator_saturation            # Indicates actuator saturation status.
+                                        # Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved.
+                                        # Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value.

msg/tools/uorb_rtps_message_ids.yaml

@@ -298,6 +298,16 @@ rtps:
     id: 141
   - msg: rtl_flight_time
     id: 142
+  - msg: vehicle_angular_acceleration_setpoint
+    id: 143
+  - msg: vehicle_torque_setpoint
+    id: 144
+  - msg: vehicle_thrust_setpoint
+    id: 145
+  - msg: vehicle_actuator_setpoint
+    id: 146
+  - msg: control_allocator_status
+    id: 147
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 170

msg/vehicle_actuator_setpoint.msg

@@ -0,0 +1,6 @@
+uint64 timestamp                   # time since system start (microseconds)
+uint64 timestamp_sample            # the timestamp the data this control response is based on was sampled
+
+uint8 NUM_ACTUATOR_SETPOINT = 16
+
+float32[16] actuator

msg/vehicle_angular_acceleration_setpoint.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp         # time since system start (microseconds)
+uint64 timestamp_sample  # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz           # angular acceleration about X, Y, Z body axis in rad/s^2

msg/vehicle_thrust_setpoint.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp        # time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz          # thrust setpoint along X, Y, Z body axis (in N)

msg/vehicle_torque_setpoint.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp        # time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz          # torque setpoint about X, Y, Z body axis (in N.m)

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp

@@ -49,6 +49,7 @@ struct wq_config_t {
 namespace wq_configurations
 {
 static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1664, 0}; // PX4 inner loop highest priority
+static constexpr wq_config_t ctrl_alloc{"wq:ctrl_alloc", 9500, 0}; // PX4 control allocation, same priority as rate_ctrl
 
 static constexpr wq_config_t SPI0{"wq:SPI0", 2336, -1};
 static constexpr wq_config_t SPI1{"wq:SPI1", 2336, -2};

platforms/posix/cmake/sitl_target.cmake

@@ -92,7 +92,7 @@ ExternalProject_Add(jsbsim_bridge
 set(viewers none jmavsim gazebo)
 set(debuggers none ide gdb lldb ddd valgrind callgrind)
 set(models none shell
-	if750a iris iris_dual_gps iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps px4vision solo typhoon_h480
+	if750a iris iris_dual_gps iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_ctrlalloc iris_obs_avoid iris_rtps px4vision solo typhoon_h480
 	plane plane_cam plane_catapult plane_lidar techpod
 	standard_vtol tailsitter tiltrotor
 	rover r1_rover boat cloudship

src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp

@@ -43,6 +43,7 @@ namespace math
 void LowPassFilter2pVector3f::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 {
 	_cutoff_freq = cutoff_freq;
+	_sample_freq = sample_freq;
 
 	// reset delay elements on filter change
 	_delay_element_1.zero();

src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp

@@ -74,12 +74,16 @@ public:
 	// Return the cutoff frequency
 	float get_cutoff_freq() const { return _cutoff_freq; }
 
+	// Return the sample frequency
+	float get_sample_freq() const { return _sample_freq; }
+
 	// Reset the filter state to this value
 	matrix::Vector3f reset(const matrix::Vector3f &sample);
 
 private:
 
 	float _cutoff_freq{0.0f};
+	float _sample_freq{0.0f};
 
 	float _a1{0.0f};
 	float _a2{0.0f};

src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 mixer_AllocatedActuatorMixer.cpp
+ *
+ * Mixer for allocated actuators.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "AllocatedActuatorMixer.hpp"
+
+#include <mathlib/mathlib.h>
+#include <cstdio>
+#include <px4_platform_common/defines.h>
+
+// #define debug(fmt, args...)	do { } while(0)
+#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+
+AllocatedActuatorMixer::AllocatedActuatorMixer(ControlCallback control_cb,
+		uintptr_t cb_handle,
+		uint8_t index) :
+	Mixer(control_cb, cb_handle)
+{
+	if (index < 8) {
+		_control_group = 4;
+		_control_index = index;
+
+	} else if (index < 16) {
+		_control_group = 5;
+		_control_index = index - 8;
+
+	} else {
+		debug("'A:' invalid index");
+	}
+}
+
+unsigned AllocatedActuatorMixer::set_trim(float trim)
+{
+	return 1;
+}
+
+unsigned AllocatedActuatorMixer::get_trim(float *trim)
+{
+	*trim = 0.0f;
+	return 1;
+}
+
+int
+AllocatedActuatorMixer::parse(const char *buf, unsigned &buflen, uint8_t &index)
+{
+	int ret;
+	int i;
+
+	// enforce that the mixer ends with a new line
+	if (!string_well_formed(buf, buflen)) {
+		return -1;
+	}
+
+	// parse line
+	if ((ret = sscanf(buf, "A: %d", &i)) != 1) {
+		debug("'A:' parser: failed on '%s'", buf);
+		return -1;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("'A:' parser: no line ending, line is incomplete");
+		return -1;
+	}
+
+	// check parsed index
+	if (i < 16) {
+		index = i;
+
+	} else {
+		debug("'A:' parser: invalid index");
+		return -1;
+	}
+
+	return 0;
+}
+
+AllocatedActuatorMixer *
+AllocatedActuatorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
+				  unsigned &buflen)
+{
+	uint8_t index;
+
+	if (parse(buf, buflen, index) == 0) {
+		return new AllocatedActuatorMixer(control_cb, cb_handle, index);
+
+	} else {
+		return nullptr;
+	}
+}
+
+unsigned
+AllocatedActuatorMixer::mix(float *outputs, unsigned space)
+{
+	if (space < 1) {
+		return 0;
+	}
+
+	_control_cb(_cb_handle,
+		    _control_group,
+		    _control_index,
+		    *outputs);
+
+	return 1;
+}
+
+void
+AllocatedActuatorMixer::groups_required(uint32_t &groups)
+{
+	groups |= 1 << _control_group;
+}

src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mixer_AllocatedActuatorMixer.hpp
+ *
+ * Mixer for allocated actuators.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include <lib/mixer/MixerBase/Mixer.hpp>
+
+/**
+ * Mixer for allocated actuators.
+ *
+ * Copies a single actuator to a single output.
+ */
+class AllocatedActuatorMixer : public Mixer
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param index	Actuator index (0..15)
+	 */
+	AllocatedActuatorMixer(ControlCallback control_cb,
+			       uintptr_t cb_handle,
+			       uint8_t index);
+	virtual ~AllocatedActuatorMixer() = default;
+
+	// no copy, assignment, move, move assignment
+	AllocatedActuatorMixer(const AllocatedActuatorMixer &) = delete;
+	AllocatedActuatorMixer &operator=(const AllocatedActuatorMixer &) = delete;
+	AllocatedActuatorMixer(AllocatedActuatorMixer &&) = delete;
+	AllocatedActuatorMixer &operator=(AllocatedActuatorMixer &&) = delete;
+
+	/**
+	 * Factory method with full external configuration.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param buf			Buffer containing a text description of
+	 *				the mixer.
+	 * @param buflen		Length of the buffer in bytes, adjusted
+	 *				to reflect the bytes consumed.
+	 * @return			A new AllocatedActuatorMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static AllocatedActuatorMixer *from_text(Mixer::ControlCallback control_cb,
+			uintptr_t cb_handle,
+			const char *buf,
+			unsigned &buflen);
+
+	unsigned mix(float *outputs, unsigned space) override;
+	void groups_required(uint32_t &groups) override;
+	unsigned set_trim(float trim) override;
+	unsigned get_trim(float *trim) override;
+
+private:
+	uint8_t	_control_group;	/**< group from which the input reads */
+	uint8_t	_control_index;	/**< index within the control group */
+
+	static int parse(const char *buf,
+			 unsigned &buflen,
+			 uint8_t &index);
+};

src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+
+add_library(AllocatedActuatorMixer
+	AllocatedActuatorMixer.cpp
+	AllocatedActuatorMixer.hpp
+)
+target_link_libraries(AllocatedActuatorMixer PRIVATE MixerBase)
+add_dependencies(AllocatedActuatorMixer prebuild_targets)

src/lib/mixer/CMakeLists.txt

@@ -34,6 +34,7 @@
 # required by other mixers
 add_subdirectory(MixerBase)
 
+add_subdirectory(AllocatedActuatorMixer)
 add_subdirectory(HelicopterMixer)
 add_subdirectory(MultirotorMixer)
 add_subdirectory(NullMixer)
@@ -48,6 +49,7 @@ add_library(mixer
 
 target_link_libraries(mixer
 	PRIVATE
+		AllocatedActuatorMixer
 		HelicopterMixer
 		MultirotorMixer
 		NullMixer

src/lib/mixer/MixerGroup.cpp

@@ -39,6 +39,7 @@
 
 #include "MixerGroup.hpp"
 
+#include "AllocatedActuatorMixer/AllocatedActuatorMixer.hpp"
 #include "HelicopterMixer/HelicopterMixer.hpp"
 #include "MultirotorMixer/MultirotorMixer.hpp"
 #include "NullMixer/NullMixer.hpp"
@@ -192,6 +193,10 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle
 			m = NullMixer::from_text(p, resid);
 			break;
 
+		case 'A':
+			m = AllocatedActuatorMixer::from_text(control_cb, cb_handle, p, resid);
+			break;
+
 		case 'M':
 			m = SimpleMixer::from_text(control_cb, cb_handle, p, resid);
 			break;

src/lib/mixer_module/mixer_module.cpp

@@ -49,7 +49,9 @@ MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &inter
 	{&interface, ORB_ID(actuator_controls_0)},
 	{&interface, ORB_ID(actuator_controls_1)},
 	{&interface, ORB_ID(actuator_controls_2)},
-	{&interface, ORB_ID(actuator_controls_3)}
+	{&interface, ORB_ID(actuator_controls_3)},
+	{&interface, ORB_ID(actuator_controls_4)},
+	{&interface, ORB_ID(actuator_controls_5)},
 },
 _scheduling_policy(scheduling_policy),
 _support_esc_calibration(support_esc_calibration),
@@ -536,9 +538,11 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
 
 	/* motor spinup phase - lock throttle to zero */
 	if (output->_output_limit.state == OUTPUT_LIMIT_STATE_RAMP) {
-		if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
+		if (((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
 		      control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
-		    control_index == actuator_controls_s::INDEX_THROTTLE) {
+		     control_index == actuator_controls_s::INDEX_THROTTLE) ||
+		    (control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 ||
+		     control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) {
 			/* limit the throttle output to zero during motor spinup,
 			 * as the motors cannot follow any demand yet
 			 */
@@ -548,9 +552,11 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
 
 	/* throttle not arming - mark throttle input as invalid */
 	if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) {
-		if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
+		if (((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
 		      control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
-		    control_index == actuator_controls_s::INDEX_THROTTLE) {
+		     control_index == actuator_controls_s::INDEX_THROTTLE) ||
+		    (control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 ||
+		     control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) {
 			/* set the throttle to an invalid value */
 			input = NAN;
 		}

src/lib/parameters/px4params/srcparser.py

@@ -348,8 +348,8 @@ class SourceParser(object):
         seenParamNames = []
         #allowedUnits should match set defined in /Firmware/validation/module_schema.yaml
         allowedUnits = set ([
-                                '%', 'Hz', 'mAh',
+                                '%', 'Hz', '1/s', 'mAh',
-                                'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s',  'rad s^2/m','rad s/m',
+                                'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s', 'rad s^2/m', 'rad s/m',
                                 'bit/s', 'B/s',
                                 'deg', 'deg*1e7', 'deg/s',
                                 'celcius', 'gauss', 'gauss/s', 'gauss^2',
@@ -359,7 +359,7 @@ class SourceParser(object):
                                 'us', 'ms', 's',
                                 'S', 'A/%', '(m/s^2)^2', 'm/m',  'tan(rad)^2', '(m/s)^2', 'm/rad',
                                 'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C',
-                                'N/(m/s)', 'Nm/(rad/s)', 'Nm', 'N',
+                                'N/(m/s)', 'Nm/rad', 'Nm/(rad/s)', 'Nm', 'N',
                                 'normalized_thrust/s', 'normalized_thrust', 'norm', 'SD',''])
         for group in self.GetParamGroups():
             for param in group.GetParams():

src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file AngularVelocityControl.cpp
+ */
+
+#include <AngularVelocityControl.hpp>
+#include <px4_platform_common/defines.h>
+
+using namespace matrix;
+
+void AngularVelocityControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
+{
+	_gain_p = P;
+	_gain_i = I;
+	_gain_d = D;
+}
+
+void AngularVelocityControl::setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
+		const matrix::Vector<bool, 3> &saturation_negative)
+{
+	_saturation_positive = saturation_positive;
+	_saturation_negative = saturation_negative;
+}
+
+void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_velocity_sp,
+				    const Vector3f &angular_acceleration, const float dt, const bool landed)
+{
+	// angular rates error
+	Vector3f angular_velocity_error = angular_velocity_sp - angular_velocity;
+
+	// P + D control
+	_angular_accel_sp = _gain_p.emult(angular_velocity_error) - _gain_d.emult(angular_acceleration);
+
+	// I + FF control
+	Vector3f torque_feedforward = _angular_velocity_int + _gain_ff.emult(angular_velocity_sp);
+
+	// compute torque setpoint
+	_torque_sp = _inertia * _angular_accel_sp + torque_feedforward + angular_velocity.cross(_inertia * angular_velocity);
+
+	// update integral only if we are not landed
+	if (!landed) {
+		updateIntegral(angular_velocity_error, dt);
+	}
+}
+
+void AngularVelocityControl::updateIntegral(Vector3f &angular_velocity_error, const float dt)
+{
+	for (int i = 0; i < 3; i++) {
+		// prevent further positive control saturation
+		if (_saturation_positive(i)) {
+			angular_velocity_error(i) = math::min(angular_velocity_error(i), 0.f);
+		}
+
+		// prevent further negative control saturation
+		if (_saturation_negative(i)) {
+			angular_velocity_error(i) = math::max(angular_velocity_error(i), 0.f);
+		}
+
+		// I term factor: reduce the I gain with increasing rate error.
+		// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
+		// change (noticeable in a bounce-back effect after a flip).
+		// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
+		// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
+		// and up to 200 deg error leads to <25% reduction of I.
+		float i_factor = angular_velocity_error(i) / math::radians(400.f);
+		i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
+
+		// Perform the integration using a first order method
+		float angular_velocity_i = _angular_velocity_int(i) + i_factor * _gain_i(i) * angular_velocity_error(i) * dt;
+
+		// do not propagate the result if out of range or invalid
+		if (PX4_ISFINITE(angular_velocity_i)) {
+			_angular_velocity_int(i) = math::constrain(angular_velocity_i, -_lim_int(i), _lim_int(i));
+		}
+	}
+}
+
+void AngularVelocityControl::reset()
+{
+	_angular_velocity_int.zero();
+	_torque_sp.zero();
+	_angular_accel_sp.zero();
+}

src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file AngularVelocityControl.hpp
+ *
+ * PID 3 axis angular velocity control.
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
+
+class AngularVelocityControl
+{
+public:
+	AngularVelocityControl() = default;
+	~AngularVelocityControl() = default;
+
+	/**
+	 * Set the control gains
+	 * @param P 3D vector of proportional gains for body x,y,z axis
+	 * @param I 3D vector of integral gains
+	 * @param D 3D vector of derivative gains
+	 */
+	void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
+
+	/**
+	 * Set the mximum absolute value of the integrator for all axes
+	 * @param integrator_limit limit value for all axes x, y, z
+	 */
+	void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
+
+	/**
+	 * Set direct angular velocity setpoint to torque feed forward gain
+	 * @see _gain_ff
+	 * @param FF 3D vector of feed forward gains for body x,y,z axis
+	 */
+	void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
+
+	/**
+	 * Set inertia matrix
+	 * @see _inertia
+	 * @param inertia inertia matrix
+	 */
+	void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; };
+
+	/**
+	 * Set saturation status
+	 * @see _saturation_positive
+	 * @see _saturation_negative
+	 * @param saturation_positive positive saturation
+	 * @param saturation_negative negative saturation
+	 */
+	void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
+				 const matrix::Vector<bool, 3> &saturation_negative);
+
+	/**
+	 * Run one control loop cycle calculation
+	 * @param angular_velocity estimation of the current vehicle angular velocity
+	 * @param angular_velocity_sp desired vehicle angular velocity setpoint
+	 * @param angular_acceleration estimation of the current vehicle angular acceleration
+	 * @param dt elapsed time since last update
+	 * @param landed whether the vehicle is on the ground
+	 */
+	void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_velocity_sp,
+		    const matrix::Vector3f &angular_acceleration, const float dt, const bool landed);
+
+	/**
+	 * Get the desired angular acceleration
+	 * @see _angular_accel_sp
+	 */
+	const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;};
+
+	/**
+	 * Get the torque vector to apply to the vehicle
+	 * @see _torque_sp
+	 */
+	const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;};
+
+	/**
+	 * Get the integral term
+	 * @see _torque_sp
+	 */
+	const matrix::Vector3f &getIntegral() {return _angular_velocity_int;};
+
+	/**
+	 * Set the integral term to 0 to prevent windup
+	 * @see _angular_velocity_int
+	 */
+	void resetIntegral() { _angular_velocity_int.zero(); }
+
+	/**
+	 * ReSet the controller state
+	 */
+	void reset();
+
+private:
+	void updateIntegral(matrix::Vector3f &angular_velocity_error, const float dt);
+
+	// Gains
+	matrix::Vector3f _gain_p; ///< proportional gain for all axes x, y, z
+	matrix::Vector3f _gain_i; ///< integral gain
+	matrix::Vector3f _gain_d; ///< derivative gain
+	matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
+	matrix::Vector3f _gain_ff; ///< direct angular velocity to torque feed forward gain
+	matrix::Matrix3f _inertia{matrix::eye<float, 3>()}; ///< inertia matrix
+
+	// States
+	matrix::Vector3f _angular_velocity_int;
+	matrix::Vector<bool, 3> _saturation_positive;
+	matrix::Vector<bool, 3> _saturation_negative;
+
+	// Output
+	matrix::Vector3f _angular_accel_sp; 	//< Angular acceleration setpoint computed using P and D gains
+	matrix::Vector3f _torque_sp;		//< Torque setpoint to apply to the vehicle
+};

src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp

@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <AngularVelocityControl.hpp>
+
+using namespace matrix;
+
+TEST(AngularVelocityControlTest, AllZeroCase)
+{
+	AngularVelocityControl control;
+	control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
+	Vector3f torque = control.getTorqueSetpoint();
+	EXPECT_EQ(torque, Vector3f());
+}

src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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(AngularVelocityControl
+	AngularVelocityControl.cpp
+)
+target_compile_options(AngularVelocityControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(AngularVelocityControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(AngularVelocityControl PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC AngularVelocityControlTest.cpp LINKLIBS AngularVelocityControl)

src/modules/angular_velocity_controller/AngularVelocityController.cpp

@@ -0,0 +1,344 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 "AngularVelocityController.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <circuit_breaker/circuit_breaker.h>
+#include <mathlib/math/Limits.hpp>
+#include <mathlib/math/Functions.hpp>
+#include <ecl/geo/geo.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+AngularVelocityController::AngularVelocityController() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+
+	parameters_updated();
+}
+
+AngularVelocityController::~AngularVelocityController()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+AngularVelocityController::init()
+{
+	if (!_vehicle_angular_velocity_sub.registerCallback()) {
+		PX4_ERR("vehicle_angular_velocity callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void
+AngularVelocityController::parameters_updated()
+{
+	// Control parameters
+	// The controller gain K is used to convert the parallel (P + I/s + sD) form
+	// to the ideal (K * [1 + 1/sTi + sTd]) form
+	const Vector3f k_gains = Vector3f(_param_avc_x_k.get(), _param_avc_y_k.get(), _param_avc_z_k.get());
+
+	_control.setGains(
+		k_gains.emult(Vector3f(_param_avc_x_p.get(), _param_avc_y_p.get(), _param_avc_z_p.get())),
+		k_gains.emult(Vector3f(_param_avc_x_i.get(), _param_avc_y_i.get(), _param_avc_z_i.get())),
+		k_gains.emult(Vector3f(_param_avc_x_d.get(), _param_avc_y_d.get(), _param_avc_z_d.get())));
+
+	_control.setIntegratorLimit(
+		Vector3f(_param_avc_x_i_lim.get(), _param_avc_y_i_lim.get(), _param_avc_z_i_lim.get()));
+
+	_control.setFeedForwardGain(
+		Vector3f(_param_avc_x_ff.get(), _param_avc_y_ff.get(), _param_avc_z_ff.get()));
+
+	// inertia matrix
+	const float inertia[3][3] = {
+		{_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()},
+		{_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()},
+		{_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()}
+	};
+	_control.setInertiaMatrix(matrix::Matrix3f(inertia));
+
+	// Hover thrust
+	if (!_param_mpc_use_hte.get()) {
+		_hover_thrust = _param_mpc_thr_hover.get();
+	}
+}
+
+void
+AngularVelocityController::Run()
+{
+	if (should_exit()) {
+		_vehicle_angular_velocity_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+		parameters_updated();
+	}
+
+	/* run controller on gyro changes */
+	vehicle_angular_velocity_s vehicle_angular_velocity;
+
+	if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) {
+		const hrt_abstime now = hrt_absolute_time();
+		_timestamp_sample = vehicle_angular_velocity.timestamp_sample;
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
+		_last_run = now;
+
+		const Vector3f angular_velocity{vehicle_angular_velocity.xyz};
+
+		/* check for updates in other topics */
+		_vehicle_status_sub.update(&_vehicle_status);
+		_vehicle_control_mode_sub.update(&_vehicle_control_mode);
+
+		if (_vehicle_land_detected_sub.updated()) {
+			vehicle_land_detected_s vehicle_land_detected;
+
+			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+				_landed = vehicle_land_detected.landed;
+				_maybe_landed = vehicle_land_detected.maybe_landed;
+			}
+		}
+
+		// Check for updates of hover thrust
+		if (_param_mpc_use_hte.get()) {
+			hover_thrust_estimate_s hte;
+
+			if (_hover_thrust_estimate_sub.update(&hte)) {
+				_hover_thrust = hte.hover_thrust;
+			}
+		}
+
+		// check angular acceleration topic
+		vehicle_angular_acceleration_s vehicle_angular_acceleration;
+
+		if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) {
+			_angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz);
+		}
+
+		// check rates setpoint topic
+		vehicle_rates_setpoint_s vehicle_rates_setpoint;
+
+		if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
+			_angular_velocity_sp(0) = vehicle_rates_setpoint.roll;
+			_angular_velocity_sp(1) = vehicle_rates_setpoint.pitch;
+			_angular_velocity_sp(2) = vehicle_rates_setpoint.yaw;
+			_thrust_sp = Vector3f(vehicle_rates_setpoint.thrust_body);
+
+			// Scale _thrust_sp in Newton, assuming _hover_thrust is equivalent to 1G
+			_thrust_sp *= (_param_vm_mass.get() * CONSTANTS_ONE_G / _hover_thrust);
+		}
+
+		// run the controller
+		if (_vehicle_control_mode.flag_control_rates_enabled) {
+			// reset integral if disarmed
+			if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+				_control.resetIntegral();
+			}
+
+			// update saturation status from mixer feedback
+			if (_control_allocator_status_sub.updated()) {
+				control_allocator_status_s control_allocator_status;
+
+				if (_control_allocator_status_sub.copy(&control_allocator_status)) {
+					Vector<bool, 3> saturation_positive;
+					Vector<bool, 3> saturation_negative;
+
+					if (not control_allocator_status.torque_setpoint_achieved) {
+						for (size_t i = 0; i < 3; i++) {
+							if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
+								saturation_positive(i) = true;
+
+							} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
+								saturation_negative(i) = true;
+							}
+						}
+					}
+
+					_control.setSaturationStatus(saturation_positive, saturation_negative);
+				}
+			}
+
+			// run rate controller
+			_control.update(angular_velocity, _angular_velocity_sp, _angular_acceleration, dt, _maybe_landed || _landed);
+
+			// publish rate controller status
+			rate_ctrl_status_s rate_ctrl_status{};
+			Vector3f integral = _control.getIntegral();
+			rate_ctrl_status.timestamp = hrt_absolute_time();
+			rate_ctrl_status.rollspeed_integ = integral(0);
+			rate_ctrl_status.pitchspeed_integ = integral(1);
+			rate_ctrl_status.yawspeed_integ = integral(2);
+			_rate_ctrl_status_pub.publish(rate_ctrl_status);
+
+			// publish controller output
+			publish_angular_acceleration_setpoint();
+			publish_torque_setpoint();
+			publish_thrust_setpoint();
+		}
+	}
+
+	perf_end(_loop_perf);
+}
+
+void
+AngularVelocityController::publish_angular_acceleration_setpoint()
+{
+	Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint();
+
+	vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
+	v_angular_accel_sp.timestamp = hrt_absolute_time();
+	v_angular_accel_sp.timestamp_sample = _timestamp_sample;
+	v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
+	v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
+	v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
+
+	_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
+}
+
+void
+AngularVelocityController::publish_torque_setpoint()
+{
+	Vector3f torque_sp = _control.getTorqueSetpoint();
+
+	vehicle_torque_setpoint_s v_torque_sp = {};
+	v_torque_sp.timestamp = hrt_absolute_time();
+	v_torque_sp.timestamp_sample = _timestamp_sample;
+	v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
+	v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
+	v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
+
+	_vehicle_torque_setpoint_pub.publish(v_torque_sp);
+}
+
+void
+AngularVelocityController::publish_thrust_setpoint()
+{
+	vehicle_thrust_setpoint_s v_thrust_sp = {};
+	v_thrust_sp.timestamp = hrt_absolute_time();
+	v_thrust_sp.timestamp_sample = _timestamp_sample;
+	v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f;
+	v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f;
+	v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f;
+
+	_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
+}
+
+int AngularVelocityController::task_spawn(int argc, char *argv[])
+{
+	AngularVelocityController *instance = new AngularVelocityController();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int AngularVelocityController::print_status()
+{
+	PX4_INFO("Running");
+
+	perf_print_counter(_loop_perf);
+
+	return 0;
+}
+
+int AngularVelocityController::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int AngularVelocityController::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the angular velocity controller.
+It takes angular velocity setpoints and measured angular
+velocity as inputs and outputs actuator setpoints.
+
+The controller has a PID loop for angular rate error.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+/**
+ * Angular velocity controller app start / stop handling function
+ */
+extern "C" __EXPORT int angular_velocity_controller_main(int argc, char *argv[]);
+
+int angular_velocity_controller_main(int argc, char *argv[])
+{
+	return AngularVelocityController::main(argc, argv);
+}

src/modules/angular_velocity_controller/AngularVelocityController.hpp

@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <AngularVelocityControl.hpp>
+
+#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+
+#include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/hover_thrust_estimate.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
+#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
+
+
+class AngularVelocityController : public ModuleBase<AngularVelocityController>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	AngularVelocityController();
+
+	virtual ~AngularVelocityController();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void Run() override;
+
+	bool init();
+
+private:
+
+	/**
+	 * initialize some vectors/matrices from parameters
+	 */
+	void		parameters_updated();
+
+	void		vehicle_status_poll();
+
+	void		publish_angular_acceleration_setpoint();
+	void		publish_torque_setpoint();
+	void		publish_thrust_setpoint();
+	void		publish_actuator_controls();
+
+	AngularVelocityControl _control; 	///< class for control calculations
+
+	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};				/**< motor limits subscription */
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};				/**< parameter updates subscription */
+	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; 	/**< vehicle angular acceleration subscription */
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};			/**< vehicle control mode subscription */
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};			/**< vehicle land detected subscription */
+	uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};			/**< vehicle rates setpoint subscription */
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};					/**< vehicle status subscription */
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::Publication<rate_ctrl_status_s>			   _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};						/**< controller status publication */
+	uORB::Publication<vehicle_angular_acceleration_setpoint_s> _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)};	/**< angular acceleration setpoint publication */
+	uORB::Publication<vehicle_thrust_setpoint_s>		   _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};				/**< thrust setpoint publication */
+	uORB::Publication<vehicle_torque_setpoint_s>		   _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};				/**< torque setpoint publication */
+
+	vehicle_control_mode_s		_vehicle_control_mode{};
+	vehicle_status_s		_vehicle_status{};
+
+	bool _landed{true};
+	bool _maybe_landed{true};
+
+	float _battery_status_scale{0.0f};
+
+	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
+
+	matrix::Vector3f _angular_velocity_sp;		/**< angular velocity setpoint */
+	matrix::Vector3f _angular_acceleration;		/**< angular acceleration (estimated) */
+	matrix::Vector3f _thrust_sp;			/**< thrust setpoint */
+
+	float _hover_thrust{0.5f};			/**< Normalized hover thrust **/
+
+	bool _gear_state_initialized{false};		/**< true if the gear state has been initialized */
+
+	hrt_abstime _task_start{hrt_absolute_time()};
+	hrt_abstime _last_run{0};
+	hrt_abstime _timestamp_sample{0};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::AVC_X_P>) _param_avc_x_p,
+		(ParamFloat<px4::params::AVC_X_I>) _param_avc_x_i,
+		(ParamFloat<px4::params::AVC_X_I_LIM>) _param_avc_x_i_lim,
+		(ParamFloat<px4::params::AVC_X_D>) _param_avc_x_d,
+		(ParamFloat<px4::params::AVC_X_FF>) _param_avc_x_ff,
+		(ParamFloat<px4::params::AVC_X_K>) _param_avc_x_k,
+
+		(ParamFloat<px4::params::AVC_Y_P>) _param_avc_y_p,
+		(ParamFloat<px4::params::AVC_Y_I>) _param_avc_y_i,
+		(ParamFloat<px4::params::AVC_Y_I_LIM>) _param_avc_y_i_lim,
+		(ParamFloat<px4::params::AVC_Y_D>) _param_avc_y_d,
+		(ParamFloat<px4::params::AVC_Y_FF>) _param_avc_y_ff,
+		(ParamFloat<px4::params::AVC_Y_K>) _param_avc_y_k,
+
+		(ParamFloat<px4::params::AVC_Z_P>) _param_avc_z_p,
+		(ParamFloat<px4::params::AVC_Z_I>) _param_avc_z_i,
+		(ParamFloat<px4::params::AVC_Z_I_LIM>) _param_avc_z_i_lim,
+		(ParamFloat<px4::params::AVC_Z_D>) _param_avc_z_d,
+		(ParamFloat<px4::params::AVC_Z_FF>) _param_avc_z_ff,
+		(ParamFloat<px4::params::AVC_Z_K>) _param_avc_z_k,
+
+		(ParamFloat<px4::params::VM_MASS>) _param_vm_mass,
+		(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx,
+		(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
+		(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
+		(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
+		(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
+		(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz,
+
+		(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
+		(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte
+	)
+
+};

src/modules/angular_velocity_controller/CMakeLists.txt

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(AngularVelocityControl)
+
+px4_add_module(
+	MODULE modules__angular_velocity_control
+	MAIN angular_velocity_controller
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+	SRCS
+		AngularVelocityController.cpp
+		AngularVelocityController.hpp
+	DEPENDS
+		circuit_breaker
+		mathlib
+		AngularVelocityControl
+		px4_work_queue
+	)

src/modules/angular_velocity_controller/angular_velocity_controller_params.c

@@ -0,0 +1,297 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 angular_velocity_controller_params.c
+ * Parameters for angular velocity controller.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Anton Babushkin <anton@px4.io>
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+/**
+ * Body X axis angular velocity P gain
+ *
+ * Body X axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @max 20.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_P, 18.f);
+
+/**
+ * Body X axis angular velocity I gain
+ *
+ * Body X axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @unit Nm/rad
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_I, 0.2f);
+
+/**
+ * Body X axis angular velocity integrator limit
+ *
+ * Body X axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_I_LIM, 0.3f);
+
+/**
+ * Body X axis angular velocity D gain
+ *
+ * Body X axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_D, 0.36f);
+
+/**
+ * Body X axis angular velocity feedforward gain
+ *
+ * Improves tracking performance.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 4
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_FF, 0.0f);
+
+/**
+ * Body X axis angular velocity controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = AVC_X_K * (AVC_X_P * error
+ * 			+ AVC_X_I * error_integral
+ * 			+ AVC_X_D * error_derivative)
+ * Set AVC_X_P=1 to implement a PID in the ideal form.
+ * Set AVC_X_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_K, 1.0f);
+
+/**
+ * Body Y axis angular velocity P gain
+ *
+ * Body Y axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @max 20.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_P, 18.f);
+
+/**
+ * Body Y axis angular velocity I gain
+ *
+ * Body Y axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @unit Nm/rad
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_I, 0.2f);
+
+/**
+ * Body Y axis angular velocity integrator limit
+ *
+ * Body Y axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_I_LIM, 0.3f);
+
+/**
+ * Body Y axis angular velocity D gain
+ *
+ * Body Y axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_D, 0.36f);
+
+/**
+ * Body Y axis angular velocity feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 4
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_FF, 0.0f);
+
+/**
+ * Body Y axis angular velocity controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = AVC_Y_K * (AVC_Y_P * error
+ * 			     + AVC_Y_I * error_integral
+ * 			     + AVC_Y_D * error_derivative)
+ * Set AVC_Y_P=1 to implement a PID in the ideal form.
+ * Set AVC_Y_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 20.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_K, 1.0f);
+
+/**
+ * Body Z axis angular velocity P gain
+ *
+ * Body Z axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @max 20.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_P, 7.f);
+
+/**
+ * Body Z axis angular velocity I gain
+ *
+ * Body Z axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @unit Nm/rad
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_I, 0.1f);
+
+/**
+ * Body Z axis angular velocity integrator limit
+ *
+ * Body Z axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_I_LIM, 0.30f);
+
+/**
+ * Body Z axis angular velocity D gain
+ *
+ * Body Z axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_D, 0.0f);
+
+/**
+ * Body Z axis angular velocity feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_FF, 0.0f);
+
+/**
+ * Body Z axis angular velocity controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = AVC_Z_K * (AVC_Z_P * error
+ * 			     + AVC_Z_I * error_integral
+ * 			     + AVC_Z_D * error_derivative)
+ * Set AVC_Z_P=1 to implement a PID in the ideal form.
+ * Set AVC_Z_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_K, 1.0f);

src/modules/angular_velocity_controller/vehicle_model_params.c

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 vehicle_model_params.c
+ * Parameters for vehicle model.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+/**
+ * Mass
+ *
+ * @unit kg
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_MASS, 1.f);
+
+/**
+ * Inertia matrix, XX component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 0.01f);
+
+/**
+ * Inertia matrix, YY component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 0.01f);
+
+/**
+ * Inertia matrix, ZZ component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 0.01f);
+
+/**
+ * Inertia matrix, XY component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f);
+
+/**
+ * Inertia matrix, XZ component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f);
+
+/**
+ * Inertia matrix, YZ component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f);

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 ActuatorEffectiveness.hpp
+ *
+ * Interface for Actuator Effectiveness
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include <ControlAllocation/ControlAllocation.hpp>
+
+#include <matrix/matrix/math.hpp>
+#include <uORB/topics/vehicle_actuator_setpoint.h>
+
+class ActuatorEffectiveness
+{
+public:
+	ActuatorEffectiveness() = default;
+	virtual ~ActuatorEffectiveness() = default;
+
+	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
+	};
+
+	/**
+	 * Update effectiveness matrix
+	 *
+	 * @return True if the effectiveness matrix has changed
+	 */
+	virtual bool update() = 0;
+
+	/**
+	 * Set the current flight phase
+	 *
+	 * @param Flight phase
+	 */
+	virtual void setFlightPhase(const FlightPhase &flight_phase)
+	{
+		_flight_phase = flight_phase;
+	};
+
+	/**
+	 * Get the control effectiveness matrix
+	 *
+	 * @return Effectiveness matrix
+	 */
+	const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &getEffectivenessMatrix() const
+	{
+		return _effectiveness;
+	};
+
+	/**
+	 * Get the actuator trims
+	 *
+	 * @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;
+	};
+
+protected:
+	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness;  //< Effectiveness matrix
+	matrix::Vector<float, NUM_ACTUATORS> _trim;			//< Actuator trim
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		//< Current flight phase
+};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -0,0 +1,201 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 ActuatorEffectivenessMultirotor.hpp
+ *
+ * Actuator effectiveness computed from rotors position and orientation
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "ActuatorEffectivenessMultirotor.hpp"
+
+ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor():
+	ModuleParams(nullptr)
+{
+	parameters_updated();
+}
+
+bool
+ActuatorEffectivenessMultirotor::update()
+{
+	bool updated = false;
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+		parameters_updated();
+
+		updated = true;
+	}
+
+	return updated;
+}
+
+matrix::Matrix<float, ActuatorEffectivenessMultirotor::NUM_AXES, ActuatorEffectivenessMultirotor::NUM_ACTUATORS>
+ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry geometry)
+{
+	matrix::Matrix<float, ActuatorEffectivenessMultirotor::NUM_AXES, ActuatorEffectivenessMultirotor::NUM_ACTUATORS>
+	effectiveness;
+
+	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
+		);
+
+		// Normalize axis
+		float axis_norm = axis.norm();
+
+		if (axis_norm > FLT_EPSILON) {
+			axis /= axis_norm;
+
+		} else {
+			// Bad axis definition, ignore this rotor
+			continue;
+		}
+
+		// Get rotor 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;
+		float km = geometry.rotors[i].moment_ratio;
+
+		// Compute thrust generated by this rotor
+		matrix::Vector3f thrust = ct * axis;
+
+		// Compute moment generated by this rotor
+		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
+
+		// Fill corresponding items in effectiveness matrix
+		for (size_t j = 0; j < 3; j++) {
+			effectiveness(j, i) = moment(j);
+			effectiveness(j + 3, i) = thrust(j);
+		}
+	}
+
+	return effectiveness;
+}
+
+void
+ActuatorEffectivenessMultirotor::parameters_updated()
+{
+	// 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();
+
+	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();
+
+	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();
+
+	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();
+
+	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[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[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();
+
+	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();
+
+	// Compute effectiveness matrix
+	_effectiveness = computeEffectivenessMatrix(geometry);
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 ActuatorEffectivenessMultirotor.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/topics/parameter_update.h>
+
+class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	ActuatorEffectivenessMultirotor();
+	virtual ~ActuatorEffectivenessMultirotor() = default;
+
+	/**
+	 * Update effectiveness matrix
+	 *
+	 * @return True if the effectiveness matrix has changed
+	 */
+	virtual bool update() override;
+
+	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;
+
+	typedef struct {
+		RotorGeometry rotors[NUM_ROTORS_MAX];
+	} MultirotorGeometry;
+
+	static matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> computeEffectivenessMatrix(MultirotorGeometry);
+
+private:
+	/**
+	 * initialize some vectors/matrices from parameters
+	 */
+	void parameters_updated();
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};		/**< parameter updates subscription */
+
+	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/ActuatorEffectivenessMultirotorTest.cpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 ActuatorEffectivenessMultirotorTest.cpp
+ *
+ * Tests for Control Allocation Algorithms
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <ActuatorEffectivenessMultirotor.hpp>
+
+using namespace matrix;
+
+TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase)
+{
+	// Quad wide geometry
+	ActuatorEffectivenessMultirotor::MultirotorGeometry geometry = {};
+	geometry.rotors[0].position_x = 1.0f;
+	geometry.rotors[0].position_y = 1.0f;
+	geometry.rotors[0].position_z = 0.0f;
+	geometry.rotors[0].axis_x = 0.0f;
+	geometry.rotors[0].axis_y = 0.0f;
+	geometry.rotors[0].axis_z = -1.0f;
+	geometry.rotors[0].thrust_coef = 1.0f;
+	geometry.rotors[0].moment_ratio = 0.05f;
+
+	geometry.rotors[1].position_x = -1.0f;
+	geometry.rotors[1].position_y = -1.0f;
+	geometry.rotors[1].position_z = 0.0f;
+	geometry.rotors[1].axis_x = 0.0f;
+	geometry.rotors[1].axis_y = 0.0f;
+	geometry.rotors[1].axis_z = -1.0f;
+	geometry.rotors[1].thrust_coef = 1.0f;
+	geometry.rotors[1].moment_ratio = 0.05f;
+
+	geometry.rotors[2].position_x = 1.0f;
+	geometry.rotors[2].position_y = -1.0f;
+	geometry.rotors[2].position_z = 0.0f;
+	geometry.rotors[2].axis_x = 0.0f;
+	geometry.rotors[2].axis_y = 0.0f;
+	geometry.rotors[2].axis_z = -1.0f;
+	geometry.rotors[2].thrust_coef = 1.0f;
+	geometry.rotors[2].moment_ratio = -0.05f;
+
+	geometry.rotors[3].position_x = -1.0f;
+	geometry.rotors[3].position_y = 1.0f;
+	geometry.rotors[3].position_z = 0.0f;
+	geometry.rotors[3].axis_x = 0.0f;
+	geometry.rotors[3].axis_y = 0.0f;
+	geometry.rotors[3].axis_z = -1.0f;
+	geometry.rotors[3].thrust_coef = 1.0f;
+	geometry.rotors[3].moment_ratio = -0.05f;
+
+	matrix::Matrix<float, ActuatorEffectiveness::NUM_AXES, ActuatorEffectiveness::NUM_ACTUATORS> effectiveness =
+		ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry);
+
+	const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
+		{-1.0f,   1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 1.0f,  -1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.05f,  0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{-1.0f,  -1.0f,  -1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
+	};
+	matrix::Matrix<float, ActuatorEffectiveness::NUM_AXES, ActuatorEffectiveness::NUM_ACTUATORS> effectiveness_expected(
+		expected);
+
+	EXPECT_EQ(effectiveness, effectiveness_expected);
+}