control_allocator: add fixed-wing actuator effectiveness

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ActuatorEffectivenessFixedWing.hpp"
+#include <ControlAllocation/ControlAllocation.hpp>
+
+using namespace matrix;
+
+ActuatorEffectivenessFixedWing::ActuatorEffectivenessFixedWing(ModuleParams *parent)
+	: ModuleParams(parent), _rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::FixedForward),
+	  _control_surfaces(this)
+{
+}
+
+bool
+ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configuration, bool force)
+{
+	if (!force) {
+		return false;
+	}
+
+	// Motors
+	_rotors.enableYawControl(false);
+	_rotors.getEffectivenessMatrix(configuration, true);
+
+	// Control Surfaces
+	_first_control_surface_idx = configuration.num_actuators_matrix[0];
+	_control_surfaces.getEffectivenessMatrix(configuration, true);
+
+	return true;
+}
+
+void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp)
+{
+	// apply flaps
+	actuator_controls_s actuator_controls_0;
+
+	if (_actuator_controls_0_sub.copy(&actuator_controls_0)) {
+		float control_flaps = actuator_controls_0.control[actuator_controls_s::INDEX_FLAPS];
+		float airbrakes_control = actuator_controls_0.control[actuator_controls_s::INDEX_AIRBRAKES];
+		_control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp);
+	}
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+#include "ActuatorEffectivenessRotors.hpp"
+#include "ActuatorEffectivenessControlSurfaces.hpp"
+
+#include <uORB/topics/actuator_controls.h>
+
+class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	ActuatorEffectivenessFixedWing(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessFixedWing() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+
+	const char *name() const override { return "Fixed Wing"; }
+
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp) override;
+
+private:
+	ActuatorEffectivenessRotors _rotors;
+	ActuatorEffectivenessControlSurfaces _control_surfaces;
+
+	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+
+	int _first_control_surface_idx{0}; ///< applies to matrix 1
+};

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -36,6 +36,8 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectiveness.hpp
 	ActuatorEffectivenessControlSurfaces.cpp
 	ActuatorEffectivenessControlSurfaces.hpp
+	ActuatorEffectivenessFixedWing.cpp
+	ActuatorEffectivenessFixedWing.hpp
 	ActuatorEffectivenessTilts.cpp
 	ActuatorEffectivenessTilts.hpp
 	ActuatorEffectivenessRotors.cpp

src/modules/control_allocator/ControlAllocator.cpp

@@ -199,6 +199,10 @@ ControlAllocator::update_effectiveness_source()
 			tmp = new ActuatorEffectivenessRoverDifferential();
 			break;
 
+		case EffectivenessSource::FIXED_WING:
+			tmp = new ActuatorEffectivenessFixedWing(this);
+			break;
+
 		default:
 			PX4_ERR("Unknown airframe");
 			break;

src/modules/control_allocator/ControlAllocator.hpp

@@ -47,6 +47,7 @@
 #include <ActuatorEffectivenessTiltrotorVTOL.hpp>
 #include <ActuatorEffectivenessRoverAckermann.hpp>
 #include <ActuatorEffectivenessRoverDifferential.hpp>
+#include <ActuatorEffectivenessFixedWing.hpp>
 
 #include <ControlAllocation.hpp>
 #include <ControlAllocationPseudoInverse.hpp>
@@ -125,6 +126,7 @@ private:
 		TILTROTOR_VTOL = 2,
 		ROVER_ACKERMANN = 3,
 		ROVER_DIFFERENTIAL = 4,
+		FIXED_WING = 5,
 	};
 
 	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};

src/modules/control_allocator/module.yaml

@@ -20,6 +20,7 @@ parameters:
                 2: Tiltrotor VTOL (WIP)
                 3: Rover (Ackermann)
                 4: Rover (Differential)
+                5: Fixed Wing
             default: 0
 
         CA_METHOD:
@@ -492,5 +493,31 @@ mixer:
                       - name: 'Right Motor'
                         position: [ 0, 1., 0 ]
 
+            5: # Fixed Wing
+                actuators:
+                  - actuator_type: 'motor'
+                    group_label: 'Motors'
+                    count: 'CA_ROTOR_COUNT'
+                    per_item_parameters:
+                        standard:
+                            position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ]
+                  - actuator_type: 'servo'
+                    group_label: 'Control Surfaces'
+                    count: 'CA_SV_CS_COUNT'
+                    per_item_parameters:
+                        extra:
+                          - name: 'CA_SV_CS${i}_TYPE'
+                            label: 'Type'
+                            function: 'type'
+                            identifier: 'servo-type'
+                          - name: 'CA_SV_CS${i}_TRQ_R'
+                            label: 'Roll Torque'
+                            identifier: 'servo-torque-roll'
+                          - name: 'CA_SV_CS${i}_TRQ_P'
+                            label: 'Pitch Torque'
+                            identifier: 'servo-torque-pitch'
+                          - name: 'CA_SV_CS${i}_TRQ_Y'
+                            label: 'Yaw Torque'
+                            identifier: 'servo-torque-yaw'