mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 19:23:34 +08:00
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>
This commit is contained in:
Julien Lecoeur
committed by
Daniel Agar
Daniel Agar
co-authored by
Silvan Fuhrer
Roman Bapst
Silvan Fuhrer
Roman Bapst
parent
fc6b61dad1
commit
343cf5603e
@@ -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
|
||||
};
|
||||
+201
@@ -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(¶m_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);
|
||||
}
|
||||
+163
@@ -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
|
||||
)
|
||||
};
|
||||
+560
@@ -0,0 +1,560 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 ActuatorEffectivenessMultirotorParams.c
|
||||
*
|
||||
* Parameters for the actuator effectiveness of multirotors.
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Position of rotor 0 along X body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R0_PX, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 0 along Y body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R0_PY, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 0 along Z body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R0_PZ, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 0 thrust vector, X body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R0_AX, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 0 thrust vector, Y body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R0_AY, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 0 thrust vector, Z body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R0_AZ, -1.0);
|
||||
|
||||
/**
|
||||
* Thrust coefficient of rotor 0
|
||||
*
|
||||
* The thrust coefficient if defined as Thrust = CT * u^2,
|
||||
* where u (with value between CA_ACT0_MIN and CA_ACT0_MAX)
|
||||
* is the output signal sent to the motor controller.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R0_CT, 0.0);
|
||||
|
||||
/**
|
||||
* Moment coefficient of rotor 0
|
||||
*
|
||||
* The moment coefficient if defined as Torque = KM * Thrust
|
||||
*
|
||||
* Use a positive value for a rotor with CCW rotation.
|
||||
* Use a negative value for a rotor with CW rotation.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R0_KM, 0.05);
|
||||
|
||||
/**
|
||||
* Position of rotor 1 along X body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R1_PX, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 1 along Y body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R1_PY, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 1 along Z body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R1_PZ, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 1 thrust vector, X body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R1_AX, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 1 thrust vector, Y body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R1_AY, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 1 thrust vector, Z body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R1_AZ, -1.0);
|
||||
|
||||
/**
|
||||
* Thrust coefficient of rotor 1
|
||||
*
|
||||
* The thrust coefficient if defined as Thrust = CT * u^2,
|
||||
* where u (with value between CA_ACT1_MIN and CA_ACT1_MAX)
|
||||
* is the output signal sent to the motor controller.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R1_CT, 0.0);
|
||||
|
||||
/**
|
||||
* Moment coefficient of rotor 1
|
||||
*
|
||||
* The moment coefficient if defined as Torque = KM * Thrust,
|
||||
*
|
||||
* Use a positive value for a rotor with CCW rotation.
|
||||
* Use a negative value for a rotor with CW rotation.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R1_KM, 0.05);
|
||||
|
||||
/**
|
||||
* Position of rotor 2 along X body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R2_PX, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 2 along Y body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R2_PY, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 2 along Z body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R2_PZ, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 2 thrust vector, X body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R2_AX, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 2 thrust vector, Y body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R2_AY, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 2 thrust vector, Z body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R2_AZ, -1.0);
|
||||
|
||||
/**
|
||||
* Thrust coefficient of rotor 2
|
||||
*
|
||||
* The thrust coefficient if defined as Thrust = CT * u^2,
|
||||
* where u (with value between CA_ACT2_MIN and CA_ACT2_MAX)
|
||||
* is the output signal sent to the motor controller.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R2_CT, 0.0);
|
||||
|
||||
/**
|
||||
* Moment coefficient of rotor 2
|
||||
*
|
||||
* The moment coefficient if defined as Torque = KM * Thrust
|
||||
*
|
||||
* Use a positive value for a rotor with CCW rotation.
|
||||
* Use a negative value for a rotor with CW rotation.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R2_KM, 0.05);
|
||||
|
||||
/**
|
||||
* Position of rotor 3 along X body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R3_PX, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 3 along Y body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R3_PY, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 3 along Z body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R3_PZ, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 3 thrust vector, X body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R3_AX, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 3 thrust vector, Y body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R3_AY, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 3 thrust vector, Z body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R3_AZ, -1.0);
|
||||
|
||||
/**
|
||||
* Thrust coefficient of rotor 3
|
||||
*
|
||||
* The thrust coefficient if defined as Thrust = CT * u^2,
|
||||
* where u (with value between CA_ACT3_MIN and CA_ACT3_MAX)
|
||||
* is the output signal sent to the motor controller.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R3_CT, 0.0);
|
||||
|
||||
/**
|
||||
* Moment coefficient of rotor 3
|
||||
*
|
||||
* The moment coefficient if defined as Torque = KM * Thrust
|
||||
*
|
||||
* Use a positive value for a rotor with CCW rotation.
|
||||
* Use a negative value for a rotor with CW rotation.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R3_KM, 0.05);
|
||||
|
||||
/**
|
||||
* Position of rotor 4 along X body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R4_PX, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 4 along Y body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R4_PY, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 4 along Z body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R4_PZ, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 4 thrust vector, X body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R4_AX, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 4 thrust vector, Y body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R4_AY, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 4 thrust vector, Z body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R4_AZ, -1.0);
|
||||
|
||||
/**
|
||||
* Thrust coefficient of rotor 4
|
||||
*
|
||||
* The thrust coefficient if defined as Thrust = CT * u^2,
|
||||
* where u (with value between CA_ACT4_MIN and CA_ACT4_MAX)
|
||||
* is the output signal sent to the motor controller.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R4_CT, 0.0);
|
||||
|
||||
/**
|
||||
* Moment coefficient of rotor 4
|
||||
*
|
||||
* The moment coefficient if defined as Torque = KM * Thrust
|
||||
*
|
||||
* Use a positive value for a rotor with CCW rotation.
|
||||
* Use a negative value for a rotor with CW rotation.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R4_KM, 0.05);
|
||||
|
||||
/**
|
||||
* Position of rotor 5 along X body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R5_PX, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 5 along Y body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R5_PY, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 5 along Z body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R5_PZ, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 5 thrust vector, X body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R5_AX, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 5 thrust vector, Y body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R5_AY, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 5 thrust vector, Z body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R5_AZ, -1.0);
|
||||
|
||||
/**
|
||||
* Thrust coefficient of rotor 5
|
||||
*
|
||||
* The thrust coefficient if defined as Thrust = CT * u^2,
|
||||
* where u (with value between CA_ACT5_MIN and CA_ACT5_MAX)
|
||||
* is the output signal sent to the motor controller.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R5_CT, 0.0);
|
||||
|
||||
/**
|
||||
* Moment coefficient of rotor 5
|
||||
*
|
||||
* The moment coefficient if defined as Torque = KM * Thrust
|
||||
*
|
||||
* Use a positive value for a rotor with CCW rotation.
|
||||
* Use a negative value for a rotor with CW rotation.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R5_KM, 0.05);
|
||||
|
||||
/**
|
||||
* Position of rotor 6 along X body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R6_PX, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 6 along Y body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R6_PY, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 6 along Z body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R6_PZ, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 6 thrust vector, X body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R6_AX, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 6 thrust vector, Y body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R6_AY, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 6 thrust vector, Z body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R6_AZ, -1.0);
|
||||
|
||||
/**
|
||||
* Thrust coefficient of rotor 6
|
||||
*
|
||||
* The thrust coefficient if defined as Thrust = CT * u^2,
|
||||
* where u (with value between CA_ACT6_MIN and CA_ACT6_MAX)
|
||||
* is the output signal sent to the motor controller.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R6_CT, 0.0);
|
||||
|
||||
/**
|
||||
* Moment coefficient of rotor 6
|
||||
*
|
||||
* The moment coefficient if defined as Torque = KM * Thrust
|
||||
*
|
||||
* Use a positive value for a rotor with CCW rotation.
|
||||
* Use a negative value for a rotor with CW rotation.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R6_KM, 0.05);
|
||||
|
||||
/**
|
||||
* Position of rotor 7 along X body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R7_PX, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 7 along Y body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R7_PY, 0.0);
|
||||
|
||||
/**
|
||||
* Position of rotor 7 along Z body axis
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R7_PZ, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 7 thrust vector, X body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R7_AX, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 7 thrust vector, Y body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R7_AY, 0.0);
|
||||
|
||||
/**
|
||||
* Axis of rotor 7 thrust vector, Z body axis component
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R7_AZ, -1.0);
|
||||
|
||||
/**
|
||||
* Thrust coefficient of rotor 7
|
||||
*
|
||||
* The thrust coefficient if defined as Thrust = CT * u^2,
|
||||
* where u (with value between CA_ACT7_MIN and CA_ACT7_MAX)
|
||||
* is the output signal sent to the motor controller.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R7_CT, 0.0);
|
||||
|
||||
/**
|
||||
* Moment coefficient of rotor 7
|
||||
*
|
||||
* The moment coefficient if defined as Torque = KM * Thrust
|
||||
*
|
||||
* Use a positive value for a rotor with CCW rotation.
|
||||
* Use a negative value for a rotor with CW rotation.
|
||||
*
|
||||
* @group Control Allocation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(CA_MC_R7_KM, 0.05);
|
||||
+102
@@ -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);
|
||||
}
|
||||
+108
@@ -0,0 +1,108 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 ActuatorEffectivenessStandardVTOL.hpp
|
||||
*
|
||||
* Actuator effectiveness for standard VTOL
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ActuatorEffectivenessStandardVTOL.hpp"
|
||||
|
||||
ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
|
||||
{
|
||||
setFlightPhase(FlightPhase::HOVER_FLIGHT);
|
||||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessStandardVTOL::update()
|
||||
{
|
||||
if (_updated) {
|
||||
_updated = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
|
||||
{
|
||||
ActuatorEffectiveness::setFlightPhase(flight_phase);
|
||||
|
||||
_updated = true;
|
||||
|
||||
switch (_flight_phase) {
|
||||
case FlightPhase::HOVER_FLIGHT: {
|
||||
const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
|
||||
{-0.5f, 0.5f, 0.5f, -0.5f, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.25f, 0.25f, -0.25f, -0.25f, 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.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.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
|
||||
};
|
||||
_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
|
||||
break;
|
||||
}
|
||||
|
||||
case FlightPhase::FORWARD_FLIGHT: {
|
||||
const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, -0.5f, 0.5f, 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.5f, 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.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}
|
||||
};
|
||||
_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
|
||||
break;
|
||||
}
|
||||
|
||||
case FlightPhase::TRANSITION_HF_TO_FF:
|
||||
case FlightPhase::TRANSITION_FF_TO_HF: {
|
||||
const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
|
||||
{ -0.5f, 0.5f, 0.5f, -0.5f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.25f, 0.25f, -0.25f, -0.25f, 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.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.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
|
||||
};
|
||||
_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
+68
@@ -0,0 +1,68 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 ActuatorEffectivenessStandardVTOL.hpp
|
||||
*
|
||||
* Actuator effectiveness for standard VTOL
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
|
||||
class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
ActuatorEffectivenessStandardVTOL();
|
||||
virtual ~ActuatorEffectivenessStandardVTOL() = default;
|
||||
|
||||
/**
|
||||
* Update effectiveness matrix
|
||||
*
|
||||
* @return True if the effectiveness matrix has changed
|
||||
*/
|
||||
virtual bool update() override;
|
||||
|
||||
/**
|
||||
* Set the current flight phase
|
||||
*
|
||||
* @param Flight phase
|
||||
*/
|
||||
void setFlightPhase(const FlightPhase &flight_phase) override;
|
||||
|
||||
protected:
|
||||
bool _updated{false};
|
||||
};
|
||||
+116
@@ -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 ActuatorEffectivenessTiltrotorVTOL.hpp
|
||||
*
|
||||
* Actuator effectiveness for tiltrotor VTOL
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ActuatorEffectivenessTiltrotorVTOL.hpp"
|
||||
|
||||
ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
|
||||
{
|
||||
setFlightPhase(FlightPhase::HOVER_FLIGHT);
|
||||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessTiltrotorVTOL::update()
|
||||
{
|
||||
if (_updated) {
|
||||
_updated = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
|
||||
{
|
||||
ActuatorEffectiveness::setFlightPhase(flight_phase);
|
||||
|
||||
_updated = true;
|
||||
|
||||
// Trim
|
||||
float tilt = 0.0f;
|
||||
|
||||
switch (_flight_phase) {
|
||||
case FlightPhase::HOVER_FLIGHT: {
|
||||
tilt = 0.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
case FlightPhase::FORWARD_FLIGHT: {
|
||||
tilt = 1.5f;
|
||||
break;
|
||||
}
|
||||
|
||||
case FlightPhase::TRANSITION_FF_TO_HF:
|
||||
case FlightPhase::TRANSITION_HF_TO_FF: {
|
||||
tilt = 1.0f;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Trim: half throttle, tilted motors
|
||||
_trim(0) = 0.5f;
|
||||
_trim(1) = 0.5f;
|
||||
_trim(2) = 0.5f;
|
||||
_trim(3) = 0.5f;
|
||||
_trim(4) = tilt;
|
||||
_trim(5) = tilt;
|
||||
_trim(6) = tilt;
|
||||
_trim(7) = tilt;
|
||||
|
||||
// Effectiveness
|
||||
const float tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = {
|
||||
{-0.5f * cosf(_trim(4)), 0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), 0.5f * _trim(0) *sinf(_trim(4)), -0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.5f * cosf(_trim(4)), -0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), -0.5f * _trim(0) *sinf(_trim(4)), 0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{-0.5f * sinf(_trim(4)), 0.5f * sinf(_trim(5)), 0.5f * sinf(_trim(6)), -0.5f * sinf(_trim(7)), -0.5f * _trim(0) *cosf(_trim(4)), 0.5f * _trim(1) *cosf(_trim(5)), 0.5f * _trim(2) *cosf(_trim(6)), -0.5f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.25f * sinf(_trim(4)), 0.25f * sinf(_trim(5)), 0.25f * sinf(_trim(6)), 0.25f * sinf(_trim(7)), 0.25f * _trim(0) *cosf(_trim(4)), 0.25f * _trim(1) *cosf(_trim(5)), 0.25f * _trim(2) *cosf(_trim(6)), 0.25f * _trim(3) *cosf(_trim(7)), 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.25f * cosf(_trim(4)), -0.25f * cosf(_trim(5)), -0.25f * cosf(_trim(6)), -0.25f * cosf(_trim(7)), 0.25f * _trim(0) *sinf(_trim(4)), 0.25f * _trim(1) *sinf(_trim(5)), 0.25f * _trim(2) *sinf(_trim(6)), 0.25f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
|
||||
};
|
||||
_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(tiltrotor_vtol);
|
||||
|
||||
// Temporarily disable a few controls (WIP)
|
||||
for (size_t j = 4; j < 8; j++) {
|
||||
_effectiveness(3, j) = 0.0f;
|
||||
_effectiveness(4, j) = 0.0f;
|
||||
_effectiveness(5, j) = 0.0f;
|
||||
}
|
||||
|
||||
}
|
||||
+68
@@ -0,0 +1,68 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 ActuatorEffectivenessTiltrotorVTOL.hpp
|
||||
*
|
||||
* Actuator effectiveness for tiltrotor VTOL
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
|
||||
class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
ActuatorEffectivenessTiltrotorVTOL();
|
||||
virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
|
||||
|
||||
/**
|
||||
* Update effectiveness matrix
|
||||
*
|
||||
* @return True if the effectiveness matrix has changed
|
||||
*/
|
||||
virtual bool update() override;
|
||||
|
||||
/**
|
||||
* Set the current flight phase
|
||||
*
|
||||
* @param Flight phase
|
||||
*/
|
||||
void setFlightPhase(const FlightPhase &flight_phase) override;
|
||||
|
||||
protected:
|
||||
bool _updated{false};
|
||||
};
|
||||
@@ -0,0 +1,52 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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(ActuatorEffectiveness
|
||||
ActuatorEffectiveness.hpp
|
||||
ActuatorEffectivenessMultirotor.cpp
|
||||
ActuatorEffectivenessMultirotor.hpp
|
||||
ActuatorEffectivenessStandardVTOL.cpp
|
||||
ActuatorEffectivenessStandardVTOL.hpp
|
||||
ActuatorEffectivenessTiltrotorVTOL.cpp
|
||||
ActuatorEffectivenessTiltrotorVTOL.hpp
|
||||
)
|
||||
|
||||
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(ActuatorEffectiveness
|
||||
PRIVATE
|
||||
mathlib
|
||||
ControlAllocation
|
||||
)
|
||||
|
||||
# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)
|
||||
Reference in New Issue
Block a user