From 45c8b648108dbe3b4986371414ff35b76f56188f Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 26 Jul 2022 15:45:22 +0300 Subject: [PATCH] hacks to make unified effectiveness matrix work Signed-off-by: RomanBapst --- .../init.d-posix/airframes/1040_standard_vtol | 8 +++--- .../ActuatorEffectiveness.hpp | 12 +++++++++ .../ActuatorEffectivenessControlSurfaces.cpp | 8 +++++- .../ActuatorEffectivenessStandardVTOL.cpp | 2 +- ...tuatorEffectivenessStandardVTOLUnified.cpp | 26 +++++-------------- ...tuatorEffectivenessStandardVTOLUnified.hpp | 8 +++--- .../control_allocator/ControlAllocator.cpp | 7 +++++ .../control_allocator/ControlAllocator.hpp | 2 ++ src/modules/vtol_att_control/standard.cpp | 26 ++++++++++++------- 9 files changed, 59 insertions(+), 40 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol index 1b67a5fa66..f31ce2fc44 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol @@ -12,8 +12,8 @@ param set-default FD_ACT_EN 0 param set-default FD_ACT_MOT_TOUT 500 -# param set-default SYS_CTRL_ALLOC 1 -param set-default CA_AIRFRAME 2 +param set-default SYS_CTRL_ALLOC 1 +param set-default CA_AIRFRAME 11 param set-default CA_ROTOR_COUNT 5 param set-default CA_ROTOR0_PX 0.1515 @@ -84,5 +84,5 @@ param set-default VT_FW_MOT_OFFID 1234 param set-default VT_B_TRANS_DUR 8 param set-default VT_TYPE 2 -set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix -set MIXER custom +#set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix +set MIXER skip diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 3a5793e582..1a51ba9a5f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -63,6 +63,7 @@ enum class EffectivenessUpdateReason { NO_EXTERNAL_UPDATE = 0, CONFIGURATION_UPDATE = 1, MOTOR_ACTIVATION_UPDATE = 2, + AIRSPEED_CHANGED = 3 }; @@ -133,6 +134,16 @@ public: _flight_phase = flight_phase; } + /** + * Set the equivalent airspeed + * + * @param Equivalent airspeed + */ + virtual void setEquivalentAirspeed(const float airspeed) + { + _airspeed_equivalent = airspeed; + } + /** * Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES. * This is expected to stay constant. @@ -198,4 +209,5 @@ public: protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase + float _airspeed_equivalent{NAN}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 95bf818320..cbbf6d43ca 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -34,6 +34,7 @@ #include #include "ActuatorEffectivenessControlSurfaces.hpp" +#include using namespace matrix; @@ -127,7 +128,12 @@ void ActuatorEffectivenessControlSurfaces::updateParams() bool ActuatorEffectivenessControlSurfaces::addActuators(Configuration &configuration) { for (int i = 0; i < _count; i++) { - int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{}); + const float airspeed_trim = 15.0f; + _airspeed_equivalent = math::max(_airspeed_equivalent, 3.0f); + float airspeed_scaling = _airspeed_equivalent / airspeed_trim; + airspeed_scaling = math::constrain(airspeed_scaling, 0.001f, 1000.0f); + Vector3f actual_torque = _params[i].torque * airspeed_scaling * airspeed_scaling; + int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, actual_torque, Vector3f{}); if (actuator_idx >= 0) { configuration.trim[configuration.selected_matrix](actuator_idx) = _params[i].trim; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 692f76e53c..5202edf86f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -89,7 +89,7 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors = _mc_motors_mask; + //_stopped_motors = _mc_motors_mask; break; case FlightPhase::HOVER_FLIGHT: diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.cpp index 4824bb444b..d8ac00b8cc 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.cpp @@ -50,34 +50,20 @@ bool ActuatorEffectivenessStandardVTOLUnified::getEffectivenessMatrix(Configurat return false; } - // TODO: combine _rotors and _control_surfaces into one matrix - - // TODO: airspeed - - - - - // _rotors.addActuators(configuration); - // - // int num_actuators = _rotors.computeEffectivenessMatrix(_rotors.geometry(), configuration.effectiveness_matrices[0], - // configuration.num_actuators_matrix[0]); - // configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); - - + if (external_update == EffectivenessUpdateReason::AIRSPEED_CHANGED) { + //printf("Updating due to airspeed %.2f\n", (double)_airspeed_equivalent); + _control_surfaces.setEquivalentAirspeed(_airspeed_equivalent); + } // Motors configuration.selected_matrix = 0; - _rotors.enablePropellerTorqueNonUpwards(false); - const bool mc_rotors_added_successfully = _rotors.addActuators(configuration); - _mc_motors_mask = _rotors.getUpwardsMotors(); // Control Surfaces - configuration.selected_matrix = 1; + configuration.selected_matrix = 0; _first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix]; - const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration); return (mc_rotors_added_successfully && surfaces_added_successfully); @@ -109,7 +95,7 @@ void ActuatorEffectivenessStandardVTOLUnified::setFlightPhase(const FlightPhase // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors = _mc_motors_mask; + //_stopped_motors = _mc_motors_mask; break; case FlightPhase::HOVER_FLIGHT: diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.hpp index 5b888c7572..723eff26dd 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.hpp @@ -60,15 +60,15 @@ public: void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override { - static_assert(MAX_NUM_MATRICES >= 2, "expecting at least 2 matrices"); - allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; - allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE; + //static_assert(MAX_NUM_MATRICES >= 2, "expecting at least 2 matrices"); + allocation_method_out[0] = AllocationMethod::PSEUDO_INVERSE; + //allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE; } void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override { normalize[0] = true; - normalize[1] = false; + //normalize[1] = false; } void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index b936844192..e9f949be48 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -365,6 +365,13 @@ ControlAllocator::Run() vehicle_torque_setpoint_s vehicle_torque_setpoint; vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + if (_airspeed_sub.updated()) { + airspeed_validated_s airspeed{}; + _airspeed_sub.copy(&airspeed); + _actuator_effectiveness->setEquivalentAirspeed(airspeed.calibrated_airspeed_m_s); + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::AIRSPEED_CHANGED); + } + // Run allocator on torque changes if (_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)) { _torque_sp = matrix::Vector3f(vehicle_torque_setpoint.xyz); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 6692dcbcd1..6e63f696f0 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -76,6 +76,7 @@ #include #include #include +#include class ControlAllocator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -186,6 +187,7 @@ private: uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; + uORB::Subscription _airspeed_sub{ORB_ID(airspeed_validated)}; matrix::Vector3f _torque_sp; matrix::Vector3f _thrust_sp; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 20cd0d0b2d..4752bef8dc 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -400,11 +400,17 @@ void Standard::fill_actuator_outputs() _torque_setpoint_0->xyz[1] = mc_out[actuator_controls_s::INDEX_PITCH]; _torque_setpoint_0->xyz[2] = mc_out[actuator_controls_s::INDEX_YAW]; - _torque_setpoint_1->timestamp = hrt_absolute_time(); - _torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - _torque_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_ROLL]; - _torque_setpoint_1->xyz[1] = fw_out[actuator_controls_s::INDEX_PITCH]; - _torque_setpoint_1->xyz[2] = fw_out[actuator_controls_s::INDEX_YAW]; + if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { + _torque_setpoint_0->xyz[0] = fw_out[actuator_controls_s::INDEX_ROLL]; + _torque_setpoint_0->xyz[1] = fw_out[actuator_controls_s::INDEX_PITCH]; + _torque_setpoint_0->xyz[2] = fw_out[actuator_controls_s::INDEX_YAW]; + } + + // _torque_setpoint_1->timestamp = hrt_absolute_time(); + // _torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + // _torque_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_ROLL]; + // _torque_setpoint_1->xyz[1] = fw_out[actuator_controls_s::INDEX_PITCH]; + // _torque_setpoint_1->xyz[2] = fw_out[actuator_controls_s::INDEX_YAW]; _thrust_setpoint_0->timestamp = hrt_absolute_time(); _thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; @@ -412,11 +418,11 @@ void Standard::fill_actuator_outputs() _thrust_setpoint_0->xyz[1] = 0.f; _thrust_setpoint_0->xyz[2] = -mc_out[actuator_controls_s::INDEX_THROTTLE]; - _thrust_setpoint_1->timestamp = hrt_absolute_time(); - _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - _thrust_setpoint_1->xyz[0] = 0.f; - _thrust_setpoint_1->xyz[1] = 0.f; - _thrust_setpoint_1->xyz[2] = 0.f; + // _thrust_setpoint_1->timestamp = hrt_absolute_time(); + // _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + // _thrust_setpoint_1->xyz[0] = 0.f; + // _thrust_setpoint_1->xyz[1] = 0.f; + // _thrust_setpoint_1->xyz[2] = 0.f; _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;