diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 68fbc6dd9d..cdc1fd9a72 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -44,7 +44,6 @@ #include #include -#include class ActuatorEffectiveness { @@ -55,23 +54,6 @@ public: static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES; - enum class FlightPhase { - HOVER_FLIGHT = 0, - FORWARD_FLIGHT = 1, - TRANSITION_HF_TO_FF = 2, - TRANSITION_FF_TO_HF = 3 - }; - - /** - * Set the current flight phase - * - * @param Flight phase - */ - virtual void setFlightPhase(const FlightPhase &flight_phase) - { - _flight_phase = flight_phase; - } - /** * Get the control effectiveness matrix if updated * @@ -84,20 +66,7 @@ public: * * @return Actuator trims */ - const matrix::Vector &getActuatorTrim() const - { - return _trim; - } - - /** - * Get the current flight phase - * - * @return Flight phase - */ - const FlightPhase &getFlightPhase() const - { - return _flight_phase; - } + const matrix::Vector &getActuatorTrim() const { return _trim; } /** * Get the number of actuators @@ -106,5 +75,4 @@ public: protected: matrix::Vector _trim; ///< Actuator trim - FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index beb36b952e..8b07c26a41 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,15 +41,40 @@ #include "ActuatorEffectivenessStandardVTOL.hpp" -ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL() -{ - setFlightPhase(FlightPhase::HOVER_FLIGHT); -} - bool ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix &matrix, bool force) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + + FlightPhase flight_phase{FlightPhase::HOVER_FLIGHT}; + + // Check if the current flight phase is HOVER or FIXED_WING + if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + flight_phase = FlightPhase::HOVER_FLIGHT; + + } else { + flight_phase = FlightPhase::FORWARD_FLIGHT; + } + + // Special cases for VTOL in transition + if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) { + if (vehicle_status.in_transition_to_fw) { + flight_phase = FlightPhase::TRANSITION_HF_TO_FF; + + } else { + flight_phase = FlightPhase::TRANSITION_FF_TO_HF; + } + } + + if (flight_phase != _flight_phase) { + _flight_phase = flight_phase; + _updated = true; + } + } + if (!(_updated || force)) { return false; } @@ -99,11 +124,3 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix +#include + class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness { public: - ActuatorEffectivenessStandardVTOL(); + ActuatorEffectivenessStandardVTOL() = default; virtual ~ActuatorEffectivenessStandardVTOL() = default; bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force) override; - /** - * Set the current flight phase - * - * @param Flight phase - */ - void setFlightPhase(const FlightPhase &flight_phase) override; - int numActuators() const override { return 7; } protected: + + enum class FlightPhase { + HOVER_FLIGHT = 0, + FORWARD_FLIGHT = 1, + TRANSITION_HF_TO_FF = 2, + TRANSITION_FF_TO_HF = 3 + }; + + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _updated{true}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index d3baed187b..3532102c67 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,14 +41,40 @@ #include "ActuatorEffectivenessTiltrotorVTOL.hpp" -ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL() -{ - setFlightPhase(FlightPhase::HOVER_FLIGHT); -} bool ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix &matrix, bool force) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + + FlightPhase flight_phase{FlightPhase::HOVER_FLIGHT}; + + // Check if the current flight phase is HOVER or FIXED_WING + if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + flight_phase = FlightPhase::HOVER_FLIGHT; + + } else { + flight_phase = FlightPhase::FORWARD_FLIGHT; + } + + // Special cases for VTOL in transition + if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) { + if (vehicle_status.in_transition_to_fw) { + flight_phase = FlightPhase::TRANSITION_HF_TO_FF; + + } else { + flight_phase = FlightPhase::TRANSITION_FF_TO_HF; + } + } + + if (flight_phase != _flight_phase) { + _flight_phase = flight_phase; + _updated = true; + } + } + if (!(_updated || force)) { return false; } @@ -106,11 +132,3 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix +#include + class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness { public: - ActuatorEffectivenessTiltrotorVTOL(); + ActuatorEffectivenessTiltrotorVTOL() = default; virtual ~ActuatorEffectivenessTiltrotorVTOL() = default; bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force) override; - /** - * Set the current flight phase - * - * @param Flight phase - */ - void setFlightPhase(const FlightPhase &flight_phase) override; - int numActuators() const override { return 10; } protected: + + enum class FlightPhase { + HOVER_FLIGHT = 0, + FORWARD_FLIGHT = 1, + TRANSITION_HF_TO_FF = 2, + TRANSITION_FF_TO_HF = 3 + }; + + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _updated{true}; }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 86a2cfc8ed..bdbd84cad6 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -70,7 +70,6 @@ #pragma once #include -#include class ControlAllocation { @@ -81,7 +80,7 @@ public: static constexpr uint8_t NUM_ACTUATORS = 16; static constexpr uint8_t NUM_AXES = 6; - typedef matrix::Vector ActuatorVector; + using ActuatorVector = matrix::Vector; enum ControlAxis { ROLL = 0, diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 156147595f..d2f6f91b14 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -260,34 +260,6 @@ ControlAllocator::Run() return; } - vehicle_status_s vehicle_status; - - if (_vehicle_status_sub.update(&vehicle_status)) { - - ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT}; - - // Check if the current flight phase is HOVER or FIXED_WING - if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT; - - } else { - flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT; - } - - // Special cases for VTOL in transition - if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) { - if (vehicle_status.in_transition_to_fw) { - flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF; - - } else { - flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF; - } - } - - // Forward to effectiveness source - _actuator_effectiveness->setFlightPhase(flight_phase); - } - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const hrt_abstime now = hrt_absolute_time(); const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index b72e5bb17a..f725d7930e 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -68,7 +68,6 @@ #include #include #include -#include class ControlAllocator : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -147,7 +146,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; /**< airspeed subscription */ - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; matrix::Vector3f _torque_sp; matrix::Vector3f _thrust_sp;