From 75ee48d8bbfb8716d72909623015bba0c6e2af98 Mon Sep 17 00:00:00 2001
From: JaeyoungLim <jalim@ethz.ch>
Date: Tue, 8 Jul 2025 14:13:54 +0900
Subject: [PATCH] WIP direct actuator control

---
 .../FixedwingIndiPosControl.cpp               | 20 +++++++++++++++----
 .../FixedwingIndiPosControl.hpp               |  3 ++-
 2 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp
index 85baf2851f..cc79294664 100644
--- a/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp
+++ b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp
@@ -312,6 +312,11 @@ Vector3f FixedwingIndiPosControl::controlAttitude(Quatf ref_attitude)
 	Dcmf R_ib(vehicle_attitude_);
 	Dcmf R_bi(R_ib.transpose());
 
+	// Vector3f omega_filtered;
+	// omega_filtered(0) = _lp_filter_omega[0].apply(_omega(0));
+	// omega_filtered(1) = _lp_filter_omega[1].apply(_omega(1));
+	// omega_filtered(2) = _lp_filter_omega[2].apply(_omega(2));
+
 	// get attitude error
 	Dcmf R_ref_true(R_ref.transpose()*R_ib);
 	// get required rotation vector (in body frame)
@@ -335,8 +340,10 @@ Vector3f FixedwingIndiPosControl::controlAttitude(Quatf ref_attitude)
 	// =========================================
 	// apply PD control law on the body attitude
 	// =========================================
-	// Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref;
-	Vector3f rot_vel_command = _K_q * w_err;
+	Vector3f alpha_ref{0.0, 0.0, 0.0};
+	Vector3f omega_ref{0.0, 0.0, 0.0};
+	Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref;
+	// Vector3f rot_vel_command = _K_q * w_err;
 	PX4_INFO("[PX4PosiNDI]   - w_err: %f, %f, %f", double(w_err(0)), double(w_err(1)), double(w_err(2)));
 
 	if (sqrtf(w_err * w_err) > M_PI_F) {
@@ -373,9 +380,13 @@ Vector3f FixedwingIndiPosControl::controlAttitude(Quatf ref_attitude)
 	omega_turn_ref(2) = _lp_filter_rud.apply(omega_turn_ref(2));
 	PX4_INFO("[PX4PosiNDI]   - omega_turn_ref: %f, %f, %f", double(omega_turn_ref(0)), double(omega_turn_ref(1)),
 		 double(omega_turn_ref(2)));
-	rot_vel_command(2) = omega_turn_ref(2);
 
-	return rot_vel_command;
+	// transform rate vector to body frame
+	float scaler = (_stall_speed*_stall_speed)/fmaxf(sqrtf(vel_body*vel_body)*vel_body(0), _stall_speed*_stall_speed);
+	// not really an accel command, rather a FF-P command
+	rot_acc_command(2) = _K_q(2,2)*omega_turn_ref(2)*scaler + _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2))* scaler*scaler;
+
+	return rot_acc_command;
 }
 
 void FixedwingIndiPosControl::Run()
@@ -389,6 +400,7 @@ void FixedwingIndiPosControl::Run()
 	perf_begin(_loop_perf);
 
 	// only run controller if angular velocity changed
+	vehicle_angular_velocity_s angular_velocity;
 	if (_vehicle_angular_velocity_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) { //TODO rate!
 
 		// only update parameters if they changed
diff --git a/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp
index ce1bae9f52..cc73ae37c6 100644
--- a/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp
+++ b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp
@@ -192,6 +192,7 @@ private:
 	matrix::Matrix3f _K_v;
 	matrix::Matrix3f _K_a;
 	matrix::Matrix3f _K_q;
+	matrix::Matrix3f _K_w;
 
 	// Vehicle parameters (From Easyglider)
 	float _aoa_offset{0.07};
@@ -201,7 +202,7 @@ private:
 	float _C_D1{0.3783};
 	float _C_D2{1.984};
 	float _area{0.4};
-	float _stall_speed{1.0};
+	float _stall_speed{10.0};
 	float _mass{1.5};
 
 	bool _landed{true};