fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(fw_att_control): magic numbers

Browse Source
This commit is contained in:
ttechnick 2026-03-03 10:41:12 +01:00 committed by Nick
parent fdf258c2aa
commit 7e5aca5540
4 changed files with 15 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -85,7 +85,7 @@ FixedwingAttitudeControl::parameters_update()
}
void
FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body, const float roll)
{
if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
@ -103,7 +103,9 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
pitch_body = constrain(pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
float yaw_sp = yaw_body + sinf(roll) * 0.0f;
const Quatf q(Eulerf(roll_body, pitch_body, yaw_sp));
q.copyTo(_att_sp.q_d);
_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
@ -252,7 +254,7 @@ void FixedwingAttitudeControl::Run()
const matrix::Eulerf euler_angles(_R);
vehicle_manual_poll(euler_angles.psi());
vehicle_manual_poll(euler_angles.psi(), euler_angles.phi());
vehicle_attitude_setpoint_poll();
@ -294,57 +296,22 @@ void FixedwingAttitudeControl::Run()
const Quatf q_sp(_att_sp.q_d);
if (q_sp.isAllFinite()) {
Quatf q_current(att.q);
const Quatf q_current(att.q);
const Vector3f ez_world(0.f, 0.f, 1.f);
const Vector3f ex_c = q_current.dcm_x();
const Vector3f ez_c = q_current.dcm_z();
const Vector3f ez_sp = q_sp.dcm_z();
// Tilt-only error quaternion
Quatf q_tilt_err(ez_c, ez_sp);
// Reduced desired attitude
Quatf q_des_red = (q_tilt_err * q_current).normalized();
Quatf q_err = (q_current.inversed() * q_des_red).canonical();
Quatf q_err = (q_current.inversed() * q_sp).canonical();
Vector3f e = 2.f * q_err.imag();
Vector3f body_rates_setpoint;
body_rates_setpoint(0) = _proportional_gain(0) * e(0);
body_rates_setpoint(1) = _proportional_gain(1) * e(1) * 2.5f; // multiplied by 2.5 to maintain the gains
body_rates_setpoint(2) = 0.f;
body_rates_setpoint(1) = _proportional_gain(1) * e(1);
body_rates_setpoint(2) = 0.f; // yaw handled later
// Turn coordination
const float V = math::max(get_airspeed_constrained(), 0.1f);
const float r_tc_ff = 9.81f * 2.0f * (q_current(0) * q_current(1) + q_current(2) * q_current(3)) / V;
Vector3f x_h = ex_c - ez_world * ex_c.dot(ez_world); // forward projected to horizontal
float r_tc_ff = 0.f;
body_rates_setpoint(2) = r_tc_ff;
const float xhn = x_h.norm();
if (xhn > 1e-3f) {
x_h /= xhn;
Vector3f y_h = ez_world.cross(x_h);
const float yhn = y_h.norm();
if (yhn > 1e-6f) {
y_h /= yhn;
const float cos_tilt = ez_c.dot(ez_world);
const float sin_bank = ez_c.dot(y_h);
float tan_bank = 0.f;
if (fabsf(cos_tilt) > 0.1f) {
tan_bank = sin_bank / cos_tilt;
}
r_tc_ff = (9.81f / V) * tan_bank * 0.6f;
}
}
body_rates_setpoint(2) = -r_tc_ff;
body_rates_setpoint(0) = math::constrain(body_rates_setpoint(0), -radians(_param_fw_r_rmax.get()), radians(_param_fw_r_rmax.get()));
body_rates_setpoint(1) = math::constrain(body_rates_setpoint(1), -radians(_param_fw_p_rmax_neg.get()), radians(_param_fw_p_rmax_pos.get()));

2
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -165,7 +165,7 @@ private:
WheelController _wheel_ctrl;
void parameters_update();
void vehicle_manual_poll(const float yaw_body);
void vehicle_manual_poll(const float yaw_body, const float roll);
void vehicle_attitude_setpoint_poll();
void vehicle_land_detected_poll();
float get_airspeed_constrained();

3
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
View File

@ -299,7 +299,7 @@ void FwLateralLongitudinalControl::Run()
float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f;
float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f;
float yaw_body = _yaw;
float yaw_body = _yaw + sinf(roll_body) * 0.45f;
const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f;
@ -610,6 +610,7 @@ void FwLateralLongitudinalControl::updateAttitude() {
const Eulerf euler_angles(R);
_long_control_state.pitch_rad = euler_angles.theta();
_yaw = euler_angles.psi();
_roll = euler_angles.phi();
// load factor due to banking
const float load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON);

1
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
View File

@ -192,6 +192,7 @@ private:
hrt_abstime _time_wind_last_received{0};
SlewRate<float> _roll_slew_rate;
float _yaw{0.f};
float _roll{0.f};
struct lateral_control_state {
matrix::Vector2f ground_speed;
matrix::Vector2f wind_speed;