[WIP] Implement INDI control law with supporting infrastructure

.vscode/settings.json

@@ -129,4 +129,9 @@
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
         },
+        "ros.distro": "humble",
+        "python.autoComplete.extraPaths": [
+                "/opt/ros/humble/lib/python3.10/site-packages",
+                "/opt/ros/humble/local/lib/python3.10/dist-packages"
+        ],
 }

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -19,6 +19,7 @@ mc_rate_control start
 # Start Multicopter Attitude Controller.
 #
 mc_att_control start
+#indi_att_control start
 
 if param greater -s MC_AT_EN 0
 then

msg/ActuatorEffectivenessMatrix.msg

@@ -0,0 +1,12 @@
+# Actuator effectiveness matrix for a single matrix
+uint64 timestamp
+
+uint8 NUM_AXES = 6
+uint8 NUM_ACTUATORS = 16
+
+# Effectiveness matrix (flattened row-major)
+# 6 rows, 16 columns
+float32[96] effectiveness_matrix_row_major
+
+# Number of actuators used in this matrix
+uint8 num_actuators

msg/CMakeLists.txt

@@ -40,6 +40,7 @@ set(msg_files
 	ActionRequest.msg
 	ActuatorArmed.msg
 	ActuatorControlsStatus.msg
+	ActuatorEffectivenessMatrix.msg
 	ActuatorOutputs.msg
 	ActuatorServosTrim.msg
 	ActuatorTest.msg

msg/VehicleTorqueSetpoint.msg

@@ -3,6 +3,9 @@ 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] filtered_xyz # filtered 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)
 
 # TOPICS vehicle_torque_setpoint
 # TOPICS vehicle_torque_setpoint_virtual_fw vehicle_torque_setpoint_virtual_mc

msg/versioned/VehicleControlMode.msg

@@ -15,6 +15,7 @@ bool flag_control_climb_rate_enabled		# true if climb rate is controlled
 bool flag_control_acceleration_enabled		# true if acceleration is controlled
 bool flag_control_attitude_enabled		# true if attitude stabilization is mixed in
 bool flag_control_rates_enabled			# true if rates are stabilized
+bool flag_control_rates_indi_enabled		# true if indi control with dshot to motors is enabled
 bool flag_control_allocation_enabled		# true if control allocation is enabled
 bool flag_control_termination_enabled		# true if flighttermination is enabled
 

src/modules/commander/Commander.cpp

@@ -2594,10 +2594,38 @@ void Commander::updateControlMode()
 		    || _vehicle_control_mode.flag_control_position_enabled
 		    || _vehicle_control_mode.flag_control_velocity_enabled
 		    || _vehicle_control_mode.flag_control_acceleration_enabled);
+
+	// Check for ESC RPM feedback availability for INDI control
+	checkEscRpmFeedbackAvailability();
+	_vehicle_control_mode.flag_control_rates_indi_enabled = _esc_rpm_feedback_available && _param_indi_control_enabled.get();
+
 	_vehicle_control_mode.timestamp = hrt_absolute_time();
 	_vehicle_control_mode_pub.publish(_vehicle_control_mode);
 }
 
+void Commander::checkEscRpmFeedbackAvailability()
+{
+	if (_esc_rpm_feedback_available) {
+		return; //once dshot has been seen as true, we don't need to check again and take the chance that the esc status did not update
+	}
+
+	esc_status_s esc_status;
+
+	if (_esc_status_sub.update(&esc_status)) {
+		bool rpm_feedback_available = false;
+
+		// check if connection type is DShot and we have online ESCs
+		// FIXME: this is a hack to check if the ESCs are online, we should use the esc_status_s::ESC_CONNECTION_TYPE_DSHOT flag instead but gz_x500 does not support it
+		if (esc_status.esc_count > 0 &&
+		    esc_status.esc_online_flags > 0) {
+
+			rpm_feedback_available = true;
+		}
+
+		_esc_rpm_feedback_available = rpm_feedback_available;
+	}
+}
+
 void Commander::printRejectMode(uint8_t nav_state)
 {
 	if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) {

src/modules/commander/Commander.hpp

@@ -68,6 +68,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/esc_status.h>
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mission_result.h>
@@ -163,6 +164,8 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 
 	void updateControlMode();
 
+	void checkEscRpmFeedbackAvailability();
+
 	void send_parachute_command();
 
 	void checkForMissionUpdate();
@@ -280,6 +283,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	bool _have_taken_off_since_arming{false};
 	bool _status_changed{true};
 	bool _mission_in_progress{false};
+	bool _esc_rpm_feedback_available{false};
 
 	vehicle_land_detected_s	_vehicle_land_detected{};
 
@@ -291,6 +295,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	// Subscriptions
 	uORB::Subscription					_action_request_sub{ORB_ID(action_request)};
 	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
+	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
@@ -347,6 +352,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_com_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_com_takeoff_act,
-		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max
+		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max,
+		(ParamBool<px4::params::INDI_CONTROL_EN>)   _param_indi_control_enabled
 	)
 };

src/modules/commander/commander_params.c

@@ -1041,3 +1041,13 @@ PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3);
  *
  */
 PARAM_DEFINE_INT32(COM_MODE_ARM_CHK, 0);
+
+/**
+ * Enable INDI control
+ *
+ * If enabled, the vehicle will use INDI control to control the motors.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(INDI_CONTROL_EN, 1);

src/modules/control_allocator/ControlAllocator.cpp

@@ -445,6 +445,7 @@ ControlAllocator::Run()
 	// Publish actuator setpoint and allocator status
 	publish_actuator_controls();
 
+
 	// Publish status at limited rate, as it's somewhat expensive and we use it for slower dynamics
 	// (i.e. anti-integrator windup)
 	if (now - _last_status_pub >= 5_ms) {
@@ -586,6 +587,13 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
 			int total_num_actuators = config.num_actuators_matrix[i];
 			_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i],
 					config.linearization_point[i], total_num_actuators, reason == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
+
+			// Publish effectiveness matrix (0 for hover, 1 for forward flight)
+			actuator_effectiveness_matrix_s effectiveness_matrix_pub{};
+			effectiveness_matrix_pub.timestamp = hrt_absolute_time();
+			memcpy(effectiveness_matrix_pub.effectiveness_matrix_row_major, &matrix, sizeof(matrix));
+			effectiveness_matrix_pub.num_actuators = total_num_actuators;
+			_actuator_effectiveness_matrix_pub[i].publish(effectiveness_matrix_pub);
 		}
 
 		trims.timestamp = hrt_absolute_time();
@@ -709,6 +717,7 @@ ControlAllocator::publish_actuator_controls()
 	}
 }
 
+
 void
 ControlAllocator::check_for_motor_failures()
 {

src/modules/control_allocator/ControlAllocator.hpp

@@ -78,6 +78,7 @@
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/failure_detector_status.h>
+#include <uORB/topics/actuator_effectiveness_matrix.h>
 
 class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
 {
@@ -138,6 +139,8 @@ class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParam
 
 	void publish_actuator_controls();
 
+	void publish_effectiveness_matrix();
+
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
 	int _num_control_allocation{0};
@@ -183,6 +186,8 @@ class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParam
 	// Outputs
 	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
 
+	uORB::Publication<actuator_effectiveness_matrix_s> _actuator_effectiveness_matrix_pub[ActuatorEffectiveness::MAX_NUM_MATRICES]{ORB_ID(actuator_effectiveness_matrix), ORB_ID(actuator_effectiveness_matrix)};
+
 	uORB::Publication<actuator_motors_s>	_actuator_motors_pub{ORB_ID(actuator_motors)};
 	uORB::Publication<actuator_servos_s>	_actuator_servos_pub{ORB_ID(actuator_servos)};
 	uORB::Publication<actuator_servos_trim_s>	_actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)};

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_multi("actuator_effectiveness_matrix", 20, 2);
 
 	// SYS_HITL: default ground truth logging for simulation
 	int32_t sys_hitl = 0;

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -97,6 +97,20 @@ MulticopterRateControl::parameters_updated()
 				  radians(_param_mc_acro_y_max.get()));
 
 	_output_lpf_yaw.setCutoffFreq(_param_mc_yaw_tq_cutoff.get());
+
+	// INDI
+	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
@@ -120,6 +134,8 @@ MulticopterRateControl::Run()
 		parameters_updated();
 	}
 
+	_esc_status_sub.update(&_esc_status);
+
 	/* run controller on gyro changes */
 	vehicle_angular_velocity_s angular_velocity;
 
@@ -146,7 +162,49 @@ MulticopterRateControl::Run()
 			}
 		}
 
-		_vehicle_status_sub.update(&_vehicle_status);
+		matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> filtered_radps_vec;
+
+		if (_vehicle_control_mode.flag_control_rates_indi_enabled) {
+
+			if (_actuator_effectiveness_matrix_sub.updated()) {
+				if (_actuator_effectiveness_matrix_sub.copy(&_actuator_effectiveness_matrix)) {
+					_num_actuators = _actuator_effectiveness_matrix.num_actuators;
+
+					// seperate the row major matrix into the torque (G1) and thrust (G2) matrices
+					Vector3f G1_K = Vector3f(0.00000001f, 0.0000001f, 0.0000001f);
+					Vector3f G2_K = Vector3f(1.0f, 1.0f, 0.0000000001f);
+					for (int i = 0; i < 3; i++) {
+
+						for (int j = 0; j < _num_actuators; j++) {
+							_G1(i, j) = _actuator_effectiveness_matrix.effectiveness_matrix_row_major[i * 16 + j]  * G1_K(i); //row has 16 columns (even if they are empty/not have 16 actuators)
+						}
+					}
+
+					for (int i = 0; i < 3; i++) {
+						for (int j = 0; j < _num_actuators; j++) {
+							_G2(i, j) = _actuator_effectiveness_matrix.effectiveness_matrix_row_major[(i + 3) * 16 + j] * G2_K(i); //thrust comes 3 rows after torque, so offset by 3
+						}
+					}
+				}
+				PX4_INFO("G1: %f, %f, %f, %f", (double)_G1(0, 0), (double)_G1(0, 1), (double)_G1(0, 2), (double)_G1(0, 3));
+				PX4_INFO("G1: %f, %f, %f, %f", (double)_G1(1, 0), (double)_G1(1, 1), (double)_G1(1, 2), (double)_G1(1, 3));
+				PX4_INFO("G1: %f, %f, %f, %f", (double)_G1(2, 0), (double)_G1(2, 1), (double)_G1(2, 2), (double)_G1(2, 3));
+				PX4_INFO("G2: %f, %f, %f, %f", (double)_G2(0, 0), (double)_G2(0, 1), (double)_G2(0, 2), (double)_G2(0, 3));
+				PX4_INFO("G2: %f, %f, %f, %f", (double)_G2(1, 0), (double)_G2(1, 1), (double)_G2(1, 2), (double)_G2(1, 3));
+				PX4_INFO("G2: %f, %f, %f, %f", (double)_G2(2, 0), (double)_G2(2, 1), (double)_G2(2, 2), (double)_G2(2, 3));
+			}
+
+			if (_esc_status.timestamp > 0) { //check if atleast one esc status is available
+				for (int i = 0; i < _num_actuators; 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);
+				}
+			}
+			else {
+				filtered_radps_vec.zero();
+				PX4_WARN("No ESC status available for INDI");
+			}
+		}
 
 		// use rates setpoint topic
 		vehicle_rates_setpoint_s vehicle_rates_setpoint{};
@@ -236,6 +294,36 @@ MulticopterRateControl::Run()
 			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;
+			PX4_INFO("torque_setpoint: %f, %f, %f", (double)torque_setpoint(0), (double)torque_setpoint(1), (double)torque_setpoint(2));
+
+			// TODO: a possible optimization is to manually multiply the G1 and G2 matrices with the radps_vec_squared and filtered_radps_vec - _prev_esc_rad_per_sec_filtered vectors
+			// as this would avoid the need to multiply by the full 16 vector, which is full of zeros for most cases (quadrotors)
+			if (_vehicle_control_mode.flag_control_rates_indi_enabled) {
+				matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> radps_vec_squared = filtered_radps_vec.emult(filtered_radps_vec);
+				//PX4_INFO("radps_vec_squared: %f, %f, %f", (double)radps_vec_squared(0), (double)radps_vec_squared(1), (double)radps_vec_squared(2));
+				//PX4_INFO("filtered_radps_vec: %f, %f, %f", (double)filtered_radps_vec(0), (double)filtered_radps_vec(1), (double)filtered_radps_vec(2));
+				Vector3f G1_term = _G1 * (radps_vec_squared);
+				Vector3f G2_term = (_G2 * (filtered_radps_vec - _prev_esc_rad_per_sec_filtered)) / dt;
+				Vector3f filtered_body_torque_setpoint = G1_term + G2_term;
+				//PX4_INFO("G1 term: %f, %f, %f", (double)G1_term(0), (double)G1_term(1), (double)G1_term(2));
+				//PX4_INFO("G2 term: %f, %f, %f", (double)G2_term(0), (double)G2_term(1), (double)G2_term(2));
+				vehicle_torque_setpoint.xyz[0] += PX4_ISFINITE(filtered_body_torque_setpoint(0)) ? filtered_body_torque_setpoint(0) : 0.f;
+				vehicle_torque_setpoint.xyz[1] += PX4_ISFINITE(filtered_body_torque_setpoint(1)) ? filtered_body_torque_setpoint(1) : 0.f;
+				vehicle_torque_setpoint.xyz[2] += PX4_ISFINITE(filtered_body_torque_setpoint(2)) ? filtered_body_torque_setpoint(2) : 0.f;
+
+				vehicle_torque_setpoint.g1_term[0] = PX4_ISFINITE(G1_term(0)) ? G1_term(0) : 0.f;
+				vehicle_torque_setpoint.g1_term[1] = PX4_ISFINITE(G1_term(1)) ? G1_term(1) : 0.f;
+				vehicle_torque_setpoint.g1_term[2] = PX4_ISFINITE(G1_term(2)) ? G1_term(2) : 0.f;
+				vehicle_torque_setpoint.g2_term[0] = PX4_ISFINITE(G2_term(0)) ? G2_term(0) : 0.f;
+				vehicle_torque_setpoint.g2_term[1] = PX4_ISFINITE(G2_term(1)) ? G2_term(1) : 0.f;
+				vehicle_torque_setpoint.g2_term[2] = PX4_ISFINITE(G2_term(2)) ? G2_term(2) : 0.f;
+
+				vehicle_torque_setpoint.filtered_xyz[0] = PX4_ISFINITE(filtered_body_torque_setpoint(0)) ? filtered_body_torque_setpoint(0) : 0.f;
+				vehicle_torque_setpoint.filtered_xyz[1] = PX4_ISFINITE(filtered_body_torque_setpoint(1)) ? filtered_body_torque_setpoint(1) : 0.f;
+				vehicle_torque_setpoint.filtered_xyz[2] = PX4_ISFINITE(filtered_body_torque_setpoint(2)) ? filtered_body_torque_setpoint(2) : 0.f;
+
+				_prev_esc_rad_per_sec_filtered = filtered_radps_vec;
+			}
 
 			// scale setpoints by battery status if enabled
 			if (_param_mc_bat_scale_en.get()) {