dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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:
Silvan Fuhrer 2022-12-20 13:29:34 +01:00
parent 16594bffa9
commit 1e56d9c219
37 changed files with 468 additions and 398 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/ActuatorControls.msg
View File

@ -5,9 +5,6 @@ uint8 INDEX_ROLL = 0
uint8 INDEX_PITCH = 1
uint8 INDEX_YAW = 2
uint8 INDEX_THROTTLE = 3
uint8 INDEX_FLAPS = 4
uint8 INDEX_SPOILERS = 5
uint8 INDEX_AIRBRAKES = 6
uint8 INDEX_GIMBAL_SHUTTER = 3
uint8 INDEX_CAMERA_ZOOM = 4

1
msg/CMakeLists.txt
View File

@ -128,6 +128,7 @@ set(msg_files
MountOrientation.msg
ModeCompleted.msg
NavigatorMissionItem.msg
NormalizedUnsignedSetpoint.msg
NpfgStatus.msg
ObstacleDistance.msg
OffboardControlMode.msg

2
msg/ManualControlSetpoint.msg
View File

@ -26,7 +26,7 @@ float32 pitch # move forward, negative pitch rotation, nose down
float32 yaw # positive yaw rotation, clockwise when seen top down
float32 throttle # move up, positive thrust, -1 is minimum available 0% or -100% +1 is 100% thrust
float32 flaps # flap position
float32 flaps # position of flaps switch/knob/lever [-1, 1]
float32 aux1
float32 aux2

5
msg/NormalizedUnsignedSetpoint.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
float32 normalized_setpoint # [0, 1]
# TOPICS flaps_setpoint spoilers_setpoint

10
msg/VehicleAttitudeSetpoint.msg
View File

@ -17,14 +17,4 @@ bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change)
bool fw_control_yaw_wheel # control heading with steering wheel (used for auto takeoff on runway)
uint8 apply_flaps # flap config specifier
uint8 FLAPS_OFF = 0 # no flaps
uint8 FLAPS_LAND = 1 # landing config flaps
uint8 FLAPS_TAKEOFF = 2 # take-off config flaps
uint8 apply_spoilers # spoiler config specifier
uint8 SPOILERS_OFF = 0 # no spoilers
uint8 SPOILERS_LAND = 1 # landing config spoiler
uint8 SPOILERS_DESCEND = 2 # descend config spoiler
# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint

7
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
View File

@ -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.

62
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
View File

@ -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;
}
}

23
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp
View File

@ -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;
};

22
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp
View File

@ -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);
}
}

9
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp
View File

@ -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
};

24
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
View File

@ -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);
}
}
}

10
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
View File

@ -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
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp
View File

@ -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
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp
View File

@ -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)};
};

34
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
View File

@ -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) {

9
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
View File

@ -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;

1
src/modules/control_allocator/CMakeLists.txt
View File

@ -52,4 +52,5 @@ px4_add_module(
ActuatorEffectiveness
ControlAllocation
px4_work_queue
SlewRate
)

1
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -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());

132
src/modules/control_allocator/module.yaml
View File

@ -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:

2
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2023 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

1
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -156,6 +156,7 @@ private:
(ParamFloat<px4::params::FW_WR_P>) _param_fw_wr_p,
(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax
)
ECL_RollController _roll_ctrl;

42
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
View File

@ -69,6 +69,9 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
_launch_detection_status_pub.advertise();
_landing_gear_pub.advertise();
_flaps_setpoint_pub.advertise();
_spoilers_setpoint_pub.advertise();
_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);
/* fetch initial parameter values */
@ -939,6 +942,7 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
}
_att_sp.pitch_body = get_tecs_pitch();
}
void
@ -1231,10 +1235,9 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
// have to do this switch (which can cause significant altitude errors) close to the ground.
_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : _param_fw_airspd_min.get();
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;
_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
_spoilers_setpoint = _param_fw_spoilers_lnd.get();
_new_landing_gear_position = landing_gear_s::GEAR_DOWN;
}
float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp, _param_fw_airspd_min.get(),
@ -1394,8 +1397,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust());
// apply flaps for takeoff according to the corresponding scale factor set via FW_FLAPS_TO_SCL
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF;
_flaps_setpoint = _param_fw_flaps_to_scl.get();
// retract ladning gear once passed the climbout state
if (_runway_takeoff.getState() > RunwayTakeoffState::CLIMBOUT) {
@ -1713,9 +1715,8 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
// Apply flaps and spoilers for landing. Amount of deflection is handled in the FW attitdue controller
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;
_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
_spoilers_setpoint = _param_fw_spoilers_lnd.get();
// deploy gear as soon as we're in land mode, if not already done before
_new_landing_gear_position = landing_gear_s::GEAR_DOWN;
@ -1919,9 +1920,8 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
// Apply flaps and spoilers for landing. Amount of deflection is handled in the FW attitdue controller
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;
_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
_spoilers_setpoint = _param_fw_spoilers_lnd.get();
if (!_vehicle_status.in_transition_to_fw) {
publishLocalPositionSetpoint(pos_sp_curr);
@ -2290,8 +2290,10 @@ FixedwingPositionControl::Run()
_npfg.setPeriod(_param_npfg_period.get());
_att_sp.reset_integral = false;
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
// by default no flaps/spoilers, is overwritten below in certain modes
_flaps_setpoint = 0.f;
_spoilers_setpoint = 0.f;
// by default we don't want yaw to be contoller directly with rudder
_att_sp.fw_control_yaw_wheel = false;
@ -2404,6 +2406,20 @@ FixedwingPositionControl::Run()
_landing_gear_pub.publish(landing_gear);
}
// In Manual modes flaps and spoilers are directly controlled in the Attitude controller and not published here
if (_control_mode.flag_control_auto_enabled
&& _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
normalized_unsigned_setpoint_s flaps_setpoint;
flaps_setpoint.normalized_setpoint = _flaps_setpoint;
flaps_setpoint.timestamp = hrt_absolute_time();
_flaps_setpoint_pub.publish(flaps_setpoint);
normalized_unsigned_setpoint_s spoilers_setpoint;
spoilers_setpoint.normalized_setpoint = _spoilers_setpoint;
spoilers_setpoint.timestamp = hrt_absolute_time();
_spoilers_setpoint_pub.publish(spoilers_setpoint);
}
perf_end(_loop_perf);
}
}

12
src/modules/fw_path_navigation/FixedwingPositionControl.hpp
View File

@ -74,6 +74,7 @@
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/launch_detection_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/normalized_unsigned_setpoint.h>
#include <uORB/topics/npfg_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_landing_status.h>
@ -214,6 +215,8 @@ private:
uORB::Publication<launch_detection_status_s> _launch_detection_status_pub{ORB_ID(launch_detection_status)};
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
manual_control_setpoint_s _manual_control_setpoint{};
position_setpoint_triplet_s _pos_sp_triplet{};
@ -419,6 +422,10 @@ private:
// LANDING GEAR
int8_t _new_landing_gear_position{landing_gear_s::GEAR_KEEP};
// FLAPS/SPOILERS
float _flaps_setpoint{0.f};
float _spoilers_setpoint{0.f};
hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
float _min_current_sp_distance_xy{FLT_MAX};
float _target_bearing{0.0f}; // [rad]
@ -905,6 +912,11 @@ private:
(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,
(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
(ParamFloat<px4::params::FW_SPOILERS_LND>) _param_fw_spoilers_lnd,
(ParamFloat<px4::params::FW_SPOILERS_DESC>) _param_fw_spoilers_desc,
(ParamInt<px4::params::FW_POS_STK_CONF>) _param_fw_pos_stk_conf,
(ParamInt<px4::params::FW_GPSF_LT>) _param_nav_gpsf_lt,

54
src/modules/fw_path_navigation/fw_path_navigation_params.c
View File

@ -1009,3 +1009,57 @@ PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.f);
* @group FW Launch detection
*/
PARAM_DEFINE_INT32(FW_LAUN_DETCN_ON, 0);
/**
* Flaps setting during take-off
*
* Sets a fraction of full flaps during take-off.
* Also applies to flaperons if enabled in the mixer/allocation.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f);
/**
* Flaps setting during landing
*
* Sets a fraction of full flaps during landing.
* Also applies to flaperons if enabled in the mixer/allocation.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f);
/**
* Spoiler landing setting
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
/**
* Spoiler descend setting
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);

132
src/modules/fw_rate_control/FixedwingRateControl.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2023 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
@ -51,8 +51,6 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) :
parameters_update();
_rate_ctrl_status_pub.advertise();
_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate);
_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
}
FixedwingRateControl::~FixedwingRateControl()
@ -255,11 +253,6 @@ void FixedwingRateControl::Run()
angular_velocity.xyz_derivative[0]);
}
// this is only to pass through flaps/spoiler setpoints, can be removed once flaps/spoilers
// are handled outside of attitude/rate controller.
// TODO remove it
_att_sp_sub.update(&_att_sp);
// vehicle status update must be before the vehicle_control_mode poll, otherwise rate sp are not published during whole transition
_vehicle_status_sub.update(&_vehicle_status);
const bool is_in_transition_except_tailsitter = _vehicle_status.in_transition_mode
@ -276,15 +269,10 @@ void FixedwingRateControl::Run()
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
_spoiler_setpoint_with_slewrate.setForcedValue(0.f);
_flaps_setpoint_with_slewrate.setForcedValue(0.f);
perf_end(_loop_perf);
return;
}
controlFlaps(dt);
controlSpoilers(dt);
if (_vcontrol_mode.flag_control_rates_enabled) {
const float airspeed = get_airspeed_and_update_scaling();
@ -347,13 +335,6 @@ void FixedwingRateControl::Run()
_param_fw_dtrim_y_vmax.get());
}
/* add trim increment if flaps are deployed */
trim_roll += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_r_flps.get();
trim_pitch += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_flps.get();
// add trim increment from spoilers (only pitch)
trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get();
if (_vcontrol_mode.flag_control_rates_enabled) {
_rates_sp_sub.update(&_rates_sp);
@ -434,10 +415,6 @@ void FixedwingRateControl::Run()
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = -helper;
}
_actuator_controls.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
_actuator_controls.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
_actuator_controls.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f;
/* lazily publish the setpoint only once available */
_actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls.timestamp_sample = vehicle_angular_velocity.timestamp;
@ -455,6 +432,46 @@ void FixedwingRateControl::Run()
}
updateActuatorControlsStatus(dt);
// Manual flaps/spoilers control, also active in VTOL Hover. Is handled and published in FW Position controller/VTOL module if Auto.
if (_vcontrol_mode.flag_control_manual_enabled) {
// Flaps control
float flaps_control = 0.f; // default to no flaps
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual_control_setpoint.flaps)) {
flaps_control = math::max(_manual_control_setpoint.flaps, 0.f); // do not consider negative switch settings
}
normalized_unsigned_setpoint_s flaps_setpoint;
flaps_setpoint.timestamp = hrt_absolute_time();
flaps_setpoint.normalized_setpoint = flaps_control;
_flaps_setpoint_pub.publish(flaps_setpoint);
// Spoilers control
float spoilers_control = 0.f; // default to no spoilers
switch (_param_fw_spoilers_man.get()) {
case 0:
break;
case 1:
// do not consider negative switch settings
spoilers_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? math::max(_manual_control_setpoint.flaps, 0.f) : 0.f;
break;
case 2:
// do not consider negative switch settings
spoilers_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? math::max(_manual_control_setpoint.aux1, 0.f) : 0.f;
break;
}
normalized_unsigned_setpoint_s spoilers_setpoint;
spoilers_setpoint.timestamp = hrt_absolute_time();
spoilers_setpoint.normalized_setpoint = spoilers_control;
_spoilers_setpoint_pub.publish(spoilers_setpoint);
}
}
// backup schedule
@ -487,73 +504,6 @@ void FixedwingRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sa
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
void FixedwingRateControl::controlFlaps(const float dt)
{
/* default flaps to center */
float flap_control = 0.0f;
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled) {
flap_control = _manual_control_setpoint.flaps;
} else if (_vcontrol_mode.flag_control_auto_enabled) {
switch (_att_sp.apply_flaps) {
case vehicle_attitude_setpoint_s::FLAPS_OFF:
flap_control = 0.0f; // no flaps
break;
case vehicle_attitude_setpoint_s::FLAPS_LAND:
flap_control = _param_fw_flaps_lnd_scl.get();
break;
case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF:
flap_control = _param_fw_flaps_to_scl.get();
break;
}
}
// move the actual control value continuous with time, full flap travel in 1sec
_flaps_setpoint_with_slewrate.update(math::constrain(flap_control, 0.f, 1.f), dt);
}
void FixedwingRateControl::controlSpoilers(const float dt)
{
float spoiler_control = 0.f;
if (_vcontrol_mode.flag_control_manual_enabled) {
switch (_param_fw_spoilers_man.get()) {
case 0:
break;
case 1:
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? _manual_control_setpoint.flaps : 0.f;
break;
case 2:
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? _manual_control_setpoint.aux1 : 0.f;
break;
}
} else if (_vcontrol_mode.flag_control_auto_enabled) {
switch (_att_sp.apply_spoilers) {
case vehicle_attitude_setpoint_s::SPOILERS_OFF:
spoiler_control = 0.f;
break;
case vehicle_attitude_setpoint_s::SPOILERS_LAND:
spoiler_control = _param_fw_spoilers_lnd.get();
break;
case vehicle_attitude_setpoint_s::SPOILERS_DESCEND:
spoiler_control = _param_fw_spoilers_desc.get();
break;
}
}
_spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_control, 0.f, 1.f), dt);
}
void FixedwingRateControl::updateActuatorControlsStatus(float dt)
{
for (int i = 0; i < 4; i++) {

41
src/modules/fw_rate_control/FixedwingRateControl.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2023 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
@ -59,10 +59,10 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/normalized_unsigned_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@ -77,9 +77,6 @@ using uORB::SubscriptionData;
using namespace time_literals;
static constexpr float kFlapSlewRate = 1.f; //minimum time from none to full flap deflection [s]
static constexpr float kSpoilerSlewRate = 1.f; //minimum time from none to full spoiler deflection [s]
class FixedwingRateControl final : public ModuleBase<FixedwingRateControl>, public ModuleParams,
public px4::ScheduledWorkItem
{
@ -108,7 +105,6 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
@ -127,10 +123,11 @@ private:
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
actuator_controls_s _actuator_controls{};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_attitude_setpoint_s _att_sp{};
vehicle_control_mode_s _vcontrol_mode{};
vehicle_rates_setpoint_s _rates_sp{};
vehicle_status_s _vehicle_status{};
@ -149,9 +146,6 @@ private:
float _control_energy[4] {};
float _control_prev[3] {};
SlewRate<float> _spoiler_setpoint_with_slewrate;
SlewRate<float> _flaps_setpoint_with_slewrate;
bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states
DEFINE_PARAMETERS(
@ -169,22 +163,13 @@ private:
(ParamBool<px4::params::FW_BAT_SCALE_EN>) _param_fw_bat_scale_en,
(ParamFloat<px4::params::FW_DTRIM_P_FLPS>) _param_fw_dtrim_p_flps,
(ParamFloat<px4::params::FW_DTRIM_P_SPOIL>) _param_fw_dtrim_p_spoil,
(ParamFloat<px4::params::FW_DTRIM_P_VMAX>) _param_fw_dtrim_p_vmax,
(ParamFloat<px4::params::FW_DTRIM_P_VMIN>) _param_fw_dtrim_p_vmin,
(ParamFloat<px4::params::FW_DTRIM_R_FLPS>) _param_fw_dtrim_r_flps,
(ParamFloat<px4::params::FW_DTRIM_R_VMAX>) _param_fw_dtrim_r_vmax,
(ParamFloat<px4::params::FW_DTRIM_R_VMIN>) _param_fw_dtrim_r_vmin,
(ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
(ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
(ParamFloat<px4::params::FW_SPOILERS_LND>) _param_fw_spoilers_lnd,
(ParamFloat<px4::params::FW_SPOILERS_DESC>) _param_fw_spoilers_desc,
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man,
(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
(ParamFloat<px4::params::FW_MAN_P_SC>) _param_fw_man_p_sc,
(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
@ -212,25 +197,13 @@ private:
(ParamFloat<px4::params::TRIM_PITCH>) _param_trim_pitch,
(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw
(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man
)
RateControl _rate_control; ///< class for rate control calculations
/**
* @brief Update flap control setting
*
* @param dt Current time delta [s]
*/
void controlFlaps(const float dt);
/**
* @brief Update spoiler control setting
*
* @param dt Current time delta [s]
*/
void controlSpoilers(const float dt);
void updateActuatorControlsStatus(float dt);
/**

95
src/modules/fw_rate_control/fw_rate_control_params.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2023 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
@ -469,75 +469,6 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f);
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f);
/**
* Roll trim increment for flaps configuration
*
* This increment is added to TRIM_ROLL whenever flaps are fully deployed.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f);
/**
* Pitch trim increment for flaps configuration
*
* This increment is added to the pitch trim whenever flaps are fully deployed.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f);
/**
* Pitch trim increment for spoiler configuration
*
* This increment is added to the pitch trim whenever spoilers are fully deployed.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_SPOIL, 0.f);
/**
* Flaps setting during take-off
*
* Sets a fraction of full flaps during take-off.
* Also applies to flaperons if enabled in the mixer/allocation.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f);
/**
* Flaps setting during landing
*
* Sets a fraction of full flaps during landing.
* Also applies to flaperons if enabled in the mixer/allocation.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f);
/**
* Manual roll scale
*
@ -623,30 +554,6 @@ PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);
*/
PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f);
/**
* Spoiler landing setting
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
/**
* Spoiler descend setting
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);
/**
* Spoiler input in manual flight
*

2
src/modules/logger/logged_topics.cpp
View File

@ -68,6 +68,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic("follow_target", 500);
add_optional_topic("follow_target_estimator", 200);
add_optional_topic("follow_target_status", 400);
add_optional_topic("flaps_setpoint", 1000);
add_topic("gimbal_manager_set_attitude", 500);
add_optional_topic("generator_status");
add_optional_topic("gps_dump");
@ -103,6 +104,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic("sensor_gyro_fft", 50);
add_topic("sensor_selection");
add_topic("sensors_status_imu", 200);
add_optional_topic("spoilers_setpoint", 1000);
add_topic("system_power", 500);
add_optional_topic("takeoff_status", 1000);
add_optional_topic("tecs_status", 200);

1
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
View File

@ -64,7 +64,6 @@ TEST(PositionControlTest, EmptySetpoint)
EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
EXPECT_EQ(attitude.reset_integral, false);
EXPECT_EQ(attitude.fw_control_yaw_wheel, false);
EXPECT_FLOAT_EQ(attitude.apply_flaps, 0.f);//vehicle_attitude_setpoint_s::FLAPS_OFF); // TODO why no reference?
}
class PositionControlBasicTest : public ::testing::Test

16
src/modules/uuv_att_control/uuv_att_control.cpp
View File

@ -124,22 +124,6 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
} else {
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
}
if (PX4_ISFINITE(thrust_y)) {
thrust_y = math::constrain(thrust_y, -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = thrust_y;
} else {
_actuators.control[actuator_controls_s::INDEX_FLAPS] = 0.0f;
}
if (PX4_ISFINITE(thrust_z)) {
thrust_z = math::constrain(thrust_z, -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_SPOILERS] = thrust_z;
} else {
_actuators.control[actuator_controls_s::INDEX_SPOILERS] = 0.0f;
}
}
void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude,

2
src/modules/vtol_att_control/CMakeLists.txt
View File

@ -39,7 +39,5 @@ px4_add_module(
vtol_type.cpp
tailsitter.cpp
standard.cpp
DEPENDS
SlewRate
)

13
src/modules/vtol_att_control/standard.cpp
View File

@ -229,10 +229,6 @@ void Standard::update_transition_state()
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
// set spoiler and flaps to 0
_flaps_setpoint_with_slewrate.update(0.f, _dt);
_spoiler_setpoint_with_slewrate.update(0.f, _dt);
} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
if (_v_control_mode->flag_control_climb_rate_enabled) {
@ -299,10 +295,6 @@ void Standard::fill_actuator_outputs()
fw_in[actuator_controls_s::INDEX_PITCH];
fw_out[actuator_controls_s::INDEX_YAW] = 0;
fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0.f;
break;
case vtol_mode::TRANSITION_TO_FW:
@ -320,8 +312,6 @@ void Standard::fill_actuator_outputs()
fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH] * (1.f - _mc_pitch_weight);
fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW] * (1.f - _mc_yaw_weight);
fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
break;
@ -337,9 +327,6 @@ void Standard::fill_actuator_outputs()
fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW];
fw_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0;
break;
}

12
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -299,10 +299,6 @@ void Tiltrotor::update_transition_state()
// add minimum throttle for front transition
_thrust_transition = math::max(_thrust_transition, FRONTTRANS_THR_MIN);
// set spoiler and flaps to 0
_flaps_setpoint_with_slewrate.update(0.f, _dt);
_spoiler_setpoint_with_slewrate.update(0.f, _dt);
} else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = math::constrain(_param_vt_tilt_trans.get() +
@ -319,10 +315,6 @@ void Tiltrotor::update_transition_state()
// if this is not then then there is race condition where the fw rate controller still publishes a zero sample throttle after transition
_v_att_sp->thrust_body[0] = _thrust_transition;
// set spoiler and flaps to 0
_flaps_setpoint_with_slewrate.update(0.f, _dt);
_spoiler_setpoint_with_slewrate.update(0.f, _dt);
} else if (_vtol_mode == vtol_mode::TRANSITION_BACK) {
// tilt rotors back once motors are idle
@ -463,10 +455,6 @@ void Tiltrotor::fill_actuator_outputs()
_torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW];
}
fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0;
_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;

27
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -77,6 +77,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
exit_and_cleanup();
}
_flaps_setpoint_pub.advertise();
_spoilers_setpoint_pub.advertise();
_vtol_vehicle_status_pub.advertise();
_vehicle_thrust_setpoint0_pub.advertise();
_vehicle_torque_setpoint0_pub.advertise();
@ -423,6 +425,31 @@ VtolAttitudeControl::Run()
// Advertise/Publish vtol vehicle status
_vtol_vehicle_status.timestamp = hrt_absolute_time();
_vtol_vehicle_status_pub.publish(_vtol_vehicle_status);
// Publish flaps/spoiler setpoint with configured deflection in Hover if in Auto.
// In Manual always published in FW rate controller, and in Auto FW in FW Position Controller.
if (_vehicle_control_mode.flag_control_auto_enabled
&& _vtol_vehicle_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) {
// flaps
normalized_unsigned_setpoint_s flaps_setpoint;
flaps_setpoint.normalized_setpoint = 0.f; // for now always set flaps to 0 in transitions and hover
flaps_setpoint.timestamp = hrt_absolute_time();
_flaps_setpoint_pub.publish(flaps_setpoint);
// spoilers
float spoiler_control = 0.f;
if ((_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) ||
_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
spoiler_control = _param_vt_spoiler_mc_ld.get();
}
normalized_unsigned_setpoint_s spoiler_setpoint;
spoiler_setpoint.normalized_setpoint = spoiler_control;
spoiler_setpoint.timestamp = hrt_absolute_time();
_spoilers_setpoint_pub.publish(spoiler_setpoint);
}
}
perf_end(_loop_perf);

6
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -68,6 +68,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/normalized_unsigned_setpoint.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
@ -172,6 +173,8 @@ private:
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust)
uORB::Publication<actuator_controls_s> _actuator_controls_1_pub{ORB_ID(actuator_controls_1)};
uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::PublicationMulti<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint0_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::PublicationMulti<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint1_pub{ORB_ID(vehicle_thrust_setpoint)};
@ -234,6 +237,7 @@ private:
void parameters_update();
DEFINE_PARAMETERS(
(ParamInt<px4::params::VT_TYPE>) _param_vt_type
(ParamInt<px4::params::VT_TYPE>) _param_vt_type,
(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld
)
};

16
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -79,9 +79,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
bool VtolType::init()
{
_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRateVtol);
_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRateVtol);
return true;
}
@ -106,16 +103,6 @@ void VtolType::update_mc_state()
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0f;
float spoiler_setpoint_hover = 0.f;
if (_attc->get_pos_sp_triplet()->current.valid
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
spoiler_setpoint_hover = _param_vt_spoiler_mc_ld.get();
}
_spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_setpoint_hover, 0.f, 1.f), _dt);
_flaps_setpoint_with_slewrate.update(0.f, _dt);
}
void VtolType::update_fw_state()
@ -158,9 +145,6 @@ void VtolType::update_fw_state()
}
check_quadchute_condition();
_spoiler_setpoint_with_slewrate.update(_actuators_fw_in->control[actuator_controls_s::INDEX_SPOILERS], _dt);
_flaps_setpoint_with_slewrate.update(_actuators_fw_in->control[actuator_controls_s::INDEX_FLAPS], _dt);
}
void VtolType::update_transition_state()

9
src/modules/vtol_att_control/vtol_type.h
View File

@ -45,7 +45,6 @@
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/slew_rate/SlewRate.hpp>
#include <px4_platform_common/module_params.h>
@ -306,9 +305,6 @@ protected:
bool isFrontTransitionCompleted();
virtual bool isFrontTransitionCompletedBase();
SlewRate<float> _spoiler_setpoint_with_slewrate;
SlewRate<float> _flaps_setpoint_with_slewrate;
float _dt{0.0025f}; // time step [s]
float _local_position_z_start_of_transition{0.f}; // altitude at start of transition
@ -346,10 +342,7 @@ protected:
(ParamInt<px4::params::VT_FWD_THRUST_EN>) _param_vt_fwd_thrust_en,
(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
(ParamFloat<px4::params::VT_LND_PITCH_MIN>) _param_vt_lnd_pitch_min,
(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld
(ParamFloat<px4::params::VT_LND_PITCH_MIN>) _param_vt_lnd_pitch_min
)
private: