Browse Source

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

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
release/1.12
Silvan Fuhrer 4 years ago committed by Daniel Agar
parent
commit
92f2043d8b
  1. 9
      msg/tecs_status.msg
  2. 1
      src/lib/tecs/TECS.hpp
  3. 9
      src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
  4. 2
      src/modules/mavlink/streams/HIGH_LATENCY2.hpp
  5. 2
      src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp

9
msg/tecs_status.msg

@ -12,10 +12,11 @@ float32 altitude_sp @@ -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

@ -144,6 +144,7 @@ public: @@ -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

@ -341,14 +341,15 @@ FixedwingPositionControl::tecs_status_publish() @@ -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

@ -525,7 +525,7 @@ private: @@ -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

@ -78,7 +78,7 @@ private: @@ -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);

Loading…
Cancel
Save