fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 16:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cleanup manual mode

Browse Source
This commit is contained in:
JaeyoungLim
2025-08-04 23:41:28 +09:00
parent 4266ecc124
commit 45dc5f1e8e
1 changed files with 19 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+19 -39
View File
@@ -868,7 +868,25 @@ FixedwingPositionINDIControl::Run()
// compute control input
// =====================
Vector3f acc_command = path_following_control(pos_ref, vel_ref, acc_ref);
Dcmf R_ref = compute_indi_acc(acc_command, vel_ref);
Dcmf R_ref;
// ====================================
// manual attitude setpoint feedthrough
// ====================================
if (_switch_manual) {
// get an attitude setpoint from the current manual inputs
float roll_ref = 1.f * _manual_control_setpoint.roll * 1.0f;
float pitch_ref = -1.f * _manual_control_setpoint.pitch * M_PI_4_F;
Eulerf E_current(Quatf(_attitude.q));
float yaw_ref = E_current.psi();
Dcmf R_ned_frd_ref(Eulerf(roll_ref, pitch_ref, yaw_ref));
Dcmf R_enu_frd_ref(_R_ned_to_enu * R_ned_frd_ref);
Quatf att_ref(R_enu_frd_ref);
R_ref = Dcmf(att_ref);
omega_ref = Vector3f{0.f, 0.f, 0.f};
} else {
R_ref = compute_indi_acc(acc_command, vel_ref);
}
Vector3f ang_acc_command = attitude_control(R_ref, omega_ref, alpha_ref);
Vector3f moment_command = compute_indi_rot(ang_acc_command);
@@ -1473,44 +1491,6 @@ Vector3f FixedwingPositionINDIControl::attitude_control(Dcmf R_ref, Vector3f ome
(double)(q_err.axis()*q_err.axis()));
}
// ====================================
// manual attitude setpoint feedthrough
// ====================================
if (_switch_manual) {
// get an attitude setpoint from the current manual inputs
float roll_ref = 1.f * _manual_control_setpoint.roll * 1.0f;
float pitch_ref = -1.f * _manual_control_setpoint.pitch * M_PI_4_F;
Eulerf E_current(Quatf(_attitude.q));
float yaw_ref = E_current.psi();
Dcmf R_ned_frd_ref(Eulerf(roll_ref, pitch_ref, yaw_ref));
Dcmf R_enu_frd_ref(_R_ned_to_enu * R_ned_frd_ref);
Quatf att_ref(R_enu_frd_ref);
R_ref = Dcmf(att_ref);
// get attitude error
R_ref_true = Dcmf(R_ref.transpose() * R_ib);
// get required rotation vector (in body frame)
q_err = AxisAnglef(R_ref_true);
// project rotation angle to [-pi,pi]
if (q_err.angle()*q_err.angle() < M_PI_F * M_PI_F) {
w_err = -q_err.angle() * q_err.axis();
} else {
if (q_err.angle() > 0.f) {
w_err = (2.f * M_PI_F - (float)fmod(q_err.angle(), 2.f * M_PI_F)) * q_err.axis();
} else {
w_err = (-2.f * M_PI_F - (float)fmod(q_err.angle(), 2.f * M_PI_F)) * q_err.axis();
}
}
_w_err = w_err;
// compute rot acc command
rot_acc_command = _K_q * w_err + _K_w * (Vector3f{0.f, 0.f, 0.f} -_omega);
}
// ==============================================================
// overwrite rudder rot_acc_command with turn coordination values