fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 11:00:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

control_allocator: add support for Tailsitter VTOL

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-12-08 09:23:33 +01:00
committed by Daniel Agar
parent 0568cff299
commit 07306c4be3
15 changed files with 340 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp
+89
View File
@@ -0,0 +1,89 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ActuatorEffectivenessTailsitterVTOL.hpp
*
* Actuator effectiveness for tailsitter VTOL
*/
#include "ActuatorEffectivenessTailsitterVTOL.hpp"
using namespace matrix;
ActuatorEffectivenessTailsitterVTOL::ActuatorEffectivenessTailsitterVTOL(ModuleParams *parent)
: ModuleParams(parent), _mc_rotors(this), _control_surfaces(this)
{
setFlightPhase(FlightPhase::HOVER_FLIGHT);
}
bool
ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
{
if (!force) {
return false;
}
// MC motors
configuration.selected_matrix = 0;
_mc_rotors.enableYawControl(true); //TODO enable yaw with elevons
_mc_rotors.getEffectivenessMatrix(configuration, true);
// Control Surfaces
configuration.selected_matrix = 1;
_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
_control_surfaces.getEffectivenessMatrix(configuration, true);
return true;
}
void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase)
{
if (_flight_phase == flight_phase) {
return;
}
ActuatorEffectiveness::setFlightPhase(flight_phase);
// update stopped motors //TODO: add option to switch off certain motors in FW
switch (flight_phase) {
case FlightPhase::FORWARD_FLIGHT:
_stopped_motors = 0;
break;
case FlightPhase::HOVER_FLIGHT:
case FlightPhase::TRANSITION_FF_TO_HF:
case FlightPhase::TRANSITION_HF_TO_FF:
_stopped_motors = 0;
break;
}
}
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp
+81
View File
@@ -0,0 +1,81 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ActuatorEffectivenessTailsitterVTOL.hpp
*
* Actuator effectiveness for tailsitter VTOL
*/
#pragma once
#include "ActuatorEffectiveness.hpp"
#include "ActuatorEffectivenessRotors.hpp"
#include "ActuatorEffectivenessControlSurfaces.hpp"
#include <uORB/topics/actuator_controls.h>
#include <uORB/Subscription.hpp>
class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public ActuatorEffectiveness
{
public:
ActuatorEffectivenessTailsitterVTOL(ModuleParams *parent);
virtual ~ActuatorEffectivenessTailsitterVTOL() = default;
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
int numMatrices() const override { return 2; }
void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
{
static_assert(MAX_NUM_MATRICES >= 2, "expecting at least 2 matrices");
allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
}
void setFlightPhase(const FlightPhase &flight_phase) override;
const char *name() const override { return "VTOL Tailsitter"; }
uint32_t getStoppedMotors() const override { return _stopped_motors; }
protected:
bool _updated{true};
ActuatorEffectivenessRotors _mc_rotors;
ActuatorEffectivenessControlSurfaces _control_surfaces;
uint32_t _stopped_motors{}; ///< currently stopped motors
int _first_control_surface_idx{0}; ///< applies to matrix 1
uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
};
src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt
+2
View File
@@ -46,6 +46,8 @@ px4_add_library(ActuatorEffectiveness
ActuatorEffectivenessStandardVTOL.hpp
ActuatorEffectivenessTiltrotorVTOL.cpp
ActuatorEffectivenessTiltrotorVTOL.hpp
ActuatorEffectivenessTailsitterVTOL.cpp
ActuatorEffectivenessTailsitterVTOL.hpp
ActuatorEffectivenessRoverAckermann.hpp
ActuatorEffectivenessRoverAckermann.cpp
ActuatorEffectivenessRoverDifferential.hpp
src/modules/control_allocator/ControlAllocator.cpp
+4
View File
@@ -215,6 +215,10 @@ ControlAllocator::update_effectiveness_source()
tmp = new ActuatorEffectivenessTiltrotorVTOL(this);
break;
case EffectivenessSource::TAILSITTER_VTOL:
tmp = new ActuatorEffectivenessTailsitterVTOL(this);
break;
case EffectivenessSource::ROVER_ACKERMANN:
tmp = new ActuatorEffectivenessRoverAckermann();
break;
src/modules/control_allocator/ControlAllocator.hpp
+8 -6
View File
@@ -45,6 +45,7 @@
#include <ActuatorEffectivenessRotors.hpp>
#include <ActuatorEffectivenessStandardVTOL.hpp>
#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
#include <ActuatorEffectivenessTailsitterVTOL.hpp>
#include <ActuatorEffectivenessRoverAckermann.hpp>
#include <ActuatorEffectivenessRoverDifferential.hpp>
#include <ActuatorEffectivenessFixedWing.hpp>
@@ -136,12 +137,13 @@ private:
enum class EffectivenessSource {
NONE = -1,
MULTIROTOR = 0,
STANDARD_VTOL = 1,
TILTROTOR_VTOL = 2,
ROVER_ACKERMANN = 3,
ROVER_DIFFERENTIAL = 4,
FIXED_WING = 5,
MOTORS_6DOF = 6,
FIXED_WING = 1,
STANDARD_VTOL = 2,
TILTROTOR_VTOL = 3,
TAILSITTER_VTOL = 4,
ROVER_ACKERMANN = 5,
ROVER_DIFFERENTIAL = 6,
MOTORS_6DOF = 7,
};
EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
src/modules/control_allocator/module.yaml
+74 -43
View File
@@ -16,12 +16,13 @@ parameters:
type: enum
values:
0: Multirotor
1: Standard VTOL (WIP)
2: Tiltrotor VTOL (WIP)
3: Rover (Ackermann)
4: Rover (Differential)
5: Fixed Wing
5: Motors (6DOF)
1: Fixed-wing
2: Standard VTOL
3: Tiltrotor VTOL
4: Tailsitter VTOL
5: Rover (Ackermann)
6: Rover (Differential)
7: Motors (6DOF)
default: 0
CA_METHOD:
@@ -449,7 +450,36 @@ mixer:
function: 'spin-dir'
show_as: 'true-if-positive'
1: # Standard VTOL
1: # 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'
- name: 'CA_SV_CS${i}_TRIM'
label: 'Trim'
2: # Standard VTOL
actuators:
- actuator_type: 'motor'
group_label: 'MC Motors'
@@ -487,7 +517,7 @@ mixer:
- name: 'CA_SV_CS${i}_TRIM'
label: 'Trim'
2: # Tiltrotor VTOL
3: # Tiltrotor VTOL
actuators:
- actuator_type: 'motor'
group_label: 'MC Motors'
@@ -509,13 +539,13 @@ mixer:
function: 'type'
identifier: 'servo-type'
- name: 'CA_SV_CS${i}_TRQ_R'
label: 'Roll Torque'
label: 'Roll Scale'
identifier: 'servo-torque-roll'
- name: 'CA_SV_CS${i}_TRQ_P'
label: 'Pitch Torque'
label: 'Pitch Scale'
identifier: 'servo-torque-pitch'
- name: 'CA_SV_CS${i}_TRQ_Y'
label: 'Yaw Torque'
label: 'Yaw Scale'
identifier: 'servo-torque-yaw'
- name: 'CA_SV_CS${i}_TRIM'
label: 'Trim'
@@ -534,7 +564,37 @@ mixer:
- name: 'CA_SV_TL${i}_CT'
label: 'Use for Control'
3: # Rover (Ackermann)
4: # Tailsitter VTOL
actuators:
- actuator_type: 'motor'
group_label: 'MC 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'
item_label_prefix: 'Surface ${i}'
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 Scale'
identifier: 'servo-torque-roll'
- name: 'CA_SV_CS${i}_TRQ_P'
label: 'Pitch Scale'
identifier: 'servo-torque-pitch'
- name: 'CA_SV_CS${i}_TRQ_Y'
label: 'Yaw Scale'
identifier: 'servo-torque-yaw'
- name: 'CA_SV_CS${i}_TRIM'
label: 'Trim'
5: # Rover (Ackermann)
actuators:
- actuator_type: 'motor'
instances:
@@ -545,7 +605,7 @@ mixer:
- name: 'Steering'
position: [ 1., 0, 0 ]
4: # Rover (Differential)
6: # Rover (Differential)
actuators:
- actuator_type: 'motor'
instances:
@@ -554,36 +614,7 @@ 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'
- name: 'CA_SV_CS${i}_TRIM'
label: 'Trim'
6: # Motors (6DOF)
7: # Motors (6DOF)
actuators:
- actuator_type: 'motor'
count: 'CA_ROTOR_COUNT'