dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

TECS: add EAS_sp to tecs_status.msg and rename other airspeeds to TAS 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-01-19 10:18:29 +01:00 committed by Daniel Agar
parent 5e32e9be5a
commit 92f2043d8b
5 changed files with 13 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
msg/tecs_status.msg
View File

@ -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

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

@ -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; }

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

@ -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();

2
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
View File

@ -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);
}
}

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

@ -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);