diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 461436e80f..ccc7e7eeb1 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -35,6 +35,7 @@ #include using namespace matrix; +using namespace time_literals; ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *parent) : ModuleParams(parent) @@ -59,6 +60,7 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p _param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S"); _param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S"); + _param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME"); updateParams(); } @@ -90,6 +92,7 @@ void ActuatorEffectivenessHelicopter::updateParams() param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale); param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale); + param_get(_param_handles.spoolup_time, &_geometry.spoolup_time); } bool @@ -124,6 +127,22 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector +#include +#include + class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness { public: @@ -56,6 +59,7 @@ public: float pitch_curve[NUM_CURVE_POINTS]; float yaw_collective_pitch_scale; float yaw_throttle_scale; + float spoolup_time; }; ActuatorEffectivenessHelicopter(ModuleParams *parent); @@ -84,10 +88,16 @@ private: param_t pitch_curve[NUM_CURVE_POINTS]; param_t yaw_collective_pitch_scale; param_t yaw_throttle_scale; + param_t spoolup_time; }; ParamHandles _param_handles{}; Geometry _geometry{}; int _first_swash_plate_servo_index{}; + + // Throttle spoolup state + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _armed{false}; + uint64_t _armed_time{0}; };