Skip to content

[WIP] Implement INDI control law with supporting infrastructure #25216

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
rohan908 wants to merge 34 commits into
base: main
Choose a base branch
from
Open

[WIP] Implement INDI control law with supporting infrastructure #25216

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
34 commits
Select commit Hold shift + click to select a range
3027a1b
indi
rohan908 Jun 19, 2025
756baa1
Corrected and caluclated all inertial and constants for x500 drone!
rohan908 Jun 23, 2025
afb6f58
Fixed nu to properly implement tilt-priotized error
rohan908 Jun 25, 2025
5d388b1
stopping point, still need to fix thrust clamps in params AND add rat…
rohan908 Jun 25, 2025
2fce64e
not working, but realized that control allocator has an adaptive effe…
rohan908 Jun 27, 2025
33ba896
old imp before moving to torque
rohan908 Jul 2, 2025
10f431b
added indi flag in commander, added publishing of control effectivnes…
rohan908 Jul 7, 2025
ff00bd0
compile errors
rohan908 Jul 7, 2025
6c39a4b
removed deprecated indi_att_control module
rohan908 Jul 8, 2025
38f5029
control effectiveness publishing is not working in ControlAllocator.cpp
rohan908 Jul 8, 2025
3926a8d
tuned INDI commands
rohan908 Jul 9, 2025
2d52733
plotting and windy world changes
rohan908 Jul 9, 2025
4e78aba
setting gains of control matrix as px4 params
rohan908 Jul 14, 2025
9f9a106
Merge branch 'main' into pr-indi
rohan908 Jul 14, 2025
1153de0
changed modificed mc_rate to mc_indi_rate, added conditional start in…
rohan908 Jul 15, 2025
791b453
revert commander and control allocator modules to main branch
rohan908 Jul 15, 2025
a5fa2f5
removed all edits related to urob topics for enabling indi and actuat…
rohan908 Jul 15, 2025
00ba1d7
added back mc_rate_control from main
rohan908 Jul 15, 2025
790af59
effectiveness done, still working on adaptive
rohan908 Jul 16, 2025
d5662d7
adaptive effectiveness added, needs tuning of mu1 and mu2
rohan908 Jul 16, 2025
705f413
added new implementation of indi
rohan908 Jul 16, 2025
73818c9
code compiles but doesn't work
rohan908 Jul 17, 2025
ae27648
params successfully loaded in but setpoint from indi is not finite
rohan908 Jul 17, 2025
39fb863
I believe the code works, but I need an accurate ct and km in SI units
rohan908 Jul 17, 2025
b0dbd5c
Merge branch 'pr-indi' of https://github.com/acp-lab/PX4-Autopilot-IN…
rohan908 Jul 17, 2025
d6ccb01
Merge branch 'main' into pr-indi
rohan908 Jul 17, 2025
aef7adf
Merge branch 'pr-indi' of https://github.com/acp-lab/PX4-Autopilot-IN…
rohan908 Jul 17, 2025
9163a34
i think it works?!
rohan908 Jul 17, 2025
d1cad7c
Merge branch 'main' into pr-indi
rohan908 Jul 21, 2025
c2170d2
clean up params for metric geo group and new adaptibility group
rohan908 Jul 21, 2025
8cc00c8
forgot to debug before commit lol
rohan908 Jul 21, 2025
6e10dd8
Merge branch 'pr-indi' of https://github.com/acp-lab/PX4-Autopilot-IN…
rohan908 Jul 21, 2025
0b56151
cleaned up vscode settings.json
rohan908 Jul 21, 2025
5577d08
formatting
rohan908 Jul 21, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Copy Markdown
Member

@Jaeyoung-Lim Jaeyoung-Lim Jul 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Copy Markdown
Author

@rohan908 rohan908 Jul 21, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Copy Markdown
Member

@Jaeyoung-Lim Jaeyoung-Lim Jul 23, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


param set-default SIM_GZ_EC_FUNC1 101
param set-default SIM_GZ_EC_FUNC2 102
param set-default SIM_GZ_EC_FUNC3 103
Expand Down
8 changes: 7 additions & 1 deletion ROMFS/px4fmu_common/init.d/rc.mc_apps
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
1 change: 1 addition & 0 deletions boards/px4/sitl/default.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions msg/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,6 +118,7 @@ set(msg_files
InputRc.msg
InternalCombustionEngineControl.msg
InternalCombustionEngineStatus.msg
IndiAdaptationStatus.msg
IridiumsbdStatus.msg
IrlockReport.msg
LandingGear.msg
Expand Down
24 changes: 24 additions & 0 deletions msg/IndiAdaptationStatus.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
# INDI Adaptation Status Message
# Tracks changes in G matrices during INDI adaptation
Comment on lines +1 to +2
Copy link
Copy Markdown
Contributor

@hamishwillee hamishwillee Jul 24, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # timestamp of the data used for adaptation
Comment on lines +4 to +5
Copy link
Copy Markdown
Contributor

@hamishwillee hamishwillee Jul 24, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Copy Markdown
Contributor

@hamishwillee hamishwillee Jan 29, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


# 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)
Copy link
Copy Markdown
Member

@Jaeyoung-Lim Jaeyoung-Lim Jul 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Copy Markdown
Author

@rohan908 rohan908 Jul 21, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Copy Markdown
Member

@Jaeyoung-Lim Jaeyoung-Lim Jul 23, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Copy Markdown
Author

@rohan908 rohan908 Jul 23, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Copy Markdown
Member

@Jaeyoung-Lim Jaeyoung-Lim Jul 24, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes


# 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)
Comment on lines +14 to +16
Copy link
Copy Markdown
Contributor

@hamishwillee hamishwillee Jul 24, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


# 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
Comment on lines +23 to +24
Copy link
Copy Markdown
Contributor

@hamishwillee hamishwillee Jul 24, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

5 changes: 5 additions & 0 deletions msg/VehicleTorqueSetpoint.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Comment on lines +6 to +10
Copy link
Copy Markdown
Member

@Pedro-Roque Pedro-Roque Jul 23, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


# TOPICS vehicle_torque_setpoint
# TOPICS vehicle_torque_setpoint_virtual_fw vehicle_torque_setpoint_virtual_mc
1 change: 1 addition & 0 deletions src/modules/logger/logged_topics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Loading
Loading