mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 19:03:33 +08:00
Rework flaps/spoilers logic
- remove deprecated actuator_controls[INDEX_FLAPS/SPOILERS/AIRBRAKES] - use new topic normalized_unsigned_setpoint.msg (with instances flaps_setpoint and spoilers_setpoint) to pass into control allocation - remove flaps/spoiler related fields from attitude_setpoint topic - CA: add possibility to map flaps/spoilers to any control surface - move flaps/spoiler pitch trimming to CA (previously called DTRIM_FLAPS/SPOILER) - move manual flaps/spoiler handling from rate to attitude controller FW Position controller: change how negative switch readings are intepreted for flaps/spoilers (considered negative as 0). VTOL: Rework spoiler publishing in hover - pushlish spoiler_setpoint.msg in the VTOL module if in hover - also set spoilers to land configuration if in Descend mode Allocation: add slew rate limit of 0.5 to flaps/spoilers configuration change Instead of doing the flaps/spoilers slew rate limiting in the FW Position Controller (which then only is applied in Auto flight), do it consistently over all flight modes, so also for manual modes. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
@@ -182,6 +182,13 @@ public:
|
||||
*/
|
||||
virtual const char *name() const = 0;
|
||||
|
||||
/**
|
||||
* Callback from the control allocation, allowing to manipulate the setpoint.
|
||||
* Used to allocate auxiliary controls to actuators (e.g. flaps and spoilers).
|
||||
*
|
||||
* @param actuator_sp input & output setpoint
|
||||
*/
|
||||
virtual void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) {}
|
||||
|
||||
/**
|
||||
* Callback from the control allocation, allowing to manipulate the setpoint.
|
||||
|
||||
+34
-28
@@ -52,8 +52,15 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul
|
||||
_param_handles[i].torque[2] = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i);
|
||||
_param_handles[i].trim = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_FLAP", i);
|
||||
_param_handles[i].scale_flap = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_SPOIL", i);
|
||||
_param_handles[i].scale_spoiler = param_find(buffer);
|
||||
}
|
||||
|
||||
_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
|
||||
_spoilers_setpoint_with_slewrate.setSlewRate(kSpoilersSlewRate);
|
||||
|
||||
_count_handle = param_find("CA_SV_CS_COUNT");
|
||||
updateParams();
|
||||
}
|
||||
@@ -81,6 +88,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
|
||||
}
|
||||
|
||||
param_get(_param_handles[i].trim, &_params[i].trim);
|
||||
param_get(_param_handles[i].scale_flap, &_params[i].scale_flap);
|
||||
param_get(_param_handles[i].scale_spoiler, &_params[i].scale_spoiler);
|
||||
|
||||
// TODO: enforce limits (note that tailsitter uses different limits)?
|
||||
switch (_params[i].type) {
|
||||
@@ -109,12 +118,12 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
|
||||
case Type::RightVTail:
|
||||
break;
|
||||
|
||||
case Type::LeftFlaps:
|
||||
case Type::RightFlaps:
|
||||
case Type::LeftFlap:
|
||||
case Type::RightFlap:
|
||||
torque.setZero();
|
||||
break;
|
||||
|
||||
case Type::Airbrakes:
|
||||
case Type::Airbrake:
|
||||
torque.setZero();
|
||||
break;
|
||||
|
||||
@@ -134,6 +143,10 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
|
||||
torque.setZero();
|
||||
break;
|
||||
|
||||
case Type::LeftSpoiler:
|
||||
case Type::RightSpoiler:
|
||||
torque.setZero();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -151,32 +164,25 @@ bool ActuatorEffectivenessControlSurfaces::addActuators(Configuration &configura
|
||||
return true;
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessControlSurfaces::applyFlapsAirbrakesWheel(float flaps_control, float airbrakes_control,
|
||||
float wheel_control, int first_actuator_idx,
|
||||
ActuatorVector &actuator_sp) const
|
||||
void ActuatorEffectivenessControlSurfaces::applyFlaps(float flaps_control, int first_actuator_idx, float dt,
|
||||
ActuatorVector &actuator_sp)
|
||||
{
|
||||
_flaps_setpoint_with_slewrate.update(flaps_control, dt);
|
||||
|
||||
for (int i = 0; i < _count; ++i) {
|
||||
switch (_params[i].type) {
|
||||
// TODO: check sign
|
||||
case ActuatorEffectivenessControlSurfaces::Type::LeftFlaps:
|
||||
actuator_sp(i + first_actuator_idx) += flaps_control;
|
||||
break;
|
||||
|
||||
case ActuatorEffectivenessControlSurfaces::Type::RightFlaps:
|
||||
actuator_sp(i + first_actuator_idx) -= flaps_control;
|
||||
break;
|
||||
|
||||
case ActuatorEffectivenessControlSurfaces::Type::Airbrakes:
|
||||
actuator_sp(i + first_actuator_idx) += airbrakes_control;
|
||||
break;
|
||||
|
||||
case ActuatorEffectivenessControlSurfaces::Type::SteeringWheel:
|
||||
actuator_sp(i + first_actuator_idx) += wheel_control;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
// map [0, 1] to [-1, 1]
|
||||
// TODO: this currently only works for dedicated flaps, not flaperons
|
||||
actuator_sp(i + first_actuator_idx) += (_flaps_setpoint_with_slewrate.getState() * 2.f - 1.f) * _params[i].scale_flap;
|
||||
}
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control, int first_actuator_idx, float dt,
|
||||
ActuatorVector &actuator_sp)
|
||||
{
|
||||
_spoilers_setpoint_with_slewrate.update(spoilers_control, dt);
|
||||
|
||||
for (int i = 0; i < _count; ++i) {
|
||||
// TODO: this currently only works for spoilerons, not dedicated spoilers
|
||||
actuator_sp(i + first_actuator_idx) += _spoilers_setpoint_with_slewrate.getState() * _params[i].scale_spoiler;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
+18
-5
@@ -36,6 +36,10 @@
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
|
||||
static constexpr float kFlapSlewRate = 0.5f; // slew rate for normalized flaps setpoint [1/s]
|
||||
static constexpr float kSpoilersSlewRate = 0.5f; // slew rate for normalized spoilers setpoint [1/s]
|
||||
|
||||
class ActuatorEffectivenessControlSurfaces : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
@@ -53,14 +57,16 @@ public:
|
||||
RightElevon = 6,
|
||||
LeftVTail = 7,
|
||||
RightVTail = 8,
|
||||
LeftFlaps = 9,
|
||||
RightFlaps = 10,
|
||||
Airbrakes = 11,
|
||||
LeftFlap = 9,
|
||||
RightFlap = 10,
|
||||
Airbrake = 11,
|
||||
Custom = 12,
|
||||
LeftATail = 13,
|
||||
RightATail = 14,
|
||||
SingleChannelAileron = 15,
|
||||
SteeringWheel = 16,
|
||||
LeftSpoiler = 17,
|
||||
RightSpoiler = 18,
|
||||
};
|
||||
|
||||
struct Params {
|
||||
@@ -68,6 +74,8 @@ public:
|
||||
|
||||
matrix::Vector3f torque;
|
||||
float trim;
|
||||
float scale_flap;
|
||||
float scale_spoiler;
|
||||
};
|
||||
|
||||
ActuatorEffectivenessControlSurfaces(ModuleParams *parent);
|
||||
@@ -81,8 +89,8 @@ public:
|
||||
|
||||
const Params &config(int idx) const { return _params[idx]; }
|
||||
|
||||
void applyFlapsAirbrakesWheel(float flaps_control, float airbrakes_control, float wheel_control, int first_actuator_idx,
|
||||
ActuatorVector &actuator_sp) const;
|
||||
void applyFlaps(float flaps_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp);
|
||||
void applySpoilers(float spoilers_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp);
|
||||
|
||||
private:
|
||||
void updateParams() override;
|
||||
@@ -92,10 +100,15 @@ private:
|
||||
|
||||
param_t torque[3];
|
||||
param_t trim;
|
||||
param_t scale_flap;
|
||||
param_t scale_spoiler;
|
||||
};
|
||||
ParamHandles _param_handles[MAX_COUNT];
|
||||
param_t _count_handle;
|
||||
|
||||
Params _params[MAX_COUNT] {};
|
||||
int _count{0};
|
||||
|
||||
SlewRate<float> _flaps_setpoint_with_slewrate;
|
||||
SlewRate<float> _spoilers_setpoint_with_slewrate;
|
||||
};
|
||||
|
||||
+12
-10
@@ -61,18 +61,20 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
|
||||
return (rotors_added_successfully && surfaces_added_successfully);
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
|
||||
void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index,
|
||||
ActuatorVector &actuator_sp)
|
||||
{
|
||||
// apply flaps
|
||||
actuator_controls_s actuator_controls_0;
|
||||
normalized_unsigned_setpoint_s flaps_setpoint;
|
||||
|
||||
if (_actuator_controls_0_sub.copy(&actuator_controls_0)) {
|
||||
const float flaps_control = actuator_controls_0.control[actuator_controls_s::INDEX_FLAPS];
|
||||
const float airbrakes_control = actuator_controls_0.control[actuator_controls_s::INDEX_AIRBRAKES];
|
||||
const float steering_wheel_control = actuator_controls_0.control[actuator_controls_s::INDEX_YAW];
|
||||
_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
|
||||
_first_control_surface_idx, actuator_sp);
|
||||
if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
|
||||
_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
|
||||
}
|
||||
|
||||
// apply spoilers
|
||||
normalized_unsigned_setpoint_s spoilers_setpoint;
|
||||
|
||||
if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
|
||||
_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
|
||||
}
|
||||
}
|
||||
|
||||
+4
-5
@@ -37,7 +37,7 @@
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/normalized_unsigned_setpoint.h>
|
||||
|
||||
class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
@@ -49,15 +49,14 @@ public:
|
||||
|
||||
const char *name() const override { return "Fixed Wing"; }
|
||||
|
||||
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
|
||||
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
|
||||
void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
|
||||
|
||||
private:
|
||||
ActuatorEffectivenessRotors _rotors;
|
||||
ActuatorEffectivenessControlSurfaces _control_surfaces;
|
||||
|
||||
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
|
||||
uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
|
||||
uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
|
||||
|
||||
int _first_control_surface_idx{0}; ///< applies to matrix 1
|
||||
};
|
||||
|
||||
+13
-11
@@ -63,20 +63,22 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
|
||||
return (mc_rotors_added_successfully && surfaces_added_successfully);
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
|
||||
void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt, int matrix_index,
|
||||
ActuatorVector &actuator_sp)
|
||||
{
|
||||
// apply flaps
|
||||
if (matrix_index == 1) {
|
||||
actuator_controls_s actuator_controls_1;
|
||||
// apply flaps
|
||||
normalized_unsigned_setpoint_s flaps_setpoint;
|
||||
|
||||
if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
|
||||
const float flaps_control = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
|
||||
const float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
|
||||
const float steering_wheel_control = actuator_controls_1.control[actuator_controls_s::INDEX_YAW];
|
||||
_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
|
||||
_first_control_surface_idx, actuator_sp);
|
||||
if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
|
||||
_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
|
||||
}
|
||||
|
||||
// apply spoilers
|
||||
normalized_unsigned_setpoint_s spoilers_setpoint;
|
||||
|
||||
if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
|
||||
_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
+5
-5
@@ -45,7 +45,8 @@
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/normalized_unsigned_setpoint.h>
|
||||
|
||||
|
||||
class ActuatorEffectivenessStandardVTOL : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
@@ -72,9 +73,7 @@ public:
|
||||
normalize[1] = false;
|
||||
}
|
||||
|
||||
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
|
||||
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
|
||||
void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
|
||||
|
||||
void setFlightPhase(const FlightPhase &flight_phase) override;
|
||||
|
||||
@@ -89,6 +88,7 @@ private:
|
||||
|
||||
int _first_control_surface_idx{0}; ///< applies to matrix 1
|
||||
|
||||
uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_0)};
|
||||
uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
|
||||
uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
|
||||
|
||||
};
|
||||
|
||||
+21
@@ -68,6 +68,27 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi
|
||||
return (mc_rotors_added_successfully && surfaces_added_successfully);
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float dt, int matrix_index,
|
||||
ActuatorVector &actuator_sp)
|
||||
{
|
||||
if (matrix_index == 1) {
|
||||
// apply flaps
|
||||
normalized_unsigned_setpoint_s flaps_setpoint;
|
||||
|
||||
if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
|
||||
_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
|
||||
}
|
||||
|
||||
// apply spoilers
|
||||
normalized_unsigned_setpoint_s spoilers_setpoint;
|
||||
|
||||
if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
|
||||
_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase)
|
||||
{
|
||||
if (_flight_phase == flight_phase) {
|
||||
|
||||
+7
@@ -43,6 +43,8 @@
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
#include <uORB/topics/normalized_unsigned_setpoint.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public ActuatorEffectiveness
|
||||
@@ -68,6 +70,8 @@ public:
|
||||
normalize[1] = false;
|
||||
}
|
||||
|
||||
void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
|
||||
|
||||
void setFlightPhase(const FlightPhase &flight_phase) override;
|
||||
|
||||
const char *name() const override { return "VTOL Tailsitter"; }
|
||||
@@ -80,4 +84,7 @@ protected:
|
||||
uint32_t _stopped_motors{}; ///< currently stopped motors
|
||||
|
||||
int _first_control_surface_idx{0}; ///< applies to matrix 1
|
||||
|
||||
uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
|
||||
uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
|
||||
};
|
||||
|
||||
+21
-13
@@ -91,23 +91,31 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
|
||||
return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully);
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt, int matrix_index,
|
||||
ActuatorVector &actuator_sp)
|
||||
{
|
||||
if (matrix_index == 1) {
|
||||
// apply flaps
|
||||
normalized_unsigned_setpoint_s flaps_setpoint;
|
||||
|
||||
if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
|
||||
_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
|
||||
}
|
||||
|
||||
// apply spoilers
|
||||
normalized_unsigned_setpoint_s spoilers_setpoint;
|
||||
|
||||
if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
|
||||
_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
|
||||
{
|
||||
// apply flaps
|
||||
if (matrix_index == 1) {
|
||||
actuator_controls_s actuator_controls_1;
|
||||
|
||||
if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
|
||||
const float flaps_control = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
|
||||
const float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
|
||||
const float steering_wheel_control = actuator_controls_1.control[actuator_controls_s::INDEX_YAW];
|
||||
_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
|
||||
_first_control_surface_idx, actuator_sp);
|
||||
}
|
||||
}
|
||||
|
||||
// apply tilt
|
||||
if (matrix_index == 0) {
|
||||
|
||||
|
||||
+6
-3
@@ -46,8 +46,9 @@
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
#include "ActuatorEffectivenessTilts.hpp"
|
||||
|
||||
#include <uORB/topics/normalized_unsigned_setpoint.h>
|
||||
#include <uORB/topics/tiltrotor_extra_controls.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness
|
||||
@@ -75,6 +76,8 @@ public:
|
||||
|
||||
void setFlightPhase(const FlightPhase &flight_phase) override;
|
||||
|
||||
void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
|
||||
|
||||
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
|
||||
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
|
||||
@@ -99,8 +102,8 @@ protected:
|
||||
|
||||
float _last_collective_tilt_control{NAN};
|
||||
|
||||
uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
|
||||
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
|
||||
uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
|
||||
uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
|
||||
|
||||
struct YawTiltSaturationFlags {
|
||||
bool tilt_yaw_pos;
|
||||
|
||||
@@ -52,4 +52,5 @@ px4_add_module(
|
||||
ActuatorEffectiveness
|
||||
ControlAllocation
|
||||
px4_work_queue
|
||||
SlewRate
|
||||
)
|
||||
|
||||
@@ -419,6 +419,7 @@ ControlAllocator::Run()
|
||||
|
||||
// Do allocation
|
||||
_control_allocation[i]->allocate();
|
||||
_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
|
||||
_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
|
||||
_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
|
||||
|
||||
|
||||
@@ -266,12 +266,14 @@ parameters:
|
||||
8: Right V-Tail
|
||||
9: Left Flap
|
||||
10: Right Flap
|
||||
11: Airbrakes
|
||||
11: Airbrake
|
||||
12: Custom
|
||||
13: Left A-tail
|
||||
14: Right A-tail
|
||||
15: Single Channel Aileron
|
||||
16: Steering Wheel
|
||||
17: Left Spoiler
|
||||
18: Right Spoiler
|
||||
num_instances: *max_num_servos
|
||||
instance_start: 0
|
||||
default: 0
|
||||
@@ -302,6 +304,7 @@ parameters:
|
||||
num_instances: *max_num_servos
|
||||
instance_start: 0
|
||||
default: 0.0
|
||||
|
||||
CA_SV_CS${i}_TRIM:
|
||||
description:
|
||||
short: Control Surface ${i} trim
|
||||
@@ -314,6 +317,28 @@ parameters:
|
||||
instance_start: 0
|
||||
default: 0.0
|
||||
|
||||
CA_SV_CS${i}_FLAP:
|
||||
description:
|
||||
short: Control Surface ${i} configuration as flap
|
||||
type: float
|
||||
decimal: 2
|
||||
min: -1.0
|
||||
max: 1.0
|
||||
num_instances: *max_num_servos
|
||||
instance_start: 0
|
||||
default: 0
|
||||
|
||||
CA_SV_CS${i}_SPOIL:
|
||||
description:
|
||||
short: Control Surface ${i} configuration as spoiler
|
||||
type: float
|
||||
decimal: 2
|
||||
min: -1.0
|
||||
max: 1.0
|
||||
num_instances: *max_num_servos
|
||||
instance_start: 0
|
||||
default: 0
|
||||
|
||||
# Tilts
|
||||
CA_SV_TL_COUNT:
|
||||
description:
|
||||
@@ -534,7 +559,7 @@ mixer:
|
||||
|
||||
rules:
|
||||
- select_identifier: 'servo-type' # restrict torque based on servo type
|
||||
apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw']
|
||||
apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw', 'servo-scale-flap', 'servo-scale-spoiler']
|
||||
items:
|
||||
# Convention: horizontal surfaces: up=positive, vertical: right=positive, mixed: up=positive.
|
||||
# By default the scale is set to 1/N, where N is the amount of actuators with an effect on
|
||||
@@ -543,54 +568,117 @@ mixer:
|
||||
- { 'disabled': True, 'default': 0.0 } # roll
|
||||
- { 'disabled': True, 'default': 0.0 } # pitch
|
||||
- { 'disabled': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
1: # Left Aileron
|
||||
- { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
2: # Right Aileron
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
3: # Elevator
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
4: # Rudder
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # yaw
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
5: # Left Elevon
|
||||
- { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # roll
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
6: # Right Elevon
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # roll
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
7: # Left V Tail
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
8: # Right V Tail
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
|
||||
- { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
9: # Left Flap
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
10: # Right Flap
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
11: # Airbrake
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
12: # Custom
|
||||
- { 'hidden': False, 'default': 0.0 } # roll
|
||||
- { 'hidden': False, 'default': 0.0 } # pitch
|
||||
- { 'hidden': False, 'default': 0.0 } # yaw
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
13: # Left A Tail
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
|
||||
- { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
14: # Right A Tail
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
15: # Single Channel Aileron
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
16: # Steering Wheel
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
|
||||
17: # Left Spoiler
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # spoiler
|
||||
18: # Right Spoiler
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
|
||||
- { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # spoiler
|
||||
|
||||
|
||||
- select_identifier: 'servo-type-tailsitter' # restrict torque based on servo type for tailsitters
|
||||
apply_identifiers: ['servo-torque-roll-tailsitter', 'servo-torque-pitch-tailsitter', 'servo-torque-yaw-tailsitter']
|
||||
@@ -662,6 +750,14 @@ mixer:
|
||||
identifier: 'servo-torque-yaw'
|
||||
- name: 'CA_SV_CS${i}_TRIM'
|
||||
label: 'Trim'
|
||||
- name: 'CA_SV_CS${i}_FLAP'
|
||||
label: 'Flaps Scale'
|
||||
advanced: true
|
||||
identifier: 'servo-scale-flap'
|
||||
- name: 'CA_SV_CS${i}_SPOIL'
|
||||
label: 'Spoiler Scale'
|
||||
advanced: true
|
||||
identifier: 'servo-scale-spoiler'
|
||||
|
||||
2: # Standard VTOL
|
||||
title: 'Standard VTOL'
|
||||
@@ -706,6 +802,14 @@ mixer:
|
||||
identifier: 'servo-torque-yaw'
|
||||
- name: 'CA_SV_CS${i}_TRIM'
|
||||
label: 'Trim'
|
||||
- name: 'CA_SV_CS${i}_FLAP'
|
||||
label: 'Flaps Scale'
|
||||
advanced: true
|
||||
identifier: 'servo-scale-flap'
|
||||
- name: 'CA_SV_CS${i}_SPOIL'
|
||||
label: 'Spoiler Scale'
|
||||
advanced: true
|
||||
identifier: 'servo-scale-spoiler'
|
||||
parameters:
|
||||
- label: 'Lock control surfaces in hover'
|
||||
name: VT_ELEV_MC_LOCK
|
||||
@@ -747,6 +851,14 @@ mixer:
|
||||
identifier: 'servo-torque-yaw'
|
||||
- name: 'CA_SV_CS${i}_TRIM'
|
||||
label: 'Trim'
|
||||
- name: 'CA_SV_CS${i}_FLAP'
|
||||
label: 'Flaps Scale'
|
||||
advanced: true
|
||||
identifier: 'servo-scale-flap'
|
||||
- name: 'CA_SV_CS${i}_SPOIL'
|
||||
label: 'Spoiler Scale'
|
||||
advanced: true
|
||||
identifier: 'servo-scale-spoiler'
|
||||
parameters:
|
||||
- label: 'Lock control surfaces in hover'
|
||||
name: VT_ELEV_MC_LOCK
|
||||
@@ -800,6 +912,14 @@ mixer:
|
||||
identifier: 'servo-torque-yaw-tailsitter'
|
||||
- name: 'CA_SV_CS${i}_TRIM'
|
||||
label: 'Trim'
|
||||
- name: 'CA_SV_CS${i}_FLAP'
|
||||
label: 'Flaps Scale'
|
||||
advanced: true
|
||||
identifier: 'servo-scale-flap'
|
||||
- name: 'CA_SV_CS${i}_SPOIL'
|
||||
label: 'Spoiler Scale'
|
||||
advanced: true
|
||||
identifier: 'servo-scale-spoiler'
|
||||
parameters:
|
||||
- label: 'Lock control surfaces in hover'
|
||||
name: VT_ELEV_MC_LOCK
|
||||
@@ -912,6 +1032,14 @@ mixer:
|
||||
label: 'Yaw Scale'
|
||||
- name: 'CA_SV_CS${i}_TRIM'
|
||||
label: 'Trim'
|
||||
- name: 'CA_SV_CS${i}_FLAP'
|
||||
label: 'Flaps Scale'
|
||||
advanced: true
|
||||
identifier: 'servo-scale-flap'
|
||||
- name: 'CA_SV_CS${i}_SPOIL'
|
||||
label: 'Spoiler Scale'
|
||||
advanced: true
|
||||
identifier: 'servo-scale-spoiler'
|
||||
|
||||
10: # Helicopter
|
||||
actuators:
|
||||
|
||||
Reference in New Issue
Block a user