diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index 5a87d26cae..7e4fe08228 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -12,10 +12,11 @@ float32 altitude_sp float32 altitude_filtered float32 height_rate_setpoint float32 height_rate -float32 airspeed_sp -float32 airspeed_filtered -float32 airspeed_derivative_sp -float32 airspeed_derivative +float32 equivalent_airspeed_sp +float32 true_airspeed_sp +float32 true_airspeed_filtered +float32 true_airspeed_derivative_sp +float32 true_airspeed_derivative float32 total_energy_error float32 energy_distribution_error diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 619a83cffa..3e47406630 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -144,6 +144,7 @@ public: float hgt_rate_setpoint() { return _hgt_rate_setpoint; } float vert_vel_state() { return _vert_vel_state; } + float get_EAS_setpoint() { return _EAS_setpoint; }; float TAS_rate_setpoint() { return _TAS_rate_setpoint; } float speed_derivative() { return _speed_derivative; } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 84c1a10725..4b592cae5f 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -341,14 +341,15 @@ FixedwingPositionControl::tecs_status_publish() t.altitude_sp = _tecs.hgt_setpoint_adj(); t.altitude_filtered = _tecs.vert_pos_state(); - t.airspeed_sp = _tecs.TAS_setpoint_adj(); - t.airspeed_filtered = _tecs.tas_state(); + t.true_airspeed_sp = _tecs.TAS_setpoint_adj(); + t.true_airspeed_filtered = _tecs.tas_state(); t.height_rate_setpoint = _tecs.hgt_rate_setpoint(); t.height_rate = _tecs.vert_vel_state(); - t.airspeed_derivative_sp = _tecs.TAS_rate_setpoint(); - t.airspeed_derivative = _tecs.speed_derivative(); + t.equivalent_airspeed_sp = _tecs.get_EAS_setpoint(); + t.true_airspeed_derivative_sp = _tecs.TAS_rate_setpoint(); + t.true_airspeed_derivative = _tecs.speed_derivative(); t.total_energy_error = _tecs.STE_error(); t.total_energy_rate_error = _tecs.STE_rate_error(); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index cee6010bc4..a1e913f54a 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -525,7 +525,7 @@ private: tecs_status_s tecs_status; if (_tecs_status_sub.update(&tecs_status)) { - _airspeed_sp.add_value(tecs_status.airspeed_sp, _update_rate_filtered); + _airspeed_sp.add_value(tecs_status.true_airspeed_sp, _update_rate_filtered); } } diff --git a/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp b/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp index a1a7b7a7cf..6553127dc3 100644 --- a/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp +++ b/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp @@ -78,7 +78,7 @@ private: msg.wp_dist = math::constrain(roundf(pos_ctrl_status.wp_dist), 0.f, (float)UINT16_MAX); msg.xtrack_error = pos_ctrl_status.xtrack_error; msg.alt_error = tecs_status.altitude_filtered - tecs_status.altitude_sp; - msg.aspd_error = tecs_status.airspeed_filtered - tecs_status.airspeed_sp; + msg.aspd_error = tecs_status.true_airspeed_filtered - tecs_status.true_airspeed_sp; mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);