[WIP] Implement INDI control law with supporting infrastructure by rohan908 · Pull Request #25216 · PX4/PX4-Autopilot

rohan908 · 2025-07-14T17:27:30Z

Solved Problem

INDI Rate Control Implementation for Multicopters

Solution

New INDI Rate Control Module

Added: src/modules/mc_indi_rate_control/ - Complete INDI rate control implementation
- MulticopterINDIRateControl.cpp/hpp - Main INDI controller with adaptive effectiveness
- ActuatorEffectivenessRotorsINDI.cpp/hpp - Rotor-specific actuator effectiveness matrix
- mc_indi_rate_control_params.c - Parameter definitions for INDI tuning
- module.yaml - Module configuration and parameter metadata

Core INDI Features

Adaptive Effectiveness Matrix: Dynamic calculation of G1 and G2 matrices based on current rotor speeds
Inertia Compensation: Incorporates vehicle inertia matrix for accurate torque calculations
Low-Pass Filtering: ESC status filtering for smooth control signals
Parameter-Based Configuration: Configurable rotor positions, thrust coefficients, and moment coefficients

New Parameters

Metric Geometry Group:
- MET_ROTOR${i}_CT - Thrust coefficients (N/(rad/s)²)
- MET_ROTOR${i}_KM - Moment coefficients (m)
- MET_ROTOR${i}_IZZ - Rotor moments of inertia (kg·m²)

Integration Changes

Modified: ROMFS/px4fmu_common/init.d/rc.mc_apps - Added conditional INDI module startup
Updated: Airframe configurations to support INDI rate control

Testing & Validation

Publish outputs for debugging to UROB topics!
Extra variables published in vehicle_thrust_setpoint for debugging as well as the adaptive G matrices published to indi_adaptation_status
To know if it is working the indi_torque_setpoint should follow the measured_torque closely

Context

Implemented from this paper: https://sihaosun.github.io/publication/comparativestudy/
And this paper: https://pure.tudelft.nl/ws/files/9188985/Adaptive_Incremental_Nonlinear_Dynamic_Inversion_or_Attitude_Control_of_Micro_Aerial_Vehiclesa.pdf
Equations:

…e control to nu

…ctivness and im about to nuke my code base so saving this

…s in ControlAllocator in a new uROB topic, implemented INDI with a lpf for the esc status

dagar · 2025-07-14T18:19:22Z

I've played with implementing this a few times, so I'd be very happy if we can bring it in experimentally. What would really help make that possible is if it can be carried as an optional alternate rate controller (eg mc_indi_rate_control) so that there's no potential cost or downside to regular users.

rohan908 · 2025-07-14T20:55:38Z

I've played with implementing this a few times, so I'd be very happy if we can bring it in experimentally. What would really help make that possible is if it can be carried as an optional alternate rate controller (eg mc_indi_rate_control) so that there's no potential cost or downside to regular users.

I've found some crucial errors in my current implementation that kind of forces this to be a seperate module (specifically the non-metric affectiveness matrix in the control allocator). This will be fixed!

Jaeyoung-Lim

Thanks for the contribution!

High level, you are modifying commander and the existing mc_rate_control module to implement indi control.

I would rather separate this out as a separate module (e.g. mc_indi_rate_control) so that you are less tangled with the rest of the system (such as commander) and it is less safety critical by modifying the existing rate controller.

On the allocation side, we can now use the control allocator as a library. My suggestion would be to do the allocation inside the new module, without touching the control allocator.

.vscode/settings.json

msg/versioned/VehicleControlMode.msg

src/modules/commander/Commander.hpp

src/modules/commander/commander_params.c

src/modules/control_allocator/ControlAllocator.cpp

… rcs. fix control allocator and commander, add metric rotor affectiveness matrix imp, revert mc_rate default needed

…or effectivness publication

… into pr-indi

Jaeyoung-Lim

Thank you for the adjustments.

I couldn't go through the code thoroughly yet, but here are some of my initial comments.

Jaeyoung-Lim · 2025-07-18T02:34:47Z

ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500

+param set-default MET_ROTOR3_PY -0.174
+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


Why do we need to define this in addition to the CA_ parameters?

the CT, KM for CA_ are not in metric. The IZZ is not defined in CA_. And with regards to the positioning, I can get rid of those, the CA_ params seem to give accurate enough results and are in metric.

What is the downside of just using Ct and Km in CA_ and use them as metric parameters? Defining additonal parameters will use a lot of flash space that is not necesssary.

Jaeyoung-Lim · 2025-07-18T02:35:39Z

msg/IndiAdaptationStatus.msg

+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)


Is it necessary to log these huge matrices to figure out what is going on?

hm... I would suggest leaving it in for debugging the adaptive effectiveness matrix (bc its essentially only possible through manual tuning and I assume this feature would only be used experimentally). And it would only be published to when INDI_ADAPT_EN is enabled (==1).

These would be quite expensive to log at a high rate. I would at least remove them by default.

ok! What is the best practice to do that? Just comment them out?

src/modules/mc_indi_rate_control/MulticopterINDIRateControl.cpp

Jaeyoung-Lim · 2025-07-18T02:43:06Z

src/modules/mc_indi_rate_control/MulticopterINDIRateControl.cpp

+			vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
+			//vehicle_torque_setpoint_s vehicle_torque_setpoint{};
+
+			_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);


Does the indi not track linear acceleration?

No, in the implementations I've seen so far for multicopters, the INDI has been only for the body torque.

Jaeyoung-Lim · 2025-07-18T02:45:04Z

src/modules/mc_indi_rate_control/MulticopterINDIRateControl.cpp

+			_vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(torque_setpoint(0)) ? torque_setpoint(0) : 0.f;
+			_vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(torque_setpoint(1)) ? torque_setpoint(1) : 0.f;
+			_vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(torque_setpoint(2)) ? torque_setpoint(2) : 0.f;


This seems a bit strange. So you are incrementing on top of the rate controller output, rather than incrementing against the previous torque setpoint?

It does seem strange at first but I'm basing this off of Professor Sihao Sun's work. From equation 28 we see that the desired angular acceleartion is essentially the same implementation as the current attitude controller -> attitude rate controler but without the I term. Then, in equation 33, that desired angular rate is subtracted by the filtered angular acceleration from the gyro (which is exactly what the output of the unmodified rate controller is!). The main assumption I'm using here is that the PD controller constants act as a pseudo-replacement for the moment of inertia matrix (Iv), but after talking with my colleagues they think that is a fine assumption (though I would love to hear your thoughts). Then, I add the filtered body torque from the indi_torque_setpoint to that output as done in equation 33.

I'm also going to push a new commit that cleans up the prints and all the residual debugging mess I've left behind in the code, including this awful code block that you've pointed out

Then, in equation 33, that desired angular rate is subtracted by the filtered angular acceleration from the gyro (which is exactly what the output of the unmodified rate controller is!).

I don't think this is correct. The desired angular acceleration is substracted by the filtered angular acceleration measured from the gyro. The filtered angular acceleration is NOT the same as the output of the unmodified rate controller, unless the torque is perfectly being tracked. The reason iNDI controller is robust is because it uses measurements to correct it's commands. I think this part of the implementation should be fixed.

… into pr-indi

DronecodeBot · 2025-07-22T20:36:05Z

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-dev-call-july-23-2025-team-sync-and-community-q-a/46617/2

mrpollo · 2025-07-23T15:13:52Z

@MaEtUgR would you mind giving this a quick review? thanks!

Pedro-Roque

Thanks for the contribution @rohan908 ! Here go a few comments on my side.

Pedro-Roque · 2025-07-23T16:19:10Z

msg/VehicleTorqueSetpoint.msg

+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)


I don't think this should go here. If you need all these terms, then you should have a different message definition specific for INDI. Furthermore, measured_torque shouldn't belong in a torque setpoint message.

Pedro-Roque · 2025-07-23T16:20:23Z

src/modules/mc_indi_rate_control/ActuatorEffectivenessRotorsINDI.cpp

+			_param_handles[i].axis_z = param_find(buffer);
+		}
+
+		snprintf(buffer, sizeof(buffer), "MET_ROTOR%u_CT", i);


This will be confusing for the user I believe, as they are the same as CA_..._CT/KM. I'd use those parameters and specify to the user that these should be metric.

If I replace the parameters of CA_... to be metric then the Control Allocator would not work, which is why I made them seperate. I could hypothetically have a parameter for maxRotSpeed and then use that to divide the CA_...CT/KM params? What do you suggest?

I think a maximum rotor speed, if you need that value, should be fine to have. I think that's less ambiguous/duplicating than the current implementation.

I still would like to check what exactly fails in ControlAllocator if you use those variables as metric, but don't have time for it now. You could only set metric values in case your flag is enabled, for instance.

@rohan908 Why do you care about the control_allocator module? IMO, you should not run it when doing iNDI

hamishwillee · 2025-07-24T04:33:48Z

msg/IndiAdaptationStatus.msg

+# INDI Adaptation Status Message
+# Tracks changes in G matrices during INDI adaptation


Suggested change

# INDI Adaptation Status Message

# Tracks changes in G matrices during INDI adaptation

# INDI Adaptation Status message

#

# Tracks changes in G matrices during INDI adaptation.

hamishwillee · 2025-07-24T04:36:00Z

msg/IndiAdaptationStatus.msg

+uint64 timestamp        # time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data used for adaptation


Doc standard now single space before after comment, metadata prefix [] for units, [@range low,high], [@enum constant_prefix], [@frame frame_name]. No full stop unless there is multiple sentences.

Suggested change

uint64 timestamp # time since system start (microseconds)

uint64 timestamp_sample # timestamp of the data used for adaptation

uint64 timestamp # [us] Time since system start

uint64 timestamp_sample # [us] Timestamp of the data used for adaptation

FYI the docs standard is now documented in https://docs.px4.io/main/en/uorb/uorb_documentation

hamishwillee · 2025-07-24T04:37:07Z

msg/IndiAdaptationStatus.msg

+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)


Suggested change

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)

float32[16] rotor_speeds # [rad/s] Current rotor speeds

float32[16] rotor_speed_dots # [rad/s^2] Rotor speed derivatives

float32[3] angular_accel_delta # [rad/s^2] Angular acceleration delta

hamishwillee · 2025-07-24T04:37:49Z

msg/IndiAdaptationStatus.msg

+bool adaptation_enabled          # Whether adaptation is currently enabled
+uint8 num_actuators             # Number of actuators being adapted


Suggested change

bool adaptation_enabled # Whether adaptation is currently enabled

uint8 num_actuators # Number of actuators being adapted

bool adaptation_enabled # True if adaptation is currently enabled

uint8 num_actuators # Number of actuators being adapted

github-actions · 2025-12-12T02:14:51Z

This PR was identified as stale and it will be closed in 30 days unless any activity is detected.

github-actions · 2026-01-13T02:14:05Z

This pull request has been automatically closed due to 120 days of inactivity. If you would like to continue, please feel free to reopen it or submit a new PR.

rohan908 and others added 14 commits June 19, 2025 17:28

indi

3027a1b

Corrected and caluclated all inertial and constants for x500 drone!

756baa1

Fixed nu to properly implement tilt-priotized error

afb6f58

stopping point, still need to fix thrust clamps in params AND add rat…

5d388b1

…e control to nu

not working, but realized that control allocator has an adaptive effe…

2fce64e

…ctivness and im about to nuke my code base so saving this

old imp before moving to torque

33ba896

added indi flag in commander, added publishing of control effectivnes…

10f431b

…s in ControlAllocator in a new uROB topic, implemented INDI with a lpf for the esc status

compile errors

ff00bd0

removed deprecated indi_att_control module

6c39a4b

control effectiveness publishing is not working in ControlAllocator.cpp

38f5029

tuned INDI commands

3926a8d

plotting and windy world changes

2d52733

setting gains of control matrix as px4 params

4e78aba

Merge branch 'main' into pr-indi

9f9a106

rohan908 marked this pull request as draft July 14, 2025 17:38

rohan908 changed the title ~~Pr indi~~ [WIP] Pr indi Jul 14, 2025

Jaeyoung-Lim requested changes Jul 14, 2025

View reviewed changes

julianoes changed the title ~~[WIP] Pr indi~~ [WIP] Implement INDI control law with supporting infrastructure Jul 15, 2025

rohan908 added 10 commits July 15, 2025 11:27

changed modificed mc_rate to mc_indi_rate, added conditional start in…

1153de0

… rcs. fix control allocator and commander, add metric rotor affectiveness matrix imp, revert mc_rate default needed

revert commander and control allocator modules to main branch

791b453

removed all edits related to urob topics for enabling indi and actuat…

a5fa2f5

…or effectivness publication

added back mc_rate_control from main

00ba1d7

effectiveness done, still working on adaptive

790af59

adaptive effectiveness added, needs tuning of mu1 and mu2

d5662d7

added new implementation of indi

705f413

code compiles but doesn't work

73818c9

params successfully loaded in but setpoint from indi is not finite

ae27648

I believe the code works, but I need an accurate ct and km in SI units

39fb863

rohan908 added 2 commits July 17, 2025 15:26

Merge branch 'pr-indi' of https://github.com/acp-lab/PX4-Autopilot-INDI…

aef7adf

… into pr-indi

i think it works?!

9163a34

rohan908 requested a review from Jaeyoung-Lim July 17, 2025 22:00

Jaeyoung-Lim requested changes Jul 18, 2025

View reviewed changes

rohan908 added 5 commits July 21, 2025 11:10

Merge branch 'main' into pr-indi

d1cad7c

clean up params for metric geo group and new adaptibility group

c2170d2

forgot to debug before commit lol

8cc00c8

Merge branch 'pr-indi' of https://github.com/acp-lab/PX4-Autopilot-INDI…

6e10dd8

… into pr-indi

cleaned up vscode settings.json

0b56151

rohan908 requested a review from Jaeyoung-Lim July 21, 2025 16:48

formatting

5577d08

rohan908 marked this pull request as ready for review July 22, 2025 20:33

Pedro-Roque self-requested a review July 23, 2025 15:09

mrpollo requested a review from MaEtUgR July 23, 2025 15:13

Pedro-Roque requested changes Jul 23, 2025

View reviewed changes

hamishwillee reviewed Jul 24, 2025

View reviewed changes

github-actions bot added the stale label Aug 24, 2025

mrpollo removed the stale label Sep 12, 2025

github-actions bot added the stale label Dec 12, 2025

Jaeyoung-Lim mentioned this pull request Dec 19, 2025

Model-based 3D wind estimator for fixed-wing #25966

Open

github-actions bot closed this Jan 13, 2026

Jaeyoung-Lim reopened this Jan 13, 2026

github-actions bot removed the stale label Jan 14, 2026

		# 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

		bool adaptation_enabled # Whether adaptation is currently enabled
		uint8 num_actuators # Number of actuators being adapted

Conversation

rohan908 commented Jul 14, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Solved Problem

Solution

Core INDI Features

New Parameters

Integration Changes

Testing & Validation

Context

Uh oh!

dagar commented Jul 14, 2025

Uh oh!

rohan908 commented Jul 14, 2025

Uh oh!

Jaeyoung-Lim left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Jaeyoung-Lim left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

DronecodeBot commented Jul 22, 2025

Uh oh!

mrpollo commented Jul 23, 2025

Uh oh!

Pedro-Roque left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

hamishwillee Jul 24, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

rohan908 commented Jul 14, 2025 •

edited

Loading

Jaeyoung-Lim left a comment •

edited

Loading

hamishwillee Jul 24, 2025 •

edited

Loading