diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1050_standard_vtol_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/airframes/1050_standard_vtol_ctrlalloc index cbe5fb402e..6ad1bc34f9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1050_standard_vtol_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1050_standard_vtol_ctrlalloc @@ -6,7 +6,13 @@ # . ${R}etc/init.d/rc.vtol_defaults -. ${R}etc/init.d/rc.ctrlalloc + +param set-default SYS_CTRL_ALLOC 1 + +param set-default VM_MASS 1.5 +param set-default VM_INERTIA_XX 0.03 +param set-default VM_INERTIA_YY 0.03 +param set-default VM_INERTIA_ZZ 0.05 param set-default FW_L1_PERIOD 12 param set-default FW_MAN_P_MAX 30 @@ -49,7 +55,9 @@ param set-default VT_B_TRANS_DUR 8 param set-default VT_TYPE 2 param set-default CA_AIRFRAME 1 +param set-default CA_METHOD 1 +# VTOL Motors param set-default CA_ACT0_MIN 0.0 param set-default CA_ACT1_MIN 0.0 param set-default CA_ACT2_MIN 0.0 @@ -59,9 +67,11 @@ param set-default CA_ACT1_MAX 1.0 param set-default CA_ACT2_MAX 1.0 param set-default CA_ACT3_MAX 1.0 +# Throttle param set-default CA_ACT4_MIN 0.0 param set-default CA_ACT4_MAX 1.0 +# Ailerons/Ruddervator param set-default CA_ACT5_MIN -1.0 param set-default CA_ACT5_MAX 1.0 param set-default CA_ACT6_MIN -1.0 @@ -69,8 +79,32 @@ param set-default CA_ACT6_MAX 1.0 param set-default CA_ACT7_MIN -1.0 param set-default CA_ACT7_MAX 1.0 +param set-default CA_MC_R0_PX 0.177 +param set-default CA_MC_R0_PY 0.177 +param set-default CA_MC_R0_CT 6.5 +param set-default CA_MC_R0_KM 0.05 +param set-default CA_MC_R1_PX -0.177 +param set-default CA_MC_R1_PY -0.177 +param set-default CA_MC_R1_CT 6.5 +param set-default CA_MC_R1_KM 0.05 +param set-default CA_MC_R2_PX 0.177 +param set-default CA_MC_R2_PY -0.177 +param set-default CA_MC_R2_CT 6.5 +param set-default CA_MC_R2_KM -0.05 +param set-default CA_MC_R3_PX -0.177 +param set-default CA_MC_R3_PY 0.177 +param set-default CA_MC_R3_CT 6.5 +param set-default CA_MC_R3_KM -0.05 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 +param set-default PWM_MAIN_FUNC5 105 # Setting pusher to Motor5 for now +param set-default PWM_MAIN_FUNC6 201 +param set-default PWM_MAIN_FUNC7 202 +param set-default PWM_MAIN_FUNC8 203 + set MAV_TYPE 22 -#set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix -#set MIXER custom -set MIXER direct +set MIXER skip diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 97ddee2b4b..6a7c9465d4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -15,6 +15,22 @@ ekf2 start & # End Estimator group selection # ############################################################################### +if param compare SYS_CTRL_ALLOC 1 +then + # + # Start Control Allocator + # + control_allocator start + + # + # Disable hover thrust estimator and prearming + # These features are currently incompatible with control allocation + # + # TODO: fix + # + param set MPC_USE_HTE 0 +fi + airspeed_selector start vtol_att_control start diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 68fbc6dd9d..625c9b72d6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -44,6 +44,7 @@ #include #include +#include #include class ActuatorEffectiveness @@ -104,6 +105,11 @@ public: */ virtual int numActuators() const = 0; + /** + * Get the function of an actuator + */ + virtual int actuatorFunction(uint8_t idx) const = 0; + protected: matrix::Vector _trim; ///< Actuator trim FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index 0ff86f45e1..7b441886fb 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -78,6 +78,8 @@ public: bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force) override; int numActuators() const override { return _num_actuators; } + + int actuatorFunction(uint8_t idx) const override { return (int)OutputFunction::Motor1 + idx; } private: bool _updated{true}; int _num_actuators{0}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index e3de20e583..6ff552e75a 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -58,7 +58,15 @@ public: */ void setFlightPhase(const FlightPhase &flight_phase) override; - int numActuators() const override { return 7; } + int numActuators() const override { return 8; } + + int actuatorFunction(uint8_t idx) const override { return (int)_actuator_functions[idx]; } + protected: + OutputFunction _actuator_functions[NUM_ACTUATORS] = { + OutputFunction::Motor1, OutputFunction::Motor2, OutputFunction::Motor3, OutputFunction::Motor4, + OutputFunction::Motor5, OutputFunction::Servo1, OutputFunction::Servo2, OutputFunction::Servo3 + }; + bool _updated{true}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 1204fc8e86..99ca8d3b44 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -59,6 +59,8 @@ public: void setFlightPhase(const FlightPhase &flight_phase) override; int numActuators() const override { return 10; } + + int actuatorFunction(uint8_t idx) const { return (int)OutputFunction::Motor1 + idx; }; /// TODO: This is wrong. Someone familiar with the tiltrotor should fix this. protected: bool _updated{true}; }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 156147595f..cdfb85c62d 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -436,21 +436,35 @@ ControlAllocator::publish_control_allocator_status() void ControlAllocator::publish_legacy_actuator_controls() { - actuator_motors_s actuator_motors; - actuator_motors.timestamp = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); + + actuator_motors_s actuator_motors {}; + actuator_motors.timestamp = now; actuator_motors.timestamp_sample = _timestamp_sample; + actuator_servos_s actuator_servos {}; + actuator_servos.timestamp = now; + actuator_servos.timestamp_sample = _timestamp_sample; + matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); matrix::Vector actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint( actuator_sp); - for (size_t i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) { - actuator_motors.control[i] = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN; + for (size_t i = 0; i < NUM_ACTUATORS; i++) { + const int func = _actuator_effectiveness->actuatorFunction(i); + const float control = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN; + + if (func >= (int)OutputFunction::Motor1 && func <= (int)OutputFunction::MotorMax) { + actuator_motors.control[func - (int)OutputFunction::Motor1] = control; + } + + if (func >= (int)OutputFunction::Servo1 && func <= (int)OutputFunction::ServoMax) { + actuator_servos.control[func - (int)OutputFunction::Servo1] = control; + } } _actuator_motors_pub.publish(actuator_motors); - - // TODO: servos + _actuator_servos_pub.publish(actuator_servos); } int ControlAllocator::task_spawn(int argc, char *argv[])