forked from Archive/PX4-Autopilot
control_allocator: add fixed-wing actuator effectiveness
This commit is contained in:
Beat Küng
Daniel Agar
parent
4d2a403afa
commit
8d9e2a28c4
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
};
|
||||
|
|
@ -36,6 +36,8 @@ px4_add_library(ActuatorEffectiveness
|
|||
ActuatorEffectiveness.hpp
|
||||
ActuatorEffectivenessControlSurfaces.cpp
|
||||
ActuatorEffectivenessControlSurfaces.hpp
|
||||
ActuatorEffectivenessFixedWing.cpp
|
||||
ActuatorEffectivenessFixedWing.hpp
|
||||
ActuatorEffectivenessTilts.cpp
|
||||
ActuatorEffectivenessTilts.hpp
|
||||
ActuatorEffectivenessRotors.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;
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
|
|
|
|||
|
|
@ -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'
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue