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

TECS: rename tecs_status.altitude_filtered to altitude_sp_ref

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-01-24 11:48:15 +01:00
parent 06f4195663
commit 88ec117e59
5 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/TecsStatus.msg
View File

@ -1,7 +1,7 @@
uint64 timestamp # time since system start (microseconds)
float32 altitude_sp # Altitude setpoint AMSL [m]
float32 altitude_filtered # Altitude filtered AMSL [m]
float32 altitude_sp_filtered # Altitude setpoint filtered AMSL [m]
float32 height_rate_setpoint # Height rate setpoint [m/s]
float32 height_rate # Height rate [m/s]
float32 equivalent_airspeed_sp # Equivalent airpseed setpoint [m/s]

4
src/lib/tecs/TECS.cpp
View File

@ -706,7 +706,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
const TECSReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()};
_debug_status.altitude_sp = ref_alt.alt;
_debug_status.altitude_sp_ref = ref_alt.alt;
_debug_status.altitude_rate_alt_ref = ref_alt.alt_rate;
_debug_status.altitude_rate_feedforward = _reference_model.getAltitudeRateReference();
@ -798,7 +798,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
_debug_status.control = _control.getDebugOutput();
_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
_debug_status.altitude_sp = control_setpoint.altitude_reference.alt;
_debug_status.altitude_sp_ref = control_setpoint.altitude_reference.alt;
_debug_status.altitude_rate_alt_ref = control_setpoint.altitude_reference.alt_rate;
_debug_status.altitude_rate_feedforward = control_setpoint.altitude_rate_setpoint;
}

2
src/lib/tecs/TECS.hpp
View File

@ -551,7 +551,7 @@ public:
TECSControl::DebugOutput control;
float true_airspeed_filtered;
float true_airspeed_derivative;
float altitude_sp;
float altitude_sp_ref;
float altitude_rate_alt_ref;
float altitude_rate_feedforward;
enum ECL_TECS_MODE tecs_mode;

2
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -484,7 +484,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
}
t.altitude_sp = alt_sp;
t.altitude_filtered = debug_output.altitude_sp;
t.altitude_sp_filtered = debug_output.altitude_sp_ref;
t.height_rate_setpoint = debug_output.control.altitude_rate_control;
t.height_rate = -_local_pos.vz;
t.equivalent_airspeed_sp = equivalent_airspeed_sp;

9
src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -36,6 +36,7 @@
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_global_position.h>
class MavlinkStreamNavControllerOutput : public MavlinkStream
{
@ -59,6 +60,7 @@ private:
uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)};
uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
bool send() override
{
@ -69,6 +71,9 @@ private:
tecs_status_s tecs_status{};
_tecs_status_sub.copy(&tecs_status);
vehicle_global_position_s vehicle_global_position{};
_vehicle_global_position_sub.copy(&vehicle_global_position);
mavlink_nav_controller_output_t msg{};
msg.nav_roll = math::degrees(pos_ctrl_status.nav_roll);
@ -77,7 +82,7 @@ private:
msg.target_bearing = roundf(math::degrees(pos_ctrl_status.target_bearing));
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.alt_error = tecs_status.altitude_sp - vehicle_global_position.alt;
msg.aspd_error = tecs_status.true_airspeed_filtered - tecs_status.true_airspeed_sp;
mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);