From ffc82b0bc5c8f209f6928e41253ed94c209a0421 Mon Sep 17 00:00:00 2001 From: AshhadRasheed Date: Thu, 29 Feb 2024 19:47:15 +0400 Subject: [PATCH] enabling angle of attack logging in tecs_status message --- msg/TecsStatus.msg | 1 + src/modules/fw_pos_control/FixedwingPositionControl.cpp | 9 +++++++++ src/modules/fw_pos_control/FixedwingPositionControl.hpp | 1 + 3 files changed, 11 insertions(+) diff --git a/msg/TecsStatus.msg b/msg/TecsStatus.msg index ae6835dc5249..92a4298940c2 100644 --- a/msg/TecsStatus.msg +++ b/msg/TecsStatus.msg @@ -15,6 +15,7 @@ float32 true_airspeed_derivative_raw # True airspeed derivative raw [m/s^2] float32 total_energy_rate_sp # Total energy rate setpoint [m^2/s^3] float32 total_energy_rate # Total energy rate estimate [m^2/s^3] +float32 alpha # Angle of attack in radians float32 total_energy_balance_rate_sp # Energy balance rate setpoint [m^2/s^3] float32 total_energy_balance_rate # Energy balance rate estimate [m^2/s^3] diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 8b79e1ee4d02..e259c2dd10ec 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -438,6 +438,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint(); tecs_status.throttle_trim = throttle_trim; tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio(); + tecs_status.alpha = _aoa; tecs_status.timestamp = hrt_absolute_time(); @@ -1056,6 +1057,14 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co // waypoint is a plain navigation waypoint float position_sp_alt = pos_sp_curr.alt; + if (_airspeed_valid) + { + const Vector2f airspeed_2d{_airspeed_eas*cosf(_yaw), _airspeed_eas*sinf(_yaw)}; + + const float air_gnd_angle = acosf((airspeed_2d*ground_speed)/(airspeed_2d.length()*ground_speed.length())); + _aoa = air_gnd_angle; + } + // Altitude first order hold (FOH) if (_position_setpoint_previous_valid && ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) || diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 43887707c8be..651227f509e3 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -265,6 +265,7 @@ class FixedwingPositionControl final : public ModuleBase