uas-at-ucla-dependencies · RyGuy101 · on Aug 29, 2020 · on Aug 29, 2020 · on Aug 29, 2020 · on Aug 29, 2020
diff --git a/.gitmodules b/.gitmodules
@@ -1,7 +1,3 @@
-[submodule "mavlink/include/mavlink/v2.0"]
-	path = mavlink/include/mavlink/v2.0
-	url = https://github.com/mavlink/c_library_v2.git
-	branch = master
 [submodule "src/drivers/uavcan/libuavcan"]
 	path = src/drivers/uavcan/libuavcan
 	url = https://github.com/PX4/uavcan.git

diff --git a/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_fmu b/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_fmu
@@ -0,0 +1,41 @@
+#!/bin/sh
+#
+# @name Generic Hexarotor x geometry, FMU
+#
+# @type Hexarotor x
+# @class Copter
+#
+# @output AUX1 motor 1
+# @output AUX2 motor 2
+# @output AUX3 motor 3
+# @output AUX4 motor 4
+# @output AUX5 motor 5
+# @output AUX6 motor 6
+#
+# @maintainer UAS@UCLA
+#
+# @board intel_aerofc-v1 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+set VEHICLE_TYPE mc
+
+if [ $AUTOCNF = yes ]
+then
+	param set NAV_ACC_RAD 2
+
+	param set RTL_RETURN_ALT 30
+	param set RTL_DESCEND_ALT 10
+	param set RTL_LAND_DELAY 0
+
+	param set PWM_AUX_MAX 2000
+	param set PWM_AUX_MIN 1100
+	param set PWM_AUX_DISARMED 900
+	param set PWM_AUX_RATE 400
+
+	param set GPS_UBX_DYNMODEL 6
+fi
+
+set MIXER_AUX hexa_x
+
+set PWM_AUX_OUT 123456
diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
@@ -94,6 +94,7 @@ px4_add_romfs_files(
 	# [6000, 6999] Hexarotor x"
 	6001_hexa_x
 	6002_draco_r
+	6003_hexa_x_fmu
 
 	# [7000, 7999] Hexarotor +"
 	7001_hexa_+

diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup
@@ -52,7 +52,7 @@ then
 		then
 			set MAV_TYPE 3
 		fi
-		if [ $MIXER = hexa_x -o $MIXER = hexa_+ ]
+		if [ $MIXER = hexa_x -o $MIXER = hexa_+ -o $MIXER_AUX = hexa_x ]
 		then
 			set MAV_TYPE 13
 		fi

diff --git a/ROMFS/px4fmu_common/mixers/CMakeLists.txt b/ROMFS/px4fmu_common/mixers/CMakeLists.txt
@@ -54,6 +54,7 @@ px4_add_romfs_files(
 	hexa_cox.main.mix
 	hexa_+.main.mix
 	hexa_x.main.mix
+	hexa_x.aux.mix
 	IO_pass.main.mix
 	mount.aux.mix
 	mount_legs.aux.mix

diff --git a/ROMFS/px4fmu_common/mixers/hexa_x.aux.mix b/ROMFS/px4fmu_common/mixers/hexa_x.aux.mix
@@ -0,0 +1,4 @@
+# Hexa X
+
+R: 6x 10000 10000 10000 0
+
diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
@@ -41,6 +41,9 @@ set(msg_files
 	adc_report.msg
 	airspeed.msg
 	airspeed_validated.msg
+	aviata_finalize_docking.msg
+	aviata_set_configuration.msg
+	aviata_set_standalone.msg
 	battery_status.msg
 	camera_capture.msg
 	camera_trigger.msg

@@ -0,0 +1,5 @@
+uint64 timestamp	# time since system start (microseconds)
+
+uint8 docking_slot
+uint8[6] missing_drones
+uint8 n_missing
@@ -0,0 +1,4 @@
+uint64 timestamp	# time since system start (microseconds)
+
+uint8[6] missing_drones
+uint8 n_missing
@@ -0,0 +1 @@
+uint64 timestamp	# time since system start (microseconds)
@@ -40,7 +40,6 @@
 #include "MixerGroup.hpp"
 
 #include "HelicopterMixer/HelicopterMixer.hpp"
-#include "MultirotorMixer/MultirotorMixer.hpp"
 #include "NullMixer/NullMixer.hpp"
 #include "SimpleMixer/SimpleMixer.hpp"
 
@@ -170,11 +169,15 @@ MixerGroup::groups_required(uint32_t &groups)
 }
 
 int
-MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen, MultirotorMixer** multirotor_mixer_ptr)
 {
 	int ret = -1;
 	const char *end = buf + buflen;
 
+	if (multirotor_mixer_ptr != nullptr) {
+		*multirotor_mixer_ptr = nullptr;
+	}
+
 	/*
 	 * Loop until either we have emptied the buffer, or we have failed to
 	 * allocate something when we expected to.
@@ -198,6 +201,9 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle
 
 		case 'R':
 			m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid);
+			if (multirotor_mixer_ptr != nullptr) {
+				*multirotor_mixer_ptr = (MultirotorMixer*) m;
+			}
 			break;
 
 		case 'H':

@@ -34,6 +34,7 @@
 #pragma once
 
 #include "MixerBase/Mixer.hpp"
+#include "MultirotorMixer/MultirotorMixer.hpp"
 
 /**
  * Group of mixers, built up from single mixers and processed
@@ -133,7 +134,7 @@ class MixerGroup
 	 *				bytes as they are consumed.
 	 * @return			Zero on successful load, nonzero otherwise.
 	 */
-	int				load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen);
+	int				load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen, MultirotorMixer** multirotor_mixer_ptr);
 
 	/**
 	 * @brief      Update slew rate parameter. This tells instances of the class MultirotorMixer

@@ -94,8 +94,11 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle
 	Mixer(control_cb, cb_handle),
 	_rotor_count(rotor_count),
 	_rotors(rotors),
+	_aviata_rotor_count(rotor_count),
+	_aviata_rotor_index(0),
 	_outputs_prev(new float[_rotor_count]),
-	_tmp_array(new float[_rotor_count])
+	_tmp_array(new float[_aviata_rotor_count]),
+	_tmp_outputs(new float[_aviata_rotor_count])
 {
 	for (unsigned i = 0; i < _rotor_count; ++i) {
 		_outputs_prev[i] = _idle_speed;
@@ -106,6 +109,19 @@ MultirotorMixer::~MultirotorMixer()
 {
 	delete[] _outputs_prev;
 	delete[] _tmp_array;
+	delete[] _tmp_outputs;
+}
+
+void
+MultirotorMixer::set_rotors(const Rotor* rotors, unsigned aviata_rotor_count) {
+	_rotors = rotors;
+	if (_aviata_rotor_count != aviata_rotor_count) {
+		_aviata_rotor_count = aviata_rotor_count;
+		delete[] _tmp_array;
+		delete[] _tmp_outputs;
+		_tmp_array = new float[_aviata_rotor_count];
+		_tmp_outputs = new float[_aviata_rotor_count];
+	}
 }
 
 MultirotorMixer *
@@ -172,7 +188,7 @@ MultirotorMixer::compute_desaturation_gain(const float *desaturation_vector, con
 	float k_min = 0.f;
 	float k_max = 0.f;
 
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		// Avoid division by zero. If desaturation_vector[i] is zero, there's nothing we can do to unsaturate anyway
 		if (fabsf(desaturation_vector[i]) < FLT_EPSILON) {
 			continue;
@@ -213,7 +229,7 @@ MultirotorMixer::minimize_saturation(const float *desaturation_vector, float *ou
 		return;
 	}
 
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		outputs[i] += k1 * desaturation_vector[i];
 	}
 
@@ -222,7 +238,7 @@ MultirotorMixer::minimize_saturation(const float *desaturation_vector, float *ou
 	// In that case adding 0.5 of the gain will equilibrate saturations.
 	float k2 = 0.5f * compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output);
 
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		outputs[i] += k2 * desaturation_vector[i];
 	}
 }
@@ -233,7 +249,7 @@ MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float thrust
 	// Airmode for roll and pitch, but not yaw
 
 	// Mix without yaw
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		outputs[i] = roll * _rotors[i].roll_scale +
 			     pitch * _rotors[i].pitch_scale +
 			     thrust * _rotors[i].thrust_scale;
@@ -254,7 +270,7 @@ MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrus
 	// Airmode for roll, pitch and yaw
 
 	// Do full mixing
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		outputs[i] = roll * _rotors[i].roll_scale +
 			     pitch * _rotors[i].pitch_scale +
 			     yaw * _rotors[i].yaw_scale +
@@ -268,7 +284,7 @@ MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrus
 
 	// Unsaturate yaw (in case upper and lower bounds are exceeded)
 	// to prioritize roll/pitch over yaw.
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		_tmp_array[i] = _rotors[i].yaw_scale;
 	}
 
@@ -281,7 +297,7 @@ MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float
 	// Airmode disabled: never allow to increase the thrust to unsaturate a motor
 
 	// Mix without yaw
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		outputs[i] = roll * _rotors[i].roll_scale +
 			     pitch * _rotors[i].pitch_scale +
 			     thrust * _rotors[i].thrust_scale;
@@ -294,13 +310,13 @@ MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float
 	minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
 
 	// Reduce roll/pitch acceleration if needed to unsaturate
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		_tmp_array[i] = _rotors[i].roll_scale;
 	}
 
 	minimize_saturation(_tmp_array, outputs, _saturation_status);
 
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		_tmp_array[i] = _rotors[i].pitch_scale;
 	}
 
@@ -313,7 +329,7 @@ MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float
 void MultirotorMixer::mix_yaw(float yaw, float *outputs)
 {
 	// Add yaw to outputs
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		outputs[i] += yaw * _rotors[i].yaw_scale;
 
 		// Yaw will be used to unsaturate if needed
@@ -324,7 +340,7 @@ void MultirotorMixer::mix_yaw(float yaw, float *outputs)
 	// and allow some yaw response at maximum thrust
 	minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f);
 
-	for (unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _aviata_rotor_count; i++) {
 		_tmp_array[i] = _rotors[i].thrust_scale;
 	}
 
@@ -339,30 +355,37 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		return 0;
 	}
 
-	float roll    = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
+	// AVIATA modification: removed constraints on these values
-	float pitch   = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
+	float roll    = get_control(0, 0) * _roll_scale;
-	float yaw     = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
+	float pitch   = get_control(0, 1) * _pitch_scale;
-	float thrust  = math::constrain(get_control(0, 3), 0.0f, 1.0f);
+	float yaw     = get_control(0, 2) * _yaw_scale;
+	float thrust  = get_control(0, 3);
+
 
 	// clean out class variable used to capture saturation
 	_saturation_status.value = 0;
 
 	// Do the mixing using the strategy given by the current Airmode configuration
 	switch (_airmode) {
 	case Airmode::roll_pitch:
-		mix_airmode_rp(roll, pitch, yaw, thrust, outputs);
+		mix_airmode_rp(roll, pitch, yaw, thrust, _tmp_outputs);
 		break;
 
 	case Airmode::roll_pitch_yaw:
-		mix_airmode_rpy(roll, pitch, yaw, thrust, outputs);
+		mix_airmode_rpy(roll, pitch, yaw, thrust, _tmp_outputs);
 		break;
 
 	case Airmode::disabled:
 	default: // just in case: default to disabled
-		mix_airmode_disabled(roll, pitch, yaw, thrust, outputs);
+		mix_airmode_disabled(roll, pitch, yaw, thrust, _tmp_outputs);
 		break;
 	}
 
+	// Added for AVIATA - transition from using full rotor config to using only this drone's rotors
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = _tmp_outputs[i + _aviata_rotor_index];
+	}
+
 	// Apply thrust model and scale outputs to range [idle_speed, 1].
 	// At this point the outputs are expected to be in [0, 1], but they can be outside, for example
 	// if a roll command exceeds the motor band limit.

@@ -40,7 +40,7 @@
  *
  * Values are generated by the px_generate_mixers.py script and placed to mixer_multirotor_normalized.generated.h
  */
-typedef uint8_t MultirotorGeometryUnderlyingType;
+typedef uint16_t MultirotorGeometryUnderlyingType;
 enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
 
 /**
@@ -150,6 +150,11 @@ class MultirotorMixer : public Mixer
 
 	unsigned		get_multirotor_count() override { return _rotor_count; }
 
+	// set_aviata_rotor_index(), set_rotors(), and get_rotors() added for AVIATA
+	void			set_aviata_rotor_index(unsigned aviata_rotor_index) { _aviata_rotor_index = aviata_rotor_index; }
+	void			set_rotors(const Rotor* rotors, unsigned aviata_rotor_count);
+	const Rotor*		get_rotors() { return _rotors; }
+
 	union saturation_status {
 		struct {
 			uint16_t valid		: 1; // 0 - true when the saturation status is used
@@ -254,7 +259,10 @@ class MultirotorMixer : public Mixer
 
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
+	unsigned			_aviata_rotor_count;
+	unsigned			_aviata_rotor_index;
 
 	float 				*_outputs_prev{nullptr};
 	float 				*_tmp_array{nullptr};
+	float 				*_tmp_outputs{nullptr};
 };
@@ -0,0 +1,179 @@
+#include "AviataMixerManager.hpp"
+#include <px4_platform_common/defines.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/topics/mavlink_log.h>
+#include <stdlib.h>
+int cmpfunc (const void * a, const void * b) {
+   return ( *(int*)a - *(int*)b );
+}
+
+// This doesn't make any sense but is required for some reason.
+// constexpr float AviataMixerManager::STANDALONE_SENS_BOARD_Z_OFF;
+constexpr AviataMixerManager::DualParamByName AviataMixerManager::DUAL_PARAMS_BY_NAME[];
+constexpr size_t AviataMixerManager::DUAL_PARAMS_LEN;
+
+AviataMixerManager::AviataMixerManager() : WorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+	for (size_t i = 0; i < DUAL_PARAMS_LEN; i++) {
+		_dual_params[i].param = param_find(DUAL_PARAMS_BY_NAME[i].param_name);
+		_dual_params[i].standalone_val = DUAL_PARAMS_BY_NAME[i].standalone_val;
+		_dual_params[i].aviata_val = DUAL_PARAMS_BY_NAME[i].aviata_val;
+	}
+}
+
+void AviataMixerManager::init(MultirotorMixer* m) {
+	_aviata_finalize_docking_sub.unregisterCallback();
+	_aviata_set_configuration_sub.unregisterCallback();
+	_aviata_set_standalone_sub.unregisterCallback();
+
+	if (m == nullptr || m->get_multirotor_count() != AVIATA_NUM_ROTORS) {
+		return; // Incompatible rotor count, so do nothing
+	}
+
+	_mixer = m;
+	_standalone_rotors = m->get_rotors();
+	_docked = false;
+
+	mavlink_log_info(&_mavlink_log_pub, "AVIATA: Initializing AviataMixerManager");
+	PX4_INFO("AVIATA: Initializing AviataMixerManager");
+
+	set_standalone_params();
+
+	_aviata_finalize_docking_sub.registerCallback();
+	_aviata_set_configuration_sub.registerCallback();
+	_aviata_set_standalone_sub.registerCallback();
+}
+
+void AviataMixerManager::Run() {
+	bool aviata_finalize_docking_received = _aviata_finalize_docking_sub.update(&_aviata_finalize_docking_cmd);
+	bool aviata_set_configuration_received = _aviata_set_configuration_sub.update(&_aviata_set_configuration_cmd);
+	bool aviata_set_standalone_received = _aviata_set_standalone_sub.update(&_aviata_set_standalone_cmd);
+
+	if (aviata_set_standalone_received) {
+		set_standalone();
+	} else {
+		if (aviata_finalize_docking_received){
+			finalize_docking(_aviata_finalize_docking_cmd.docking_slot,
+			                 _aviata_finalize_docking_cmd.missing_drones,
+			                 _aviata_finalize_docking_cmd.n_missing);
+		}
+		if (aviata_set_configuration_received) {
+			set_configuration(_aviata_set_configuration_cmd.missing_drones,
+			                  _aviata_set_configuration_cmd.n_missing);
+		}
+	}
+}
+
+void AviataMixerManager::finalize_docking(uint8_t docking_slot, uint8_t* missing_drones, uint8_t n_missing) {
+	if (!_docked && validate_configuration(docking_slot, missing_drones, n_missing)) {
+		_docked = true;
+		_docking_slot = docking_slot;
+		mavlink_log_info(&_mavlink_log_pub, "AVIATA DOCKED IN SLOT %u", docking_slot);
+		PX4_INFO("AVIATA DOCKED IN SLOT %u", docking_slot);
+
+		// No longer changing SENS_BOARD_Z_OFF because it confuses the estimator. See set_configuration() for how angle is shifted.
+		// float fc_yaw_rotation = STANDALONE_SENS_BOARD_Z_OFF - _config_aviata_drone_angle[docking_slot]; // Subtract due to opposite CW/CCW conventions
+		// param_set_no_notification(_handle_SENS_BOARD_Z_OFF, &fc_yaw_rotation);
+
+		// TODO Test without param setting
+		/*
+		for (size_t i = 0; i < DUAL_PARAMS_LEN; i++) {
+			param_set_no_notification(_dual_params[i].param, &_dual_params[i].aviata_val);
+		}
+		param_notify_changes();
+		*/
+
+		set_configuration(missing_drones, n_missing);
+		_mixer->set_aviata_rotor_index(AVIATA_NUM_ROTORS * docking_slot);
+	} else {
+		// AVIATA TODO print warning?
+	}
+}
+
+void AviataMixerManager::set_configuration(uint8_t* missing_drones, uint8_t n_missing) {
+	if (_docked && validate_configuration(_docking_slot, missing_drones, n_missing)) {
+		qsort(missing_drones, n_missing, sizeof(uint8_t), cmpfunc);
+
+		// Calculate index of aviata configuration, based on the predictable order in which combinations are generated.
+		MultirotorGeometryUnderlyingType mixer_index = 0;
+		int8_t missing_drone_prev = -1;
+		for (uint8_t i = 0; i < n_missing; i++) {
+			mixer_index += _n_choose_k[AVIATA_NUM_DRONES][i];
+			for (uint8_t j = missing_drone_prev+1; j < missing_drones[i]; j++) {
+				mixer_index += _n_choose_k[AVIATA_NUM_DRONES-j-1][n_missing-i-1];
+			}
+			missing_drone_prev = missing_drones[i];
+		}
+
+		// Rotate mixer according to angle of the docking slot (rotate in the opposite direction of docking slot angle)
+		for (uint8_t i = 0; i < AVIATA_NUM_DRONES * AVIATA_NUM_ROTORS; i++) {
+			_aviata_rotors[i].roll_scale = _config_aviata_index[mixer_index][i].roll_scale * _config_aviata_drone_angle_cos[_docking_slot] + _config_aviata_index[mixer_index][i].pitch_scale * _config_aviata_drone_angle_sin[_docking_slot];
+			_aviata_rotors[i].pitch_scale = _config_aviata_index[mixer_index][i].roll_scale * -_config_aviata_drone_angle_sin[_docking_slot] + _config_aviata_index[mixer_index][i].pitch_scale * _config_aviata_drone_angle_cos[_docking_slot];
+			_aviata_rotors[i].yaw_scale = _config_aviata_index[mixer_index][i].yaw_scale;
+			_aviata_rotors[i].thrust_scale = _config_aviata_index[mixer_index][i].thrust_scale;
+		}
+
+		_mixer->set_rotors(_aviata_rotors, AVIATA_NUM_DRONES * AVIATA_NUM_ROTORS);
+		mavlink_log_info(&_mavlink_log_pub, "SELECTED AVIATA MIXER: %s", _config_aviata_key[mixer_index]);
+		PX4_INFO("SELECTED AVIATA MIXER: %s", _config_aviata_key[mixer_index]);
+	} else {
+		// AVIATA TODO print warning?
+	}
+}
+
+void AviataMixerManager::set_standalone() {
+	if (_docked) {
+		set_standalone_params();
+		_mixer->set_aviata_rotor_index(0);
+		_mixer->set_rotors(_standalone_rotors, AVIATA_NUM_ROTORS);
+		_docked = false;
+		mavlink_log_info(&_mavlink_log_pub, "AVIATA UNDOCKED");
+		PX4_INFO("AVIATA UNDOCKED");
+	} else {
+		// AVIATA TODO print warning?
+	}
+}
+
+void AviataMixerManager::set_standalone_params() {
+	// param_set_no_notification(_handle_SENS_BOARD_Z_OFF, &STANDALONE_SENS_BOARD_Z_OFF);
+
+	// TODO Test without param setting
+	/*
+	for (size_t i = 0; i < DUAL_PARAMS_LEN; i++) {
+		param_set_no_notification(_dual_params[i].param, &_dual_params[i].standalone_val);
+	}
+	param_notify_changes();
+	*/
+}
+
+bool AviataMixerManager::validate_configuration(uint8_t docking_slot, uint8_t* missing_drones, uint8_t n_missing) {
+	if (n_missing > AVIATA_MAX_MISSING_DRONES) {
+		return false;
+	}
+	for (uint8_t i = 0; i < n_missing; i++) {
+		if (missing_drones[i] >= AVIATA_NUM_DRONES || missing_drones[i] == docking_slot) {
+			return false;
+		}
+	}
+	return true;
+}
+
+// From https://stackoverflow.com/a/11032879
+template <int N>
+int** AviataMixerManager::populate_n_choose_k_matrix(int C[N][N], int* C_rows[N]) {
+	for (int k = 1; k < N; k++) C[0][k] = 0;
+	for (int n = 0; n < N; n++) {
+		C[n][0] = 1;
+		C_rows[n] = C[n];
+	}
+
+	for (int n = 1; n < N; n++)
+		for (int k = 1; k < N; k++)
+			C[n][k] = C[n-1][k-1] + C[n-1][k];
+
+	return C_rows;
+}
+
+static int n_choose_k_matrix[AVIATA_NUM_DRONES+1][AVIATA_NUM_DRONES+1];
+static int* n_choose_k_rows[AVIATA_NUM_DRONES+1];
+int** AviataMixerManager::_n_choose_k = populate_n_choose_k_matrix(n_choose_k_matrix, n_choose_k_rows);
@@ -0,0 +1,128 @@
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/aviata_finalize_docking.h>
+#include <uORB/topics/aviata_set_configuration.h>
+#include <uORB/topics/aviata_set_standalone.h>
+#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
+#include "aviata_mixers.h"
+
+class AviataMixerManager: public px4::WorkItem
+{
+public:
+	AviataMixerManager();
+	void init(MultirotorMixer* m);
+
+private:
+	// static constexpr float STANDALONE_SENS_BOARD_Z_OFF = 0.0f;
+	// const param_t _handle_SENS_BOARD_Z_OFF = param_find("SENS_BOARD_Z_OFF");
+
+	struct DualParamByName {
+		const char* param_name;
+		float standalone_val;
+		float aviata_val;
+	};
+
+	struct DualParam {
+		param_t param;
+		float standalone_val;
+		float aviata_val;
+	};
+
+	// Define different parameter values for docked and undocked drones. Eventually, these should be made into PX4 parameters themselves.
+	static constexpr DualParamByName DUAL_PARAMS_BY_NAME[] = {
+		// Multicopter Attitude Control
+		{ "MC_PITCHRATE_MAX" , 220.0f , 220.0f },
+		{ "MC_ROLLRATE_MAX"  , 220.0f , 220.0f },
+		{ "MC_YAWRATE_MAX"   , 200.0f , 45.0f  },
+		{ "MPC_YAWRAUTO_MAX" , 45.0f  , 45.0f  },
+
+		{ "MC_PITCH_P"       , 6.5f   , 6.5f   },
+		{ "MC_ROLL_P"        , 6.5f   , 6.5f   },
+		{ "MC_YAW_P"         , 2.8f   , 2.8f   },
+
+		// Multicopter Position Control
+		{ "MPC_ACC_HOR"      , 3.0f   , 3.0f   },
+		{ "MPC_ACC_HOR_MAX"  , 5.0f   , 3.0f   },
+		{ "MPC_ACC_DOWN_MAX" , 3.0f   , 3.0f   },
+		{ "MPC_ACC_UP_MAX"   , 4.0f   , 4.0f   },
+
+		{ "MPC_TKO_SPEED"    , 1.5f   , 1.5f   },
+		{ "MPC_XY_CRUISE"    , 5.0f   , 5.0f   },
+		{ "MPC_XY_VEL_MAX"   , 12.0f  , 5.0f   },
+		{ "MPC_Z_VEL_MAX_DN" , 1.0f   , 1.0f   },
+		{ "MPC_Z_VEL_MAX_UP" , 3.0f   , 3.0f   },
+
+		{ "MPC_XY_P"         , 0.95f  , 0.95f  },
+		{ "MPC_XY_TRAJ_P"    , 0.5f   , 0.5f   },
+		{ "MPC_Z_P"          , 1.0f   , 1.0f   },
+
+		{ "MPC_XY_VEL_P_ACC" , 1.8f   , 1.8f   },
+		{ "MPC_XY_VEL_I_ACC" , 0.4f   , 0.4f   },
+		{ "MPC_XY_VEL_D_ACC" , 0.2f   , 0.2f   },
+
+		{ "MPC_Z_VEL_P_ACC"  , 4.0f   , 4.0f   },
+		{ "MPC_Z_VEL_I_ACC"  , 2.0f   , 2.0f   },
+		{ "MPC_Z_VEL_D_ACC"  , 0.0f   , 0.0f   },
+
+		// Multicopter Rate Control
+		{ "MC_PITCHRATE_P"   , 0.15f  , 0.15f  },
+		{ "MC_PITCHRATE_I"   , 0.2f   , 0.2f   },
+		{ "MC_PITCHRATE_D"   , 0.003f , 0.003f },
+		{ "MC_PITCHRATE_FF"  , 0.0f   , 0.0f   },
+		{ "MC_PR_INT_LIM"    , 0.30f  , 0.30f  },
+
+		{ "MC_ROLLRATE_P"    , 0.15f  , 0.15f  },
+		{ "MC_ROLLRATE_I"    , 0.2f   , 0.2f   },
+		{ "MC_ROLLRATE_D"    , 0.003f , 0.003f },
+		{ "MC_ROLLRATE_FF"   , 0.0f   , 0.0f   },
+		{ "MC_RR_INT_LIM"    , 0.30f  , 0.30f  },
+
+		{ "MC_YAWRATE_P"     , 0.2f   , 0.2f   },
+		{ "MC_YAWRATE_I"     , 0.1f   , 0.1f   },
+		{ "MC_YAWRATE_D"     , 0.0f   , 0.0f   },
+		{ "MC_YAWRATE_FF"    , 0.0f   , 0.0f   },
+		{ "MC_YR_INT_LIM"    , 0.30f  , 0.30f  }
+	};
+
+	static constexpr size_t DUAL_PARAMS_LEN = sizeof(DUAL_PARAMS_BY_NAME) / sizeof(DUAL_PARAMS_BY_NAME[0]);
+
+	DualParam _dual_params[DUAL_PARAMS_LEN];
+
+	uORB::SubscriptionCallbackWorkItem _aviata_finalize_docking_sub{this, ORB_ID(aviata_finalize_docking)};
+	uORB::SubscriptionCallbackWorkItem _aviata_set_configuration_sub{this, ORB_ID(aviata_set_configuration)};
+	uORB::SubscriptionCallbackWorkItem _aviata_set_standalone_sub{this, ORB_ID(aviata_set_standalone)};
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	aviata_finalize_docking_s _aviata_finalize_docking_cmd;
+	aviata_set_configuration_s _aviata_set_configuration_cmd;
+	aviata_set_standalone_s _aviata_set_standalone_cmd;
+
+	MultirotorMixer* _mixer;
+	const MultirotorMixer::Rotor* _standalone_rotors;
+	MultirotorMixer::Rotor _aviata_rotors[AVIATA_NUM_DRONES * AVIATA_NUM_ROTORS];
+	bool _docked;
+	uint8_t _docking_slot;
+
+	void Run() override;
+
+	// Configure the mixer once docked based on the docking slot and the missing drones.
+	// Also update PID values, flight controller orientation, etc.
+	void finalize_docking(uint8_t docking_slot, uint8_t* missing_drones, uint8_t n_missing);
+
+	// Update the mixer according to which drones are missing
+	void set_configuration(uint8_t* missing_drones, uint8_t n_missing);
+
+	// Set mixer to its original state (a standard hexacopter)
+	void set_standalone();
+
+	void set_standalone_params();
+
+	bool validate_configuration(uint8_t docking_slot, uint8_t* missing_drones, uint8_t n_missing);
+
+	template<int N>
+	static int** populate_n_choose_k_matrix(int C[N][N], int* C_rows[N]);
+
+	static int** _n_choose_k; // Used to calculate mixer index from missing_drones list
+};
@@ -31,4 +31,4 @@
 #
 ############################################################################
 
-px4_add_library(mixer_module mixer_module.cpp)
+px4_add_library(mixer_module mixer_module.cpp AviataMixerManager.cpp)
@@ -0,0 +1,105 @@
+/*
+* This file is automatically generated by px_generate_mixers.py - do not edit.
+*/
+
+#ifndef _AVIATA_MIXER_MULTI_TABLES
+#define _AVIATA_MIXER_MULTI_TABLES
+
+#define AVIATA_NUM_DRONES 4
+#define AVIATA_NUM_ROTORS 6
+#define AVIATA_MAX_MISSING_DRONES 0
+
+static constexpr float _config_aviata_drone_angle[] {
+	 0.785398, // 45.0 degrees
+	 2.356194, // 135.0 degrees
+	-2.356194, // -135.0 degrees
+	-0.785398, // -45.0 degrees
+};
+
+static constexpr float _config_aviata_drone_angle_cos[] {
+	 0.707107,
+	-0.707107,
+	-0.707107,
+	 0.707107,
+};
+
+static constexpr float _config_aviata_drone_angle_sin[] {
+	 0.707107,
+	 0.707107,
+	-0.707107,
+	-0.707107,
+};
+
+static constexpr float _config_aviata_relative_drone_angle[][4] {
+	{  0.000000, -1.570796,  3.141593,  1.570796, },
+	{  1.570796,  0.000000, -1.570796,  3.141593, },
+	{ -3.141593,  1.570796,  0.000000, -1.570796, },
+	{ -1.570796, -3.141593,  1.570796,  0.000000, },
+};
+
+static constexpr float _config_aviata_relative_drone_angle_cos[][4] {
+	{  1.000000,  0.000000, -1.000000,  0.000000, },
+	{  0.000000,  1.000000, -0.000000, -1.000000, },
+	{ -1.000000, -0.000000,  1.000000,  0.000000, },
+	{  0.000000, -1.000000,  0.000000,  1.000000, },
+};
+
+static constexpr float _config_aviata_relative_drone_angle_sin[][4] {
+	{  0.000000, -1.000000,  0.000000,  1.000000, },
+	{  1.000000,  0.000000, -1.000000,  0.000000, },
+	{ -0.000000,  1.000000,  0.000000, -1.000000, },
+	{ -1.000000, -0.000000,  1.000000,  0.000000, },
+};
+
+enum class AviataMultirotorGeometry : MultirotorGeometryUnderlyingType {
+	AVIATA_MISSING_,               // AVIATA with these drones missing:  (text key aviata_missing_)
+
+	MAX_GEOMETRY
+}; // enum class AviataMultirotorGeometry
+
+namespace {
+static constexpr MultirotorMixer::Rotor _config_aviata_aviata_missing_[] {
+	{ -0.035260,  0.022147, -0.122511,  0.774201 },
+	{ -0.022147,  0.035260,  0.122511,  0.774201 },
+	{ -0.031103,  0.037659, -0.478577,  0.774201 },
+	{ -0.026304,  0.019747, -0.233555,  0.774201 },
+	{ -0.037659,  0.031103,  0.478577,  0.774201 },
+	{ -0.019747,  0.026304,  0.233555,  0.774201 },
+	{ -0.022147, -0.035260, -0.122511,  0.774201 },
+	{ -0.035260, -0.022147,  0.122511,  0.774201 },
+	{ -0.037659, -0.031103, -0.478577,  0.774201 },
+	{ -0.019747, -0.026304, -0.233555,  0.774201 },
+	{ -0.031103, -0.037659,  0.478577,  0.774201 },
+	{ -0.026304, -0.019747,  0.233555,  0.774201 },
+	{  0.035260, -0.022147, -0.122511,  0.774201 },
+	{  0.022147, -0.035260,  0.122511,  0.774201 },
+	{  0.031103, -0.037659, -0.478577,  0.774201 },
+	{  0.026304, -0.019747, -0.233555,  0.774201 },
+	{  0.037659, -0.031103,  0.478577,  0.774201 },
+	{  0.019747, -0.026304,  0.233555,  0.774201 },
+	{  0.022147,  0.035260, -0.122511,  0.774201 },
+	{  0.035260,  0.022147,  0.122511,  0.774201 },
+	{  0.037659,  0.031103, -0.478577,  0.774201 },
+	{  0.019747,  0.026304, -0.233555,  0.774201 },
+	{  0.031103,  0.037659,  0.478577,  0.774201 },
+	{  0.026304,  0.019747,  0.233555,  0.774201 },
+};
+
+static constexpr const MultirotorMixer::Rotor *_config_aviata_index[] {
+	&_config_aviata_aviata_missing_[0],
+};
+
+static constexpr unsigned _config_aviata_rotor_count[] {
+	24, /* aviata_missing_ */
+};
+
+__attribute__((unused)) // Not really unused, but fixes compilation error
+const char* _config_aviata_key[] {
+	"aviata_missing_",	/* aviata_missing_ */
+};
+
+} // anonymous namespace
+
+#endif /* _AVIATA_MIXER_MULTI_TABLES */
+
+
@@ -80,6 +80,7 @@ _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency"))
 MixingOutput::~MixingOutput()
 {
 	perf_free(_control_latency_perf);
+	_aviata_mixer_manager.init(nullptr);
 	delete _mixers;
 	px4_sem_destroy(&_lock);
 }
@@ -520,7 +521,7 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
 	input = output->_controls[control_group].control[control_index];
 
 	/* limit control input */
-	input = math::constrain(input, -1.f, 1.f);
+	// input = math::constrain(input, -1.f, 1.f);
 
 	/* motor spinup phase - lock throttle to zero */
 	if (output->_output_limit.state == OUTPUT_LIMIT_STATE_RAMP) {
@@ -550,6 +551,7 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
 void MixingOutput::resetMixer()
 {
 	if (_mixers != nullptr) {
+		_aviata_mixer_manager.init(nullptr);
 		delete _mixers;
 		_mixers = nullptr;
 		_groups_required = 0;
@@ -569,10 +571,13 @@ int MixingOutput::loadMixer(const char *buf, unsigned len)
 		return -ENOMEM;
 	}
 
-	int ret = _mixers->load_from_buf(controlCallback, (uintptr_t)this, buf, len);
+	MultirotorMixer* multirotor_mixer_ptr;
+	int ret = _mixers->load_from_buf(controlCallback, (uintptr_t)this, buf, len, &multirotor_mixer_ptr);
+	_aviata_mixer_manager.init(multirotor_mixer_ptr);
 
 	if (ret != 0) {
 		PX4_ERR("mixer load failed with %d", ret);
+		_aviata_mixer_manager.init(nullptr);
 		delete _mixers;
 		_mixers = nullptr;
 		_groups_required = 0;

@@ -51,6 +51,7 @@
 #include <uORB/topics/multirotor_motor_limits.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/test_motor.h>
+#include "AviataMixerManager.hpp"
 
 /**
  * @class OutputModuleInterface
@@ -261,6 +262,8 @@ class MixingOutput : public ModuleParams
 	uint8_t _driver_instance{0}; ///< for boards that supports multiple outputs (e.g. PX4IO + FMU)
 	const uint8_t _max_num_outputs;
 
+	AviataMixerManager _aviata_mixer_manager;
+
 	struct MotorTest {
 		uORB::Subscription test_motor_sub{ORB_ID(test_motor)};
 		bool in_test_mode{false};

@@ -1233,10 +1233,11 @@ Mavlink::send_protocol_version()
 	msg.version = _protocol_version * 100;
 	msg.min_version = 100;
 	msg.max_version = 200;
-	uint64_t mavlink_lib_git_version_binary = px4_mavlink_lib_version_binary();
+	// uint64_t mavlink_lib_git_version_binary = px4_mavlink_lib_version_binary();
 	// TODO add when available
 	//memcpy(&msg.spec_version_hash, &mavlink_spec_git_version_binary, sizeof(msg.spec_version_hash));
-	memcpy(&msg.library_version_hash, &mavlink_lib_git_version_binary, sizeof(msg.library_version_hash));
+	// memcpy(&msg.library_version_hash, &mavlink_lib_git_version_binary, sizeof(msg.library_version_hash));
+	memset(&msg.library_version_hash, 0, sizeof(msg.library_version_hash));
 
 	// Switch to MAVLink 2
 	int curr_proto_ver = _protocol_version;
@@ -1618,6 +1619,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 
 	case MAVLINK_MODE_ONBOARD:
 		// Note: streams requiring low latency come first
+		configure_stream_local("ATTITUDE_TARGET", 50.0f); // Stream ATTITUDE_TARGET at high rate for AVIATA
+
 		configure_stream_local("TIMESYNC", 10.0f);
 		configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
 		configure_stream_local("HIGHRES_IMU", 50.0f);
@@ -1632,7 +1635,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ACTUATOR_CONTROL_TARGET0", 10.0f);
 		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
 		configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
-		configure_stream_local("ATTITUDE_TARGET", 10.0f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_CAPTURE", 2.0f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);

@@ -118,6 +118,8 @@
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/wind_estimate.h>
+#include <uORB/topics/aviata_finalize_docking.h>
+#include <uORB/topics/aviata_set_standalone.h>
 
 using matrix::Vector3f;
 using matrix::wrap_2pi;
@@ -3623,6 +3625,9 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream
 private:
 	uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _att_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
+	uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _aviata_finalize_docking_sub{ORB_ID(aviata_finalize_docking)};
+	uORB::Subscription _aviata_set_standalone_sub{ORB_ID(aviata_set_standalone)};
 
 	/* do not allow top copying this class */
 	MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &) = delete;
@@ -3643,14 +3648,38 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream
 			msg.time_boot_ms = att_sp.timestamp / 1000;
 			matrix::Quatf(att_sp.q_d).copyTo(msg.q);
 
-			vehicle_rates_setpoint_s att_rates_sp{};
+			// vehicle_rates_setpoint_s att_rates_sp{};
-			_att_rates_sp_sub.copy(&att_rates_sp);
+			// _att_rates_sp_sub.copy(&att_rates_sp);
-
+
-			msg.body_roll_rate = att_rates_sp.roll;
+			// msg.body_roll_rate = att_rates_sp.roll;
-			msg.body_pitch_rate = att_rates_sp.pitch;
+			// msg.body_pitch_rate = att_rates_sp.pitch;
-			msg.body_yaw_rate = att_rates_sp.yaw;
+			// msg.body_yaw_rate = att_rates_sp.yaw;
-
+
-			msg.thrust = att_sp.thrust_body[0];
+			msg.thrust = -att_sp.thrust_body[2];
+
+			aviata_finalize_docking_s aviata_finalize_docking_cmd;
+			bool did_dock = _aviata_finalize_docking_sub.copy(&aviata_finalize_docking_cmd);
+			aviata_set_standalone_s aviata_set_standalone_cmd;
+			bool did_undock = _aviata_set_standalone_sub.copy(&aviata_set_standalone_cmd);
+
+			// Only do the following if currently docked
+			if (did_dock && (!did_undock || aviata_finalize_docking_cmd.timestamp > aviata_set_standalone_cmd.timestamp)) {
+				vehicle_attitude_s att;
+				_att_sub.copy(&att);
+				float a = att.q[0];
+				float b = att.q[1];
+				float c = att.q[2];
+				float d = att.q[3];
+				float body_x_est_0 = a*a + b*b - c*c - d*d;
+				float body_x_est_1 = 2 * (b*c + a*d);
+				msg.body_roll_rate = atan2f(body_x_est_1, body_x_est_0); // aviata_yaw_est (not actually body_roll_rate)
+
+				msg.body_pitch_rate = (float) aviata_finalize_docking_cmd.docking_slot; // aviata_docking_slot (not actually body_pitch_rate)
+			} else {
+				msg.body_roll_rate = 0;
+				msg.body_pitch_rate = 0;
+			}
+			msg.body_yaw_rate = 0; // unused
 
 			mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);
 

@@ -472,6 +472,49 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 
 		_actuator_controls_pubs[actuator_controls_s::GROUP_INDEX_GIMBAL].publish(actuator_controls);
 
+	} else if (cmd_mavlink.command == MAV_CMD_AVIATA_FINALIZE_DOCKING) {
+		aviata_finalize_docking_s aviata_finalize_docking_cmd;
+		aviata_finalize_docking_cmd.timestamp = hrt_absolute_time();
+		aviata_finalize_docking_cmd.docking_slot = (uint8_t) (vehicle_command.param1+0.5f);
+		float missing_drones_float[6];
+		missing_drones_float[0] = vehicle_command.param2;
+		missing_drones_float[1] = vehicle_command.param3;
+		missing_drones_float[2] = vehicle_command.param4;
+		missing_drones_float[3] = vehicle_command.param5;
+		missing_drones_float[4] = vehicle_command.param6;
+		missing_drones_float[5] = vehicle_command.param7;
+		aviata_finalize_docking_cmd.n_missing = 0;
+		for (uint8_t i = 0; i < 6; i++) {
+			if (isnan(missing_drones_float[i])) {
+				break;
+			}
+			aviata_finalize_docking_cmd.missing_drones[i] = (uint8_t) (missing_drones_float[i]+0.5f);
+			aviata_finalize_docking_cmd.n_missing++;
+		}
+		_aviata_finalize_docking_pub.publish(aviata_finalize_docking_cmd);
+	} else if (cmd_mavlink.command == MAV_CMD_AVIATA_SET_CONFIGURATION) {
+		aviata_set_configuration_s aviata_set_configuration_cmd;
+		aviata_set_configuration_cmd.timestamp = hrt_absolute_time();
+		float missing_drones_float[6];
+		missing_drones_float[0] = vehicle_command.param2;
+		missing_drones_float[1] = vehicle_command.param3;
+		missing_drones_float[2] = vehicle_command.param4;
+		missing_drones_float[3] = vehicle_command.param5;
+		missing_drones_float[4] = vehicle_command.param6;
+		missing_drones_float[5] = vehicle_command.param7;
+		aviata_set_configuration_cmd.n_missing = 0;
+		for (uint8_t i = 0; i < 6; i++) {
+			if (isnan(missing_drones_float[i])) {
+				break;
+			}
+			aviata_set_configuration_cmd.missing_drones[i] = (uint8_t) (missing_drones_float[i]+0.5f);
+			aviata_set_configuration_cmd.n_missing++;
+		}
+		_aviata_set_configuration_pub.publish(aviata_set_configuration_cmd);
+	} else if (cmd_mavlink.command == MAV_CMD_AVIATA_SET_STANDALONE) {
+		aviata_set_standalone_s aviata_set_standalone_cmd;
+		aviata_set_standalone_cmd.timestamp = hrt_absolute_time();
+		_aviata_set_standalone_pub.publish(aviata_set_standalone_cmd);
 	} else {
 
 		send_ack = false;
@@ -1409,10 +1452,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 		PX4_ISFINITE(set_attitude_target.q[1]) &&
 		PX4_ISFINITE(set_attitude_target.q[2]) &&
 		PX4_ISFINITE(set_attitude_target.q[3]) &&
-		PX4_ISFINITE(set_attitude_target.thrust) &&
+		PX4_ISFINITE(set_attitude_target.thrust) /* &&
 		PX4_ISFINITE(set_attitude_target.body_roll_rate) &&
 		PX4_ISFINITE(set_attitude_target.body_pitch_rate) &&
-		PX4_ISFINITE(set_attitude_target.body_yaw_rate);
+		PX4_ISFINITE(set_attitude_target.body_yaw_rate) */;
 
 	/* Only accept messages which are intended for this system */
 	if ((mavlink_system.sysid == set_attitude_target.target_system ||
@@ -1487,6 +1530,54 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 
 					if (!ignore_attitude_msg) { // only copy att sp if message contained new data
 						matrix::Quatf q(set_attitude_target.q);
+
+						aviata_finalize_docking_s aviata_finalize_docking_cmd;
+						bool did_dock = _aviata_finalize_docking_sub.copy(&aviata_finalize_docking_cmd);
+						aviata_set_standalone_s aviata_set_standalone_cmd;
+						bool did_undock = _aviata_set_standalone_sub.copy(&aviata_set_standalone_cmd);
+
+						// Only do the following if currently docked
+						if (did_dock && (!did_undock || aviata_finalize_docking_cmd.timestamp > aviata_set_standalone_cmd.timestamp)) {
+							// Shift yaw angle according to docking slot
+							matrix::Dcmf R_sp(q);
+							float y_C_0 = -R_sp(1,0);
+							float y_C_1 = R_sp(0,0);
+							uint8_t sender_docking_slot = (uint8_t) (set_attitude_target.body_pitch_rate+0.5f); // body_pitch_rate indicates aviata_docking_slot
+							float docking_slot_rel_angle_cos = _config_aviata_relative_drone_angle_cos[aviata_finalize_docking_cmd.docking_slot][sender_docking_slot];
+							float docking_slot_rel_angle_sin = _config_aviata_relative_drone_angle_sin[aviata_finalize_docking_cmd.docking_slot][sender_docking_slot];
+							matrix::Vector3f y_C(
+								docking_slot_rel_angle_cos*y_C_0 - docking_slot_rel_angle_sin*y_C_1,
+								docking_slot_rel_angle_sin*y_C_0 + docking_slot_rel_angle_cos*y_C_1,
+								0
+							);
+							matrix::Vector3f body_z(R_sp(0,2), R_sp(1,2), R_sp(2,2));
+							matrix::Vector3f body_x = y_C % body_z;
+							body_x.normalize();
+							matrix::Vector3f body_y = body_z % body_x;
+							R_sp(2, 0) = body_x(2);
+							R_sp(2, 1) = body_y(2);
+
+							// Rotate setpoint to match yaw error of leader
+							vehicle_attitude_s att;
+							_vehicle_attitude_sub.copy(&att);
+							float a = att.q[0];
+							float b = att.q[1];
+							float c = att.q[2];
+							float d = att.q[3];
+							float body_x_est_0 = a*a + b*b - c*c - d*d;
+							float body_x_est_1 = 2 * (b*c + a*d);
+							float yaw_sp_adjust = atan2f(body_x_est_1, body_x_est_0) - (set_attitude_target.body_roll_rate + _config_aviata_relative_drone_angle[aviata_finalize_docking_cmd.docking_slot][sender_docking_slot]); // body_roll_rate indicates aviata_yaw_est
+							float cos_theta = cosf(yaw_sp_adjust);
+							float sin_theta = sinf(yaw_sp_adjust);
+							R_sp(0, 0) = cos_theta*body_x(0) - sin_theta*body_x(1);
+							R_sp(1, 0) = sin_theta*body_x(0) + cos_theta*body_x(1);
+							R_sp(0, 1) = cos_theta*body_y(0) - sin_theta*body_y(1);
+							R_sp(1, 1) = sin_theta*body_y(0) + cos_theta*body_y(1);
+							R_sp(0, 2) = cos_theta*body_z(0) - sin_theta*body_z(1);
+							R_sp(1, 2) = sin_theta*body_z(0) + cos_theta*body_z(1);
+							q = matrix::Quatf(R_sp);
+						}
+
 						q.copyTo(att_sp.q_d);
 
 						matrix::Eulerf euler{q};
@@ -1513,33 +1604,33 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 				}
 
 				/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
-				if (!offboard_control_mode.ignore_bodyrate_x ||
+				// if (!offboard_control_mode.ignore_bodyrate_x ||
-				    !offboard_control_mode.ignore_bodyrate_y ||
+				//     !offboard_control_mode.ignore_bodyrate_y ||
-				    !offboard_control_mode.ignore_bodyrate_z) {
+				//     !offboard_control_mode.ignore_bodyrate_z) {
 
-					vehicle_rates_setpoint_s rates_sp{};
+				// 	vehicle_rates_setpoint_s rates_sp{};
 
-					rates_sp.timestamp = hrt_absolute_time();
+				// 	rates_sp.timestamp = hrt_absolute_time();
 
-					// only copy att rates sp if message contained new data
+				// 	// only copy att rates sp if message contained new data
-					if (!ignore_bodyrate_msg_x) {
+				// 	if (!ignore_bodyrate_msg_x) {
-						rates_sp.roll = set_attitude_target.body_roll_rate;
+				// 		rates_sp.roll = set_attitude_target.body_roll_rate;
-					}
+				// 	}
 
-					if (!ignore_bodyrate_msg_y) {
+				// 	if (!ignore_bodyrate_msg_y) {
-						rates_sp.pitch = set_attitude_target.body_pitch_rate;
+				// 		rates_sp.pitch = set_attitude_target.body_pitch_rate;
-					}
+				// 	}
 
-					if (!ignore_bodyrate_msg_z) {
+				// 	if (!ignore_bodyrate_msg_z) {
-						rates_sp.yaw = set_attitude_target.body_yaw_rate;
+				// 		rates_sp.yaw = set_attitude_target.body_yaw_rate;
-					}
+				// 	}
 
-					if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid
+				// 	if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid
-						fill_thrust(rates_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
+				// 		fill_thrust(rates_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
-					}
+				// 	}
 
-					_rates_sp_pub.publish(rates_sp);
+				// 	_rates_sp_pub.publish(rates_sp);
-				}
+				// }
 			}
 		}
 	}

@@ -52,6 +52,8 @@
 #include <lib/drivers/barometer/PX4Barometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
+#include <lib/mixer_module/aviata_mixers.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
@@ -102,6 +104,9 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_trajectory_bezier.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/aviata_finalize_docking.h>
+#include <uORB/topics/aviata_set_configuration.h>
+#include <uORB/topics/aviata_set_standalone.h>
 
 class Mavlink;
 
@@ -261,6 +266,10 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<vehicle_trajectory_bezier_s>	_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
 	uORB::Publication<vehicle_trajectory_waypoint_s>	_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
 
+	uORB::Publication<aviata_finalize_docking_s>		_aviata_finalize_docking_pub{ORB_ID(aviata_finalize_docking)};
+	uORB::Publication<aviata_set_configuration_s>		_aviata_set_configuration_pub{ORB_ID(aviata_set_configuration)};
+	uORB::Publication<aviata_set_standalone_s>		_aviata_set_standalone_pub{ORB_ID(aviata_set_standalone)};
+
 	// ORB publications (multi)
 	uORB::PublicationMulti<distance_sensor_s>		_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
 	uORB::PublicationMulti<distance_sensor_s>		_flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
@@ -282,6 +291,8 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Subscription	_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription	_vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription	_aviata_finalize_docking_sub{ORB_ID(aviata_finalize_docking)};
+	uORB::Subscription	_aviata_set_standalone_sub{ORB_ID(aviata_set_standalone)};
 
 	// hil_sensor and hil_state_quaternion
 	enum SensorSource {

@@ -59,7 +59,7 @@ MulticopterHoverThrustEstimator::~MulticopterHoverThrustEstimator()
 
 bool MulticopterHoverThrustEstimator::init()
 {
-	if (!_vehicle_local_position_setpoint_sub.registerCallback()) {
+	if (!_att_sp_sub.registerCallback()) {
 		PX4_ERR("vehicle_local_position_setpoint callback registration failed!");
 		return false;
 	}
@@ -91,13 +91,25 @@ void MulticopterHoverThrustEstimator::updateParams()
 void MulticopterHoverThrustEstimator::Run()
 {
 	if (should_exit()) {
-		_vehicle_local_position_setpoint_sub.unregisterCallback();
+		_att_sp_sub.unregisterCallback();
 		exit_and_cleanup();
 		return;
 	}
 
-	// new local position estimate and setpoint needed every iteration
+	// AVIATA modification. Check for local position setpoint OR attitude setpoint. This is so drones in offboard attitude control will estimate hover thrust.
-	if (!_vehicle_local_pos_sub.updated() || !_vehicle_local_position_setpoint_sub.updated()) {
+	float z_thrust_sp;
+	vehicle_local_position_setpoint_s local_pos_sp;
+	vehicle_attitude_setpoint_s att_sp;
+	if (_vehicle_local_pos_sub.updated()) {
+		if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) {
+			z_thrust_sp = local_pos_sp.thrust[2];
+		} else if (_att_sp_sub.update(&att_sp)) {
+			matrix::Vector3f body_z = matrix::Quatf(att_sp.q_d).dcm_z();
+			z_thrust_sp = att_sp.thrust_body[2] * body_z(2);
+		} else {
+			return;
+		}
+	} else {
 		return;
 	}
 
@@ -149,21 +161,17 @@ void MulticopterHoverThrustEstimator::Run()
 
 		_hover_thrust_ekf.predict(dt);
 
-		vehicle_local_position_setpoint_s local_pos_sp;
+		if (PX4_ISFINITE(z_thrust_sp)) {
+			// Inform the hover thrust estimator about the measured vertical
+			// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
+			ZeroOrderHoverThrustEkf::status status;
+			_hover_thrust_ekf.fuseAccZ(-local_pos.az, -z_thrust_sp, status);
 
-		if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) {
+			const bool valid = _in_air && (status.hover_thrust_var < 0.001f) && (status.innov_test_ratio < 1.f);
-			if (PX4_ISFINITE(local_pos_sp.thrust[2])) {
+			_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
-				// Inform the hover thrust estimator about the measured vertical
+			_valid = _valid_hysteresis.get_state();
-				// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
+
-				ZeroOrderHoverThrustEkf::status status;
+			publishStatus(local_pos.timestamp, status);
-				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status);
-
-				const bool valid = _in_air && (status.hover_thrust_var < 0.001f) && (status.innov_test_ratio < 1.f);
-				_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
-				_valid = _valid_hysteresis.get_state();
-
-				publishStatus(local_pos.timestamp, status);
-			}
 		}
 
 	} else {

@@ -58,6 +58,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
 
 #include "zero_order_hover_thrust_ekf.hpp"
 
@@ -95,12 +96,13 @@ class MulticopterHoverThrustEstimator : public ModuleBase<MulticopterHoverThrust
 
 	uORB::Publication<hover_thrust_estimate_s> _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)};
 
-	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_setpoint_sub{this, ORB_ID(vehicle_local_position_setpoint)};
+	uORB::SubscriptionCallbackWorkItem _att_sp_sub{this, ORB_ID(vehicle_attitude_setpoint)};
 
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_local_pos_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
 
 	hrt_abstime _timestamp_last{0};
 

@@ -152,32 +152,32 @@ void PositionControl::_velocityControl(const float dt)
 
 	_accelerationControl();
 
-	// Integrator anti-windup in vertical direction
+	// // Integrator anti-windup in vertical direction
-	if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) ||
+	// if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) ||
-	    (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) {
+	//     (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) {
-		vel_error(2) = 0.f;
+	// 	vel_error(2) = 0.f;
-	}
+	// }
-
+
-	// Saturate maximal vertical thrust
+	// // Saturate maximal vertical thrust
-	_thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);
+	// _thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);
-
+
-	// Get allowed horizontal thrust after prioritizing vertical control
+	// // Get allowed horizontal thrust after prioritizing vertical control
-	const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
+	// const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
-	const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
+	// const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
-	const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared;
+	// const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared;
-	float thrust_max_xy = 0;
+	// float thrust_max_xy = 0;
-
+
-	if (thrust_max_xy_squared > 0) {
+	// if (thrust_max_xy_squared > 0) {
-		thrust_max_xy = sqrtf(thrust_max_xy_squared);
+	// 	thrust_max_xy = sqrtf(thrust_max_xy_squared);
-	}
+	// }
-
+
-	// Saturate thrust in horizontal direction
+	// // Saturate thrust in horizontal direction
-	const Vector2f thrust_sp_xy(_thr_sp);
+	// const Vector2f thrust_sp_xy(_thr_sp);
-	const float thrust_sp_xy_norm = thrust_sp_xy.norm();
+	// const float thrust_sp_xy_norm = thrust_sp_xy.norm();
-
+
-	if (thrust_sp_xy_norm > thrust_max_xy) {
+	// if (thrust_sp_xy_norm > thrust_max_xy) {
-		_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
+	// 	_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
-	}
+	// }
 
 	// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
 	// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990

@@ -365,13 +365,13 @@ MulticopterPositionControl::parameters_update(bool force)
 			mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed");
 		}
 
-		if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
+		// if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
-		    _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
+		//     _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
-			_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
+		// 	_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
-						 _param_mpc_thr_max.get()));
+		// 				 _param_mpc_thr_max.get()));
-			_param_mpc_thr_hover.commit();
+		// 	_param_mpc_thr_hover.commit();
-			mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max");
+		// 	mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max");
-		}
+		// }
 
 		if (!_param_mpc_use_hte.get() || !_hover_thrust_initialized) {
 			_control.setHoverThrust(_param_mpc_thr_hover.get());
	@@ -0,0 +1 @@
			uint64 timestamp # time since system start (microseconds)