From 1e56d9c219394987ec45b8591dba85ea81c325bd Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 20 Dec 2022 13:29:34 +0100 Subject: [PATCH] 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 --- msg/ActuatorControls.msg | 3 - msg/CMakeLists.txt | 1 + msg/ManualControlSetpoint.msg | 2 +- msg/NormalizedUnsignedSetpoint.msg | 5 + msg/VehicleAttitudeSetpoint.msg | 10 -- .../ActuatorEffectiveness.hpp | 7 + .../ActuatorEffectivenessControlSurfaces.cpp | 62 ++++---- .../ActuatorEffectivenessControlSurfaces.hpp | 23 ++- .../ActuatorEffectivenessFixedWing.cpp | 22 +-- .../ActuatorEffectivenessFixedWing.hpp | 9 +- .../ActuatorEffectivenessStandardVTOL.cpp | 24 ++-- .../ActuatorEffectivenessStandardVTOL.hpp | 10 +- .../ActuatorEffectivenessTailsitterVTOL.cpp | 21 +++ .../ActuatorEffectivenessTailsitterVTOL.hpp | 7 + .../ActuatorEffectivenessTiltrotorVTOL.cpp | 34 +++-- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 9 +- src/modules/control_allocator/CMakeLists.txt | 1 + .../control_allocator/ControlAllocator.cpp | 1 + src/modules/control_allocator/module.yaml | 132 +++++++++++++++++- .../FixedwingAttitudeControl.cpp | 2 +- .../FixedwingAttitudeControl.hpp | 1 + .../FixedwingPositionControl.cpp | 42 ++++-- .../FixedwingPositionControl.hpp | 12 ++ .../fw_path_navigation_params.c | 54 +++++++ .../fw_rate_control/FixedwingRateControl.cpp | 132 ++++++------------ .../fw_rate_control/FixedwingRateControl.hpp | 41 +----- .../fw_rate_control/fw_rate_control_params.c | 95 +------------ src/modules/logger/logged_topics.cpp | 2 + .../PositionControl/PositionControlTest.cpp | 1 - .../uuv_att_control/uuv_att_control.cpp | 16 --- src/modules/vtol_att_control/CMakeLists.txt | 2 - src/modules/vtol_att_control/standard.cpp | 13 -- src/modules/vtol_att_control/tiltrotor.cpp | 12 -- .../vtol_att_control_main.cpp | 27 ++++ .../vtol_att_control/vtol_att_control_main.h | 6 +- src/modules/vtol_att_control/vtol_type.cpp | 16 --- src/modules/vtol_att_control/vtol_type.h | 9 +- 37 files changed, 468 insertions(+), 398 deletions(-) create mode 100644 msg/NormalizedUnsignedSetpoint.msg diff --git a/msg/ActuatorControls.msg b/msg/ActuatorControls.msg index ef3d13861d..56699d0314 100644 --- a/msg/ActuatorControls.msg +++ b/msg/ActuatorControls.msg @@ -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 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index bbb1f159c0..f13b7fb9d7 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -128,6 +128,7 @@ set(msg_files MountOrientation.msg ModeCompleted.msg NavigatorMissionItem.msg + NormalizedUnsignedSetpoint.msg NpfgStatus.msg ObstacleDistance.msg OffboardControlMode.msg diff --git a/msg/ManualControlSetpoint.msg b/msg/ManualControlSetpoint.msg index 486aba8edd..4e4e305fd0 100644 --- a/msg/ManualControlSetpoint.msg +++ b/msg/ManualControlSetpoint.msg @@ -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 diff --git a/msg/NormalizedUnsignedSetpoint.msg b/msg/NormalizedUnsignedSetpoint.msg new file mode 100644 index 0000000000..10193b8cca --- /dev/null +++ b/msg/NormalizedUnsignedSetpoint.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32 normalized_setpoint # [0, 1] + +# TOPICS flaps_setpoint spoilers_setpoint diff --git a/msg/VehicleAttitudeSetpoint.msg b/msg/VehicleAttitudeSetpoint.msg index 151623c37b..e4f66c446a 100644 --- a/msg/VehicleAttitudeSetpoint.msg +++ b/msg/VehicleAttitudeSetpoint.msg @@ -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 diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 952179c81f..1317011850 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -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. diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 58b4579e48..23dee70563 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -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; } - } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index bb45604ce7..a0b8b06c16 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -36,6 +36,10 @@ #include "ActuatorEffectiveness.hpp" #include +#include + +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 _flaps_setpoint_with_slewrate; + SlewRate _spoilers_setpoint_with_slewrate; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index 4a8dc763f1..8e152183c8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -61,18 +61,20 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat return (rotors_added_successfully && surfaces_added_successfully); } -void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &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); } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 1f8183992d..6195946866 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -37,7 +37,7 @@ #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" -#include +#include class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffectiveness { @@ -49,15 +49,14 @@ public: const char *name() const override { return "Fixed Wing"; } - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &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 }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 587edb4edb..8d7fed6939 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -63,20 +63,22 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu return (mc_rotors_added_successfully && surfaces_added_successfully); } -void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &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); } } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 174d499941..e49edc6176 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -45,7 +45,8 @@ #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" -#include +#include + class ActuatorEffectivenessStandardVTOL : public ModuleParams, public ActuatorEffectiveness { @@ -72,9 +73,7 @@ public: normalize[1] = false; } - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &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)}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 2683c84e77..139dc36612 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -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) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 3210b48bb8..f5a8ddb460 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -43,6 +43,8 @@ #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" +#include + #include 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)}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 13faf51e6c..84695a9bb2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -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 &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, const matrix::Vector &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) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index a3c2c4c6fb..33d0df486d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -46,8 +46,9 @@ #include "ActuatorEffectivenessControlSurfaces.hpp" #include "ActuatorEffectivenessTilts.hpp" +#include #include -#include + #include 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 &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, const matrix::Vector &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; diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index cf7045d91a..65872dc432 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/src/modules/control_allocator/CMakeLists.txt @@ -52,4 +52,5 @@ px4_add_module( ActuatorEffectiveness ControlAllocation px4_work_queue + SlewRate ) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index c487b4bc4b..e7fc7b3aa8 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -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()); diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index b717d106e3..59d49b0afa 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -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: diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index b57df7962b..326bc31ac1 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -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 diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 63bb8cbd93..eba981d624 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -156,6 +156,7 @@ private: (ParamFloat) _param_fw_wr_p, (ParamFloat) _param_fw_y_rmax + ) ECL_RollController _roll_ctrl; diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 9d830f9dbc..18dddcf359 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -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); } } diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp index 8323ac1813..1ba4b8beaf 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -214,6 +215,8 @@ private: uORB::Publication _launch_detection_status_pub{ORB_ID(launch_detection_status)}; uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; + uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; + uORB::Publication _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) _param_fw_thr_min, (ParamFloat) _param_fw_thr_slew_max, + (ParamFloat) _param_fw_flaps_lnd_scl, + (ParamFloat) _param_fw_flaps_to_scl, + (ParamFloat) _param_fw_spoilers_lnd, + (ParamFloat) _param_fw_spoilers_desc, + (ParamInt) _param_fw_pos_stk_conf, (ParamInt) _param_nav_gpsf_lt, diff --git a/src/modules/fw_path_navigation/fw_path_navigation_params.c b/src/modules/fw_path_navigation/fw_path_navigation_params.c index 57d17b602d..dfabdec05d 100644 --- a/src/modules/fw_path_navigation/fw_path_navigation_params.c +++ b/src/modules/fw_path_navigation/fw_path_navigation_params.c @@ -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); diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index d1b7e4c098..9ed455b2da 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -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 ×tamp_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++) { diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index d34148bbbe..d24edd0d84 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -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 #include #include +#include #include #include #include -#include #include #include #include @@ -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, 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_pub{ORB_ID(rate_ctrl_status)}; uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; + uORB::Publication _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 _spoiler_setpoint_with_slewrate; - SlewRate _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) _param_fw_bat_scale_en, - (ParamFloat) _param_fw_dtrim_p_flps, - (ParamFloat) _param_fw_dtrim_p_spoil, (ParamFloat) _param_fw_dtrim_p_vmax, (ParamFloat) _param_fw_dtrim_p_vmin, - (ParamFloat) _param_fw_dtrim_r_flps, (ParamFloat) _param_fw_dtrim_r_vmax, (ParamFloat) _param_fw_dtrim_r_vmin, (ParamFloat) _param_fw_dtrim_y_vmax, (ParamFloat) _param_fw_dtrim_y_vmin, - (ParamFloat) _param_fw_flaps_lnd_scl, - (ParamFloat) _param_fw_flaps_to_scl, - (ParamFloat) _param_fw_spoilers_lnd, - (ParamFloat) _param_fw_spoilers_desc, - (ParamInt) _param_fw_spoilers_man, - (ParamFloat) _param_fw_man_p_max, (ParamFloat) _param_fw_man_p_sc, (ParamFloat) _param_fw_man_r_max, @@ -212,25 +197,13 @@ private: (ParamFloat) _param_trim_pitch, (ParamFloat) _param_trim_roll, - (ParamFloat) _param_trim_yaw + (ParamFloat) _param_trim_yaw, + + (ParamInt) _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); /** diff --git a/src/modules/fw_rate_control/fw_rate_control_params.c b/src/modules/fw_rate_control/fw_rate_control_params.c index 44c5e938f2..10d8172bdf 100644 --- a/src/modules/fw_rate_control/fw_rate_control_params.c +++ b/src/modules/fw_rate_control/fw_rate_control_params.c @@ -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 * diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 95e9b61dce..0545057d8a 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 7dc3e6d4e9..0e5a60619a 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -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 diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index 4f03714072..b87f6ab508 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -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, diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt index 517828377f..e6316b1459 100644 --- a/src/modules/vtol_att_control/CMakeLists.txt +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -39,7 +39,5 @@ px4_add_module( vtol_type.cpp tailsitter.cpp standard.cpp - DEPENDS - SlewRate ) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 8c4ba178bd..296975d521 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -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; } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 158fd62f83..a1a3599c9d 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -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; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 30a2a0a262..fab819e29f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 2d2ed56f9b..d44b454aee 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -68,6 +68,7 @@ #include #include #include +#include #include #include #include @@ -172,6 +173,8 @@ private: uORB::Publication _actuator_controls_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust) uORB::Publication _actuator_controls_1_pub{ORB_ID(actuator_controls_1)}; + uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; + uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; uORB::Publication _vehicle_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::PublicationMulti _vehicle_thrust_setpoint0_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::PublicationMulti _vehicle_thrust_setpoint1_pub{ORB_ID(vehicle_thrust_setpoint)}; @@ -234,6 +237,7 @@ private: void parameters_update(); DEFINE_PARAMETERS( - (ParamInt) _param_vt_type + (ParamInt) _param_vt_type, + (ParamFloat) _param_vt_spoiler_mc_ld ) }; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 3ab8c79b8c..9617dce518 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -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() diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index aa6b9128f3..ef5e85add5 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -45,7 +45,6 @@ #include #include -#include #include @@ -306,9 +305,6 @@ protected: bool isFrontTransitionCompleted(); virtual bool isFrontTransitionCompletedBase(); - SlewRate _spoiler_setpoint_with_slewrate; - SlewRate _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) _param_vt_fwd_thrust_en, (ParamFloat) _param_mpc_land_alt1, (ParamFloat) _param_mpc_land_alt2, - (ParamFloat) _param_vt_lnd_pitch_min, - - (ParamFloat) _param_vt_spoiler_mc_ld - + (ParamFloat) _param_vt_lnd_pitch_min ) private: