diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index 2add7a5cb4b1..7bb2e017dbad 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -32,6 +32,20 @@ param set-default CA_ROTOR3_PX -0.13 param set-default CA_ROTOR3_PY 0.20 param set-default CA_ROTOR3_KM -0.05 +# INDI rate control parameters +param set-default MET_ROTOR0_CT 0.0001047 +param set-default MET_ROTOR0_KM 0.0005 +param set-default MET_ROTOR0_IZZ 2.649858234714004e-05 +param set-default MET_ROTOR1_CT 0.0001047 +param set-default MET_ROTOR1_KM 0.0005 +param set-default MET_ROTOR1_IZZ 2.649858234714004e-05 +param set-default MET_ROTOR2_CT 0.0001047 +param set-default MET_ROTOR2_KM -0.0005 +param set-default MET_ROTOR2_IZZ 2.649858234714004e-05 +param set-default MET_ROTOR3_CT 0.0001047 +param set-default MET_ROTOR3_KM -0.0005 +param set-default MET_ROTOR3_IZZ 2.649858234714004e-05 + param set-default SIM_GZ_EC_FUNC1 101 param set-default SIM_GZ_EC_FUNC2 102 param set-default SIM_GZ_EC_FUNC3 103 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index d5d9989c8666..a89581dae3c6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -13,7 +13,13 @@ control_allocator start # # Start Multicopter Rate Controller. # -mc_rate_control start +if param greater -s MC_INDI_EN 0 +then + mc_indi_rate_control start +else + mc_rate_control start +fi + # # Start Multicopter Attitude Controller. diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index ebe4f4ca5b89..950882866631 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -39,6 +39,7 @@ CONFIG_MAVLINK_DIALECT="development" CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_INDI_RATE_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fdc23a889f11..765b0f51d781 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -118,6 +118,7 @@ set(msg_files InputRc.msg InternalCombustionEngineControl.msg InternalCombustionEngineStatus.msg + IndiAdaptationStatus.msg IridiumsbdStatus.msg IrlockReport.msg LandingGear.msg diff --git a/msg/IndiAdaptationStatus.msg b/msg/IndiAdaptationStatus.msg new file mode 100644 index 000000000000..61c855ed4a43 --- /dev/null +++ b/msg/IndiAdaptationStatus.msg @@ -0,0 +1,24 @@ +# INDI Adaptation Status Message +# Tracks changes in G matrices during INDI adaptation + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data used for adaptation + +# G1 matrix (moment part) - 3x16 matrix flattened +float32[48] g1_matrix # G1 matrix elements (3 rows * 16 columns) + +# G2 matrix (gyroscopic part) - 3x16 matrix flattened +float32[48] g2_matrix # G2 matrix elements (3 rows * 16 columns) + +# Adaptation inputs +float32[16] rotor_speeds # Current rotor speeds (rad/s) +float32[16] rotor_speed_dots # Rotor speed derivatives (rad/s^2) +float32[3] angular_accel_delta # Angular acceleration delta (rad/s^2) + +# Adaptation constants +float32[3] g1_adaptive_constants # G1 adaptation constants +float32[3] g2_adaptive_constants # G2 adaptation constants + +# Status flags +bool adaptation_enabled # Whether adaptation is currently enabled +uint8 num_actuators # Number of actuators being adapted diff --git a/msg/VehicleTorqueSetpoint.msg b/msg/VehicleTorqueSetpoint.msg index c20519b16e3d..307d4d3aca3d 100644 --- a/msg/VehicleTorqueSetpoint.msg +++ b/msg/VehicleTorqueSetpoint.msg @@ -3,6 +3,11 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) float32[3] xyz # torque setpoint about X, Y, Z body axis (normalized) +float32[3] g1_term # G1 term of torque setpoint about X, Y, Z body axis (normalized) +float32[3] g2_term # G2 term of torque setpoint about X, Y, Z body axis (normalized) +float32[3] pid_xyz # PID torque setpoint about X, Y, Z body axis (normalized) +float32[3] indi_torque_xyz # INDI torque setpoint about X, Y, Z body axis (normalized) +float32[3] measured_body_torque_xyz # measured body torque about X, Y, Z body axis (normalized) # TOPICS vehicle_torque_setpoint # TOPICS vehicle_torque_setpoint_virtual_fw vehicle_torque_setpoint_virtual_mc diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 597b4abf4e4e..6cdfd8802778 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -214,6 +214,7 @@ void LoggedTopics::add_default_topics() add_topic("actuator_servos", 100); add_topic_multi("vehicle_thrust_setpoint", 20, 2); add_topic_multi("vehicle_torque_setpoint", 20, 2); + add_topic("indi_adaptation_status", 100); // SYS_HITL: default ground truth logging for simulation int32_t sys_hitl = 0; diff --git a/src/modules/mc_indi_rate_control/ActuatorEffectivenessRotorsINDI.cpp b/src/modules/mc_indi_rate_control/ActuatorEffectivenessRotorsINDI.cpp new file mode 100644 index 000000000000..02d76cc69928 --- /dev/null +++ b/src/modules/mc_indi_rate_control/ActuatorEffectivenessRotorsINDI.cpp @@ -0,0 +1,337 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessRotorsINDI.cpp + * + * Actuator effectivness for model based INDI rate control + * Modified from ActuatorEffectivenessRotors.cpp + * TODO: Add tilt support, removed for now + * + * @author Rohan Inamdar + */ + +#include "ActuatorEffectivenessRotorsINDI.hpp" + +using namespace matrix; + +ActuatorEffectivenessRotorsINDI::ActuatorEffectivenessRotorsINDI(ModuleParams *parent, AxisConfiguration axis_config) + : ModuleParams(parent), _axis_config(axis_config) +{ + for (int i = 0; i < NUM_ACTUATORS; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PX", i); + _param_handles[i].position_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PY", i); + _param_handles[i].position_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PZ", i); + _param_handles[i].position_z = param_find(buffer); + + if (_axis_config == AxisConfiguration::Configurable) { + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AX", i); + _param_handles[i].axis_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AY", i); + _param_handles[i].axis_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AZ", i); + _param_handles[i].axis_z = param_find(buffer); + } + + snprintf(buffer, sizeof(buffer), "MET_ROTOR%u_CT", i); + _param_handles[i].thrust_coef = param_find(buffer); + snprintf(buffer, sizeof(buffer), "MET_ROTOR%u_KM", i); + _param_handles[i].moment_ratio = param_find(buffer); + snprintf(buffer, sizeof(buffer), "MET_ROTOR%u_IZZ", i); + _param_handles[i].moment_inertia = param_find(buffer); + snprintf(buffer, sizeof(buffer), "INDI_G1_ADAPT_%u", i); + _param_handles[i].g1_adaptive_constant = param_find(buffer); + snprintf(buffer, sizeof(buffer), "INDI_G2_ADAPT_%u", i); + _param_handles[i].g2_adaptive_constant = param_find(buffer); + } + + updateParams(); +} + +void ActuatorEffectivenessRotorsINDI::updateParams() +{ + ModuleParams::updateParams(); + + _geometry.num_rotors = math::min(NUM_ACTUATORS, static_cast(_param_rotor_count.get())); + + for (int i = 0; i < _geometry.num_rotors; ++i) { + Vector3f &position = _geometry.rotors[i].position; + param_get(_param_handles[i].position_x, &position(0)); + param_get(_param_handles[i].position_y, &position(1)); + param_get(_param_handles[i].position_z, &position(2)); + + Vector3f &axis = _geometry.rotors[i].axis; + + switch (_axis_config) { + case AxisConfiguration::Configurable: + param_get(_param_handles[i].axis_x, &axis(0)); + param_get(_param_handles[i].axis_y, &axis(1)); + param_get(_param_handles[i].axis_z, &axis(2)); + break; + + case AxisConfiguration::FixedForward: + axis = Vector3f(1.f, 0.f, 0.f); + break; + + case AxisConfiguration::FixedUpwards: + axis = Vector3f(0.f, 0.f, -1.f); + break; + } + + param_get(_param_handles[i].thrust_coef, &_geometry.rotors[i].thrust_coef); + param_get(_param_handles[i].moment_ratio, &_geometry.rotors[i].moment_ratio); + param_get(_param_handles[i].moment_inertia, &_geometry.rotors[i].moment_inertia); + + float g1_value, g2_value; + param_get(_param_handles[i].g1_adaptive_constant, &g1_value); + param_get(_param_handles[i].g2_adaptive_constant, &g2_value); + _G1_adaptive_constants(i, i) = g1_value; + _G2_adaptive_constants(i, i) = g2_value; + } + + _adaptive_constants_per_axis = diag(Vector3f(_param_indi_adapt_axis_r.get(), _param_indi_adapt_axis_p.get(), + _param_indi_adapt_axis_y.get())); + +} + +bool +ActuatorEffectivenessRotorsINDI::addActuators(Configuration &configuration) +{ + if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) { + PX4_ERR("Wrong actuator ordering: servos need to be after motors"); + return false; + } + + int num_actuators = computeEffectivenessMatrix(_geometry, + configuration.effectiveness_matrices[0], + configuration.effectiveness_matrices[1], + configuration.num_actuators_matrix[configuration.selected_matrix]); + configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); + return true; +} + +int +ActuatorEffectivenessRotorsINDI::computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &G1, EffectivenessMatrix &G2, int actuator_start_index) +{ + int num_actuators = 0; + + for (int i = 0; i < geometry.num_rotors; i++) { + + if (i + actuator_start_index >= NUM_ACTUATORS) { + break; + } + + ++num_actuators; + + // Get rotor axis + Vector3f axis = geometry.rotors[i].axis; + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + PX4_ERR("Bad axis definition, ignoring rotor %d", i); + continue; + } + + // Get rotor position + const Vector3f &position = geometry.rotors[i].position; + + // Get coefficients + float ct = geometry.rotors[i].thrust_coef; + float km = geometry.rotors[i].moment_ratio; + + if (geometry.propeller_torque_disabled) { + PX4_ERR("Propeller torque disabled, setting moment ratio to 0, this is not supported for INDI rate control"); + km = 0.f; + } + + if (geometry.propeller_torque_disabled_non_upwards) { + PX4_ERR("Propeller torque disabled non upwards, setting moment ratio to 0, this is not supported for INDI rate control"); + bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f; + + if (!upwards) { + km = 0.f; + } + } + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // Compute thrustTorque induced moment generated by this rotor + matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis; + + // Compute gyroscopic moment generated by this rotor + matrix::Vector3f gyroscopic_moment = geometry.rotors[i].moment_inertia * sign(km) * axis; + + // Fill corresponding items in effectiveness matrix + for (size_t j = 0; j < 3; j++) { + // note that thrust is not needed for indi so it is not included/calculated + G1(j + 3, i + actuator_start_index) = moment(j); + + G2(j + 3, i + actuator_start_index) = gyroscopic_moment(j); + } + } + + PX4_INFO("computeEffectivenessMatrix: Completed with %d actuators", num_actuators); + + // Debug: Print the computed matrices + PX4_INFO("G1 matrix (moment part):"); + + for (int i = 0; i < 3; i++) { + PX4_INFO(" Row %d: [%.10f, %.10f, %.10f, %.10f]", i, + (double)G1(i + 3, 0), (double)G1(i + 3, 1), (double)G1(i + 3, 2), (double)G1(i + 3, 3)); + } + + PX4_INFO("G2 matrix (gyroscopic part):"); + + for (int i = 0; i < 3; i++) { + PX4_INFO(" Row %d: [%.10f, %.10f, %.10f, %.10f]", i, + (double)G2(i + 3, 0), (double)G2(i + 3, 1), (double)G2(i + 3, 2), (double)G2(i + 3, 3)); + } + + return num_actuators; +} + +uint32_t ActuatorEffectivenessRotorsINDI::getMotors() const +{ + uint32_t motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + motors |= 1u << i; + } + + return motors; +} + +bool +ActuatorEffectivenessRotorsINDI::initializeEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + + return addActuators(configuration); +} + +/** + * @brief Adapt the effectiveness matrix to the current motor speeds and angular accelerations to account for changes in the system (weigth changes, battery voltage changes, etc.) + * @details Implementation from "Adaptive Incremental nonlinear Dynamic Inversion for Attitude Contol of Micro Air Vehicles" by Smeur et al + * @param configuration The configuration object + * @param delta_motor_speeds The change in motor speeds + * @param delta_dot_motor_speeds The change in motor speed derivatives + * @param filtered_angular_accel The filtered angular acceleration + */ +void +ActuatorEffectivenessRotorsINDI::adaptEffectivenessMatrix(Configuration &configuration, + Vector &delta_motor_speeds, + Vector &delta_dot_motor_speeds, + Vector3f &delta_filtered_angular_accel) +{ + // for readability + // use auto to point to the slice object that acts a reference, and allows to not need to reassign the new values + Matrix G1_moment = getG1( + configuration); //G1_moment is the moment part of G1 (the bottom 3 rows) + Matrix G2_moment = getG2( + configuration); //G2_moment is the moment part of G2 (the bottom 3 rows) + + // split up the equation into g1 and g2, as recombining G = [G1, G2] is not computationally necessary as done in the paper with the current layout of the effectiveness matricies + // note: mu2 = apative_constants_per_axis, mu1 is split between the two matricies: g1_adaptive_constants and g2_adaptive_constants + G1_moment = G1_moment - _adaptive_constants_per_axis * (G1_moment * delta_motor_speeds - delta_filtered_angular_accel) * + delta_motor_speeds.transpose() * _G1_adaptive_constants; + G2_moment = G2_moment - _adaptive_constants_per_axis * (G2_moment * delta_dot_motor_speeds - + delta_filtered_angular_accel) * delta_dot_motor_speeds.transpose() * _G2_adaptive_constants; + + // Publish adaptation status + publishAdaptationStatus(delta_motor_speeds, delta_dot_motor_speeds, delta_filtered_angular_accel, G1_moment, G2_moment); + +} + +void ActuatorEffectivenessRotorsINDI::publishAdaptationStatus(const Vector &delta_motor_speeds, + const Vector &delta_dot_motor_speeds, + const Vector3f &delta_filtered_angular_accel, + const Matrix &G1, + const Matrix &G2) +{ + _indi_adaptation_status.timestamp = hrt_absolute_time(); + _indi_adaptation_status.timestamp_sample = hrt_absolute_time(); + + // Copy G1 matrix (3xN) to flattened array + for (int row = 0; row < 3; row++) { + for (int col = 0; col < NUM_ACTUATORS; col++) { + _indi_adaptation_status.g1_matrix[row * _geometry.num_rotors + col] = G1(row, col); + } + } + + // Copy G2 matrix (3xN) to flattened array + for (int row = 0; row < 3; row++) { + for (int col = 0; col < NUM_ACTUATORS; col++) { + _indi_adaptation_status.g2_matrix[row * _geometry.num_rotors + col] = G2(row, col); + } + } + + // Copy rotor speeds and derivatives + for (int i = 0; i < NUM_ACTUATORS; i++) { + _indi_adaptation_status.rotor_speeds[i] = delta_motor_speeds(i); + _indi_adaptation_status.rotor_speed_dots[i] = delta_dot_motor_speeds(i); + } + + // Copy angular acceleration delta + _indi_adaptation_status.angular_accel_delta[0] = delta_filtered_angular_accel(0); // TODO: check if this is correct + _indi_adaptation_status.angular_accel_delta[1] = delta_filtered_angular_accel(1); // TODO: check if this is correct + _indi_adaptation_status.angular_accel_delta[2] = delta_filtered_angular_accel(2); // TODO: check if this is correct + + // Copy adaptation constants + _indi_adaptation_status.g1_adaptive_constants[0] = _G1_adaptive_constants(0, 0); + _indi_adaptation_status.g1_adaptive_constants[1] = _G1_adaptive_constants(1, 1); + _indi_adaptation_status.g1_adaptive_constants[2] = _G1_adaptive_constants(2, 2); + _indi_adaptation_status.g2_adaptive_constants[0] = _G2_adaptive_constants(0, 0); + _indi_adaptation_status.g2_adaptive_constants[1] = _G2_adaptive_constants(1, 1); + _indi_adaptation_status.g2_adaptive_constants[2] = _G2_adaptive_constants(2, 2); + + _indi_adaptation_status.adaptation_enabled = true; + _indi_adaptation_status.num_actuators = _geometry.num_rotors; + + _indi_adaptation_status_pub.publish(_indi_adaptation_status); +} diff --git a/src/modules/mc_indi_rate_control/ActuatorEffectivenessRotorsINDI.hpp b/src/modules/mc_indi_rate_control/ActuatorEffectivenessRotorsINDI.hpp new file mode 100644 index 000000000000..5ca0ff01b833 --- /dev/null +++ b/src/modules/mc_indi_rate_control/ActuatorEffectivenessRotorsINDI.hpp @@ -0,0 +1,160 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessRotorsINDI.hpp + * + * Actuator effectivness for model based INDI rate control + * Modified from ActuatorEffectivenessRotorsINDI.hpp + * + * @author Rohan Inamdar + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +class ActuatorEffectivenessTilts; + +using namespace time_literals; +using namespace matrix; + +class ActuatorEffectivenessRotorsINDI : public ModuleParams, public ActuatorEffectiveness +{ +public: + enum class AxisConfiguration { + Configurable, ///< axis can be configured + FixedForward, ///< axis is fixed, pointing forwards (positive X) + FixedUpwards, ///< axis is fixed, pointing upwards (negative Z) + }; + + struct RotorGeometry { + Vector3f position; + Vector3f axis; + float thrust_coef; + float moment_ratio; + float moment_inertia; + }; + + struct Geometry { + RotorGeometry rotors[NUM_ACTUATORS]; + int num_rotors{0}; + bool propeller_torque_disabled{false}; + bool yaw_by_differential_thrust_disabled{false}; + bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors + }; + + ActuatorEffectivenessRotorsINDI(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable); + virtual ~ActuatorEffectivenessRotorsINDI() = default; + + bool initializeEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update); + + void adaptEffectivenessMatrix(Configuration &configuration, Vector &delta_motor_speeds, + Vector &delta_dot_motor_speeds, Vector3f &filtered_angular_accel); + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; + } + + static int computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &G1, EffectivenessMatrix &G2, int actuator_start_index = 0); + + Slice getG1(Configuration &configuration) const {return configuration.effectiveness_matrices[0].slice<3, NUM_ACTUATORS>(3, 0);} + Slice getG2(Configuration &configuration) const {return configuration.effectiveness_matrices[1].slice<3, NUM_ACTUATORS>(3, 0);} + + bool addActuators(Configuration &configuration); + + const char *name() const override { return "RotorsINDI"; } + + const Geometry &geometry() const { return _geometry; } + + uint32_t getMotors() const; + uint32_t getUpwardsMotors() const; + uint32_t getForwardsMotors() const; + + void publishAdaptationStatus(const Vector &delta_motor_speeds, + const Vector &delta_dot_motor_speeds, + const Vector3f &filtered_angular_accel, + const Matrix &G1, + const Matrix &G2); + +private: + void updateParams() override; + const AxisConfiguration _axis_config; + + struct ParamHandles { + param_t position_x; + param_t position_y; + param_t position_z; + param_t axis_x; + param_t axis_y; + param_t axis_z; + param_t thrust_coef; + param_t moment_ratio; + param_t moment_inertia; + param_t g1_adaptive_constant; + param_t g2_adaptive_constant; + }; + ParamHandles _param_handles[NUM_ACTUATORS]; + + // for adaptive effectiveness matrix from paper: "Adaptive Incremental nonlinear Dynamic Inversion for Attitude Contol of Micro Air Vehicles" by Smeur et al + // these are the adaptive constants for the G1 and G2 matrices, and the apative constants per axis + // these are the constants that are used to adapt the effectiveness matrix to the current motor speeds and angular accelerations to account for changes in the system (weigth changes, battery voltage changes, etc.) + // mu1 is split between the two matricies: g1_adaptive_constants and g2_adaptive_constants + // mu2 = apative_constants_per_axis + // larger values = fasater adaptation rate but too large values can cause instability + // tuning is required to find the optimal values + SquareMatrix _G1_adaptive_constants; + SquareMatrix _G2_adaptive_constants; + + SquareMatrix _adaptive_constants_per_axis; + + Geometry _geometry{}; + + DEFINE_PARAMETERS( + (ParamInt) _param_rotor_count, + (ParamFloat) _param_indi_adapt_axis_r, + (ParamFloat) _param_indi_adapt_axis_p, + (ParamFloat) _param_indi_adapt_axis_y + ) + + uORB::Publication _indi_adaptation_status_pub{ORB_ID(indi_adaptation_status)}; + indi_adaptation_status_s _indi_adaptation_status{}; +}; diff --git a/src/modules/mc_indi_rate_control/CMakeLists.txt b/src/modules/mc_indi_rate_control/CMakeLists.txt new file mode 100644 index 000000000000..2cc5948148ef --- /dev/null +++ b/src/modules/mc_indi_rate_control/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################ +# +# Copyright (c) 2025 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__mc_indi_rate_control + MAIN mc_indi_rate_control + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + MulticopterINDIRateControl.cpp + MulticopterINDIRateControl.hpp + ActuatorEffectivenessRotorsINDI.cpp + ActuatorEffectivenessRotorsINDI.hpp + MODULE_CONFIG + module.yaml + DEPENDS + circuit_breaker + mathlib + px4_work_queue + RateControl + ) diff --git a/src/modules/mc_indi_rate_control/Kconfig b/src/modules/mc_indi_rate_control/Kconfig new file mode 100644 index 000000000000..fee7616cf0cb --- /dev/null +++ b/src/modules/mc_indi_rate_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_MC_INDI_RATE_CONTROL + bool "mc_indi_rate_control" + default n + ---help--- + Enable support for mc_indi_rate_control + +menuconfig USER_MC_INDI_RATE_CONTROL + bool "mc_indi_rate_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_MC_INDI_RATE_CONTROL + ---help--- + Put mc_indi_rate_control in userspace memory diff --git a/src/modules/mc_indi_rate_control/MulticopterINDIRateControl.cpp b/src/modules/mc_indi_rate_control/MulticopterINDIRateControl.cpp new file mode 100644 index 000000000000..9490d489e822 --- /dev/null +++ b/src/modules/mc_indi_rate_control/MulticopterINDIRateControl.cpp @@ -0,0 +1,463 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file MulticopterINDIRateControl.cpp + * + * Multicopter INDI rate control + * + * @author Rohan Inamdar + */ + +#include "MulticopterINDIRateControl.hpp" + +#include +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +MulticopterINDIRateControl::MulticopterINDIRateControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _rotors(this), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + parameters_updated(); + _controller_status_pub.advertise(); +} + +MulticopterINDIRateControl::~MulticopterINDIRateControl() +{ + perf_free(_loop_perf); +} + +bool +MulticopterINDIRateControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + if (!_rotors.initializeEffectivenessMatrix(_actuator_effectiveness_config, + EffectivenessUpdateReason::CONFIGURATION_UPDATE)) { + PX4_ERR("Failed to initialize INDI effectiveness matrix"); + return false; + } + + // Get the number of actuators from the configuration + _num_actuators = _actuator_effectiveness_config.num_actuators_matrix[_actuator_effectiveness_config.selected_matrix]; + PX4_INFO("INDI: Initialized with %d actuators", _num_actuators); + + return true; +} + +void +MulticopterINDIRateControl::parameters_updated() +{ + // rate control parameters + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); + + _rate_control.setPidGains( + rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())), + rate_k.emult(Vector3f(0, 0, 0)), //disable integral control (set to Ki = 0 to not effect the rate control library) + rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()))); + + _rate_control.setIntegratorLimit( + Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get())); + + _rate_control.setFeedForwardGain( + Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); + + + // manual rate control acro mode rate limits + _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), + radians(_param_mc_acro_y_max.get())); + + _output_lpf_yaw.setCutoffFreq(_param_mc_yaw_tq_cutoff.get()); + + //FIXME: find the delta t correctly bruh + if (_param_imu_gyro_cutoff.get() > 0.f) { + // The perf_mean() function returns the average elapsed time of the counter in seconds. + const float avg_interval_s = perf_mean(_loop_perf) > 0.f ? perf_mean(_loop_perf) : 0.001f; //default to 1 kHz + + if (avg_interval_s > 0.f) { // Avoid division by zero if the interval is not yet measured + const float sample_rate_hz = 1.f / avg_interval_s; + + for (auto &radps_lpf : _radps_lpf) { + radps_lpf.set_cutoff_frequency(sample_rate_hz, _param_imu_gyro_cutoff.get()); + } + } + } +} + +void +MulticopterINDIRateControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + Vector filtered_radps_vec; + Vector filtered_radps_vec_dot; + + esc_status_s esc_status; + + if (_esc_status_sub.update(&esc_status)) { + for (int i = 0; i < _num_actuators; i++) { + _prev_esc_rad_per_sec_filtered(i) = filtered_radps_vec(i); + _prev_esc_rad_per_sec_filtered_dot(i) = filtered_radps_vec_dot(i); + float rad_per_sec = (float)(esc_status.esc[i].esc_rpm) * 2.f * M_PI_F / 60.f; + filtered_radps_vec(i) = _radps_lpf[i].apply(rad_per_sec); + const float dt = math::constrain(((esc_status.timestamp - _last_esc_status_update) * 1e-6f), 0.000125f, 0.02f); + filtered_radps_vec_dot(i) = (filtered_radps_vec(i) - _prev_esc_rad_per_sec_filtered(i)) / dt; + + } + + _last_esc_status_update = esc_status.timestamp; + } + + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + + const hrt_abstime now = angular_velocity.timestamp_sample; + + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + const Vector3f angular_accel{angular_velocity.xyz_derivative}; + + /* check for updates in other topics */ + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + } + } + + if (_param_mc_indi_adapt_en.get()) { + + matrix::Vector3f angular_accel_delta = angular_accel - _prev_angular_accel; + matrix::Vector delta_radps_vec = filtered_radps_vec - + _prev_esc_rad_per_sec_filtered; + matrix::Vector delta_radps_vec_dot = filtered_radps_vec_dot - + _prev_esc_rad_per_sec_filtered_dot; + _rotors.adaptEffectivenessMatrix(_actuator_effectiveness_config, delta_radps_vec, delta_radps_vec_dot, + angular_accel_delta); + + } + + // use rates setpoint topic + vehicle_rates_setpoint_s vehicle_rates_setpoint{}; + + if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { + // generate the rate setpoint from sticks + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + // manual rates control - ACRO mode + const Vector3f man_rate_sp{ + math::superexpo(manual_control_setpoint.roll, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-manual_control_setpoint.pitch, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(manual_control_setpoint.yaw, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; + + _rates_setpoint = man_rate_sp.emult(_acro_rate_max); + _thrust_setpoint(2) = -(manual_control_setpoint.throttle + 1.f) * .5f; + _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; + + // publish rate setpoint + vehicle_rates_setpoint.roll = _rates_setpoint(0); + vehicle_rates_setpoint.pitch = _rates_setpoint(1); + vehicle_rates_setpoint.yaw = _rates_setpoint(2); + _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); + vehicle_rates_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); + } + + } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { + _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); + _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); + _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); + _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); + } + } + + // run the rate controller + if (_vehicle_control_mode.flag_control_rates_enabled) { + + // reset integral if disarmed + if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _rate_control.resetIntegral(); + } + + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } + } + + // TODO: send the unallocated value directly for better anti-windup + _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + } + + // run rate controller + Vector3f torque_setpoint = + _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); + + Matrix G1 = _rotors.getG1(_actuator_effectiveness_config); + Matrix G2 = _rotors.getG2(_actuator_effectiveness_config); + Vector3f indi_torque_setpoint = computeIndiTorqueSetpoint(filtered_radps_vec, _prev_esc_rad_per_sec_filtered, dt, G1, + G2); + + // apply low-pass filtering on yaw axis to reduce high frequency torque caused by rotor acceleration + torque_setpoint(2) = _output_lpf_yaw.update(torque_setpoint(2), dt); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + + + _vehicle_torque_setpoint.pid_xyz[0] = PX4_ISFINITE(torque_setpoint(0)) ? torque_setpoint(0) : 0.f; + _vehicle_torque_setpoint.pid_xyz[1] = PX4_ISFINITE(torque_setpoint(1)) ? torque_setpoint(1) : 0.f; + _vehicle_torque_setpoint.pid_xyz[2] = PX4_ISFINITE(torque_setpoint(2)) ? torque_setpoint(2) : 0.f; + + Vector3f measured_body_torque = _Iv * angular_accel; + + + // NOTE: to confirm tuning, the indi_torque_xyz should be the same as the measured_body_torque_xyz when graphed in plotjuggler + _vehicle_torque_setpoint.indi_torque_xyz[0] = PX4_ISFINITE(indi_torque_setpoint(0)) ? indi_torque_setpoint(0) : 0.f; + _vehicle_torque_setpoint.indi_torque_xyz[1] = PX4_ISFINITE(indi_torque_setpoint(1)) ? indi_torque_setpoint(1) : 0.f; + _vehicle_torque_setpoint.indi_torque_xyz[2] = PX4_ISFINITE(indi_torque_setpoint(2)) ? indi_torque_setpoint(2) : 0.f; + + _vehicle_torque_setpoint.xyz[0] = _vehicle_torque_setpoint.pid_xyz[0] + _vehicle_torque_setpoint.indi_torque_xyz[0]; + _vehicle_torque_setpoint.xyz[1] = _vehicle_torque_setpoint.pid_xyz[1] + _vehicle_torque_setpoint.indi_torque_xyz[1]; + _vehicle_torque_setpoint.xyz[2] = _vehicle_torque_setpoint.pid_xyz[2] + _vehicle_torque_setpoint.indi_torque_xyz[2]; + + _vehicle_torque_setpoint.measured_body_torque_xyz[0] = PX4_ISFINITE(measured_body_torque(0)) ? measured_body_torque( + 0) : 0.f; + _vehicle_torque_setpoint.measured_body_torque_xyz[1] = PX4_ISFINITE(measured_body_torque(1)) ? measured_body_torque( + 1) : 0.f; + _vehicle_torque_setpoint.measured_body_torque_xyz[2] = PX4_ISFINITE(measured_body_torque(2)) ? measured_body_torque( + 2) : 0.f; + + _prev_esc_rad_per_sec_filtered = filtered_radps_vec; + _prev_angular_accel = angular_accel; + + + // scale setpoints by battery status if enabled + if (_param_mc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.f) { + for (int i = 0; i < 3; i++) { + vehicle_thrust_setpoint.xyz[i] = math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + _vehicle_torque_setpoint.xyz[i] = math::constrain(_vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + } + } + } + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + + _vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + _vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint); + + updateActuatorControlsStatus(_vehicle_torque_setpoint, dt); + + } + } + + perf_end(_loop_perf); +} + +Vector3f MulticopterINDIRateControl::computeIndiTorqueSetpoint(const Vector + &filtered_radps_vec, + const Vector &prev_esc_rad_per_sec_filtered, + float dt, + const Matrix &G1, + const Matrix &G2) +{ + Vector radps_vec_squared = filtered_radps_vec.emult(filtered_radps_vec); + Vector3f G1_term = (G1 * (radps_vec_squared)); + _vehicle_torque_setpoint.g1_term[0] = G1_term(0); + _vehicle_torque_setpoint.g1_term[1] = G1_term(1); + _vehicle_torque_setpoint.g1_term[2] = G1_term(2); + Vector3f G2_term = (G2 * (filtered_radps_vec - prev_esc_rad_per_sec_filtered)) / dt; + _vehicle_torque_setpoint.g2_term[0] = G2_term(0); + _vehicle_torque_setpoint.g2_term[1] = G2_term(1); + _vehicle_torque_setpoint.g2_term[2] = G2_term(2); + Vector3f filtered_body_torque_setpoint = G1_term + G2_term; + + return filtered_body_torque_setpoint; +} + +void MulticopterINDIRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, + float dt) +{ + for (int i = 0; i < 3; i++) { + _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; + } + + _energy_integration_time += dt; + + if (_energy_integration_time > 500e-3f) { + + actuator_controls_status_s status; + status.timestamp = vehicle_torque_setpoint.timestamp; + + for (int i = 0; i < 3; i++) { + status.control_power[i] = _control_energy[i] / _energy_integration_time; + _control_energy[i] = 0.f; + } + + _actuator_controls_status_pub.publish(status); + _energy_integration_time = 0.f; + } +} + +int MulticopterINDIRateControl::task_spawn(int argc, char *argv[]) +{ + MulticopterINDIRateControl *instance = new MulticopterINDIRateControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int MulticopterINDIRateControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MulticopterINDIRateControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the multicopter rate controller. It takes rate setpoints (in acro mode +via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. + +The controller has a PID loop for angular rate error. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_rate_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_indi_rate_control_main(int argc, char *argv[]) +{ + return MulticopterINDIRateControl::main(argc, argv); +} diff --git a/src/modules/mc_indi_rate_control/MulticopterINDIRateControl.hpp b/src/modules/mc_indi_rate_control/MulticopterINDIRateControl.hpp new file mode 100644 index 000000000000..a33e5bd60bd4 --- /dev/null +++ b/src/modules/mc_indi_rate_control/MulticopterINDIRateControl.hpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file MulticopterINDIRateControl.hpp + * + * Multicopter INDI rate control + * + * @author Rohan Inamdar + */ + +#pragma once + +#include +#include +#include +#include +#include +#include "ActuatorEffectivenessRotorsINDI.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace time_literals; + +class MulticopterINDIRateControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + MulticopterINDIRateControl(); + ~MulticopterINDIRateControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + + + void Run() override; + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt); + + matrix::Vector3f computeIndiTorqueSetpoint(const matrix::Vector + &filtered_radps_vec, const matrix::Vector &prev_esc_rad_per_sec_filtered, + float dt, const matrix::Matrix &G1, + const matrix::Matrix &G2); + + RateControl _rate_control; ///< class for rate control calculations + ActuatorEffectivenessRotorsINDI _rotors; + ActuatorEffectiveness::Configuration _actuator_effectiveness_config; + + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::Publication _actuator_controls_status_pub{ORB_ID(actuator_controls_status_0)}; + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + + vehicle_torque_setpoint_s _vehicle_torque_setpoint{}; + + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_status_s _vehicle_status{}; + + // Used for INDI + matrix::Matrix + _G1; //16 is the max number of actuators as defined in the ActuatorEffectivenessMatrix + matrix::Matrix _G2; + int _num_actuators{0}; + matrix::Vector _prev_esc_rad_per_sec_filtered; + matrix::Vector _prev_esc_rad_per_sec_filtered_dot; + matrix::Vector3f _prev_angular_accel; + + bool _landed{true}; + bool _maybe_landed{true}; + + hrt_abstime _last_run{0}; + hrt_abstime _last_esc_status_update{0}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + // keep setpoint values between updates + matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + + // for testing: x500 Iv base + Iv motors using parallel axis thm + // need this to matrix multiply Iv * Omega_f, as error = torque_f - Iv * Omega_f for INDI + matrix::SquareMatrix _Iv = matrix::diag(matrix::Vector3f(0.02385f, 0.02395f, 0.04400f)); + + matrix::Vector3f _rates_setpoint{}; + + float _battery_status_scale{0.0f}; + matrix::Vector3f _thrust_setpoint{}; + + float _energy_integration_time{0.0f}; + float _control_energy[4] {}; + + AlphaFilter _output_lpf_yaw; + + LowPassFilter2p _radps_lpf[ActuatorEffectiveness::NUM_ACTUATORS] {}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mc_rollrate_p, + (ParamFloat) _param_mc_rollrate_i, + (ParamFloat) _param_mc_rr_int_lim, + (ParamFloat) _param_mc_rollrate_d, + (ParamFloat) _param_mc_rollrate_ff, + (ParamFloat) _param_mc_rollrate_k, + + (ParamFloat) _param_mc_pitchrate_p, + (ParamFloat) _param_mc_pitchrate_i, + (ParamFloat) _param_mc_pr_int_lim, + (ParamFloat) _param_mc_pitchrate_d, + (ParamFloat) _param_mc_pitchrate_ff, + (ParamFloat) _param_mc_pitchrate_k, + + (ParamFloat) _param_mc_yawrate_p, + (ParamFloat) _param_mc_yawrate_i, + (ParamFloat) _param_mc_yr_int_lim, + (ParamFloat) _param_mc_yawrate_d, + (ParamFloat) _param_mc_yawrate_ff, + (ParamFloat) _param_mc_yawrate_k, + (ParamFloat) _param_mc_yaw_tq_cutoff, + + (ParamFloat) _param_mc_acro_r_max, + (ParamFloat) _param_mc_acro_p_max, + (ParamFloat) _param_mc_acro_y_max, + (ParamFloat) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */ + (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ + + (ParamBool) _param_mc_bat_scale_en, + + (ParamInt) _param_mc_indi_adapt_en, + + (ParamFloat) _param_imu_gyro_cutoff + ) +}; diff --git a/src/modules/mc_indi_rate_control/actuator_effectiveness_indi_rotors_params.c b/src/modules/mc_indi_rate_control/actuator_effectiveness_indi_rotors_params.c new file mode 100644 index 000000000000..83caad1c4599 --- /dev/null +++ b/src/modules/mc_indi_rate_control/actuator_effectiveness_indi_rotors_params.c @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_effectiveness_indi_rotors_params.c + * + * Parameters for INDI actuator effectiveness rotors + */ + +/** + * INDI Adaptive Constant - Roll Axis + * + * Adaptive constant for the roll axis in the INDI effectiveness matrix adaptation. + * Larger values result in faster adaptation but can cause instability if too high. + * Based on the paper "Adaptive Incremental Nonlinear Dynamic Inversion for Attitude Control of Micro Air Vehicles" by Smeur et al. + * + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.1 + * @group INDI Adaptive Constants + */ +PARAM_DEFINE_FLOAT(INDI_ADAPT_R, 1.5f); + +/** + * INDI Adaptive Constant - Pitch Axis + * + * Adaptive constant for the pitch axis in the INDI effectiveness matrix adaptation. + * Larger values result in faster adaptation but can cause instability if too high. + * Based on the paper "Adaptive Incremental Nonlinear Dynamic Inversion for Attitude Control of Micro Air Vehicles" by Smeur et al. + * + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.1 + * @group INDI Adaptive Constants + */ +PARAM_DEFINE_FLOAT(INDI_ADAPT_P, 1.5f); + +/** + * INDI Adaptive Constant - Yaw Axis + * + * Adaptive constant for the yaw axis in the INDI effectiveness matrix adaptation. + * Larger values result in faster adaptation but can cause instability if too high. + * Based on the paper "Adaptive Incremental Nonlinear Dynamic Inversion for Attitude Control of Micro Air Vehicles" by Smeur et al. + * + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.1 + * @group INDI Adaptive Constants + */ +PARAM_DEFINE_FLOAT(INDI_ADAPT_Y, 1.5f); diff --git a/src/modules/mc_indi_rate_control/mc_indi_rate_control_params.c b/src/modules/mc_indi_rate_control/mc_indi_rate_control_params.c new file mode 100644 index 000000000000..f293e5eb088c --- /dev/null +++ b/src/modules/mc_indi_rate_control/mc_indi_rate_control_params.c @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_indi_rate_control_params.c + * + * Parameters for INDI rate controller + * Note many params defined in the Multicopter Params file are used within INDI rate control + */ + +/** + * INDI Rate Control Enable + * + * This parameter enables the INDI rate control. + * + * @min 0 + * @max 1 + * @decimal 0 + * @increment 1 + */ +PARAM_DEFINE_INT32(MC_INDI_EN, 1); + +/** + * Incremental Nonlinear Dynamic Inversion (INDI) Adaptation Enable + * + * This parameter enables the adaptation of the INDI effectiveness matrix. + * + * @min 0 + * @max 1 + * @decimal 0 + * @increment 1 + */ +PARAM_DEFINE_INT32(MC_INDI_ADAPT_EN, 0); diff --git a/src/modules/mc_indi_rate_control/module.yaml b/src/modules/mc_indi_rate_control/module.yaml new file mode 100644 index 000000000000..8f268d850acd --- /dev/null +++ b/src/modules/mc_indi_rate_control/module.yaml @@ -0,0 +1,80 @@ +__max_num_mc_motors: &max_num_mc_motors 12 + +module_name: INDI Multicopter Rate Control + +parameters: + - group: Metric Geometry + definitions: + MET_ROTOR${i}_CT: + description: + short: Thrust coefficient of rotor ${i} + long: | + The thrust coefficient is defined as Thrust = CT * omega^2, + where omega is the angular velocity of the rotor in rad/s. + The unit is in metric units (N/(rad/s)^2) + type: float + decimal: 10 + increment: 0.00000001 + num_instances: *max_num_mc_motors + min: 0 + max: 1 + default: 0.0 + MET_ROTOR${i}_KM: + description: + short: Moment coefficient of rotor ${i} + long: | + The moment coefficient is defined as Torque = Km * Thrust (N) + + Use a positive value for a rotor with CCW rotation. + Use a negative value for a rotor with CW rotation. + type: float + decimal: 4 + increment: 0.0001 + unit: m + num_instances: *max_num_mc_motors + min: -1 + max: 1 + default: 0.0 + MET_ROTOR${i}_IZZ: + description: + short: Moment of inertia of rotor ${i} about its spin axis + long: | + The unit is in metric units (kg*m^2) + type: float + decimal: 10 + increment: 0.00000001 + num_instances: *max_num_mc_motors + min: 0 + max: 1 + default: 0.0 + + - group: INDI Adaptive Constants + definitions: + INDI_G1_ADAPT_${i}: + description: + short: INDI G1 adaptive constant for rotor ${i} + long: | + G1 matrix adaptive constant for rotor ${i} in the INDI effectiveness matrix adaptation. + This constant affects the adaptation rate of the G1 matrix (moment part) for this specific rotor. + Larger values result in faster adaptation but can cause instability if too high. + type: float + decimal: 2 + increment: 0.01 + min: 0.0 + max: 1.0 + default: 0.2 + num_instances: *max_num_mc_motors + INDI_G2_ADAPT_${i}: + description: + short: INDI G2 adaptive constant for rotor ${i} + long: | + G2 matrix adaptive constant for rotor ${i} in the INDI effectiveness matrix adaptation. + This constant affects the adaptation rate of the G2 matrix (gyroscopic part) for this specific rotor. + Larger values result in faster adaptation but can cause instability if too high. + type: float + decimal: 2 + increment: 0.01 + min: 0.0 + max: 1.0 + default: 0.2 + num_instances: *max_num_mc_motors