fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 04:01:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: directly publish tecs_status from within TECS instance

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-11-25 09:50:28 +01:00
parent c5fb68b7a1
commit e76734fa5e
4 changed files with 76 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+58 -1
View File
@@ -227,7 +227,7 @@ void TECS::_update_energy_estimates()
// Calculate the specific energy balance demand which specifies how the available total
// energy should be allocated to speed (kinetic energy) and height (potential energy)
// Calculate the specific energy balance error
_SEB_error = SEB_setpoint() - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting);
_SEB_error = get_SEB_setpoint() - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting);
// Calculate specific energy rate demands in units of (m**2/sec**3)
_SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
@@ -623,6 +623,7 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
_tecs_mode = ECL_TECS_MODE_NORMAL;
}
tecs_status_publish(now);
}
void TECS::_update_speed_height_weights()
@@ -642,3 +643,59 @@ void TECS::_update_speed_height_weights()
_SPE_weighting = constrain(2.0f - _SKE_weighting, 0.f, 1.f);
_SKE_weighting = constrain(_SKE_weighting, 0.f, 1.f);
}
void
TECS::tecs_status_publish(const hrt_abstime &now)
{
tecs_status_s tecs_status{};
switch (_tecs_mode) {
case TECS::ECL_TECS_MODE_NORMAL:
tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL;
break;
case TECS::ECL_TECS_MODE_UNDERSPEED:
tecs_status.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
break;
case TECS::ECL_TECS_MODE_BAD_DESCENT:
tecs_status.mode = tecs_status_s::TECS_MODE_BAD_DESCENT;
break;
case TECS::ECL_TECS_MODE_CLIMBOUT:
tecs_status.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
break;
}
tecs_status.altitude_sp = _hgt_setpoint;
tecs_status.altitude_filtered = _vert_pos_state;
tecs_status.energy_distribution_error = _SEB_error;
tecs_status.energy_distribution_rate_error = _SEB_rate_error;
tecs_status.equivalent_airspeed_sp = _EAS_setpoint;
tecs_status.height_rate_setpoint = _hgt_rate_setpoint;
tecs_status.height_rate = _vert_vel_state;
tecs_status.pitch_integ = _pitch_integ_state;
tecs_status.pitch_sp_rad = get_pitch_setpoint();
tecs_status.throttle_integ = _throttle_integ_state;
tecs_status.throttle_sp = get_throttle_setpoint();
tecs_status.total_energy_balance = get_SEB();
tecs_status.total_energy_balance_sp = get_SEB_setpoint();
tecs_status.total_energy_balance_rate = get_SEB_rate();
tecs_status.total_energy_balance_rate_sp = get_SEB_rate_setpoint();
tecs_status.total_energy = get_STE();
tecs_status.total_energy_error = _STE_error;
tecs_status.total_energy_sp = get_STE_setpoint();
tecs_status.total_energy_rate = get_STE_rate();
tecs_status.total_energy_rate_error = _STE_rate_error;
tecs_status.total_energy_rate_sp = get_STE_rate_setpoint();
tecs_status.true_airspeed_sp = _TAS_setpoint_adj;
tecs_status.true_airspeed_filtered = _tas_state;
tecs_status.true_airspeed_derivative = _tas_rate_filtered;
tecs_status.true_airspeed_derivative_raw = _tas_rate_raw;
tecs_status.true_airspeed_derivative_sp = _TAS_rate_setpoint;
tecs_status.true_airspeed_innovation = _tas_innov;
tecs_status.timestamp = now;
_tecs_status_pub.publish(tecs_status);
}
src/lib/tecs/TECS.hpp
+18 -46
View File
@@ -49,6 +49,10 @@
#include <motion_planning/VelocitySmoothing.hpp>
#include <motion_planning/ManualVelocitySmoothingZ.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/tecs_status.h>
#include <uORB/uORB.h>
class TECS
{
public:
@@ -92,10 +96,6 @@ public:
float throttle_min, float throttle_setpoint_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN);
float get_throttle_setpoint() { return _last_throttle_setpoint; }
float get_pitch_setpoint() { return _last_pitch_setpoint; }
float get_speed_weight() { return _pitch_speed_weight; }
void reset_state() { _states_initialized = false; }
enum ECL_TECS_MODE {
@@ -141,50 +141,22 @@ public:
void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }
// TECS status
uint64_t timestamp() { return _pitch_update_timestamp; }
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
// getters
float get_throttle_setpoint() { return _last_throttle_setpoint; }
float get_pitch_setpoint() { return _last_pitch_setpoint; }
float hgt_setpoint() { return _hgt_setpoint; }
float vert_pos_state() { return _vert_pos_state; }
float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
float tas_state() { return _tas_state; }
float getTASInnovation() { return _tas_innov; }
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 _tas_rate_filtered; }
float speed_derivative_raw() { return _tas_rate_raw; }
float STE_error() { return _STE_error; }
float STE_rate_error() { return _STE_rate_error; }
float SEB_error() { return _SEB_error; }
float SEB_rate_error() { return _SEB_rate_error; }
float throttle_integ_state() { return _throttle_integ_state; }
float pitch_integ_state() { return _pitch_integ_state; }
float STE() { return _SPE_estimate + _SKE_estimate; }
float STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; }
float STE_rate() { return _SPE_rate + _SKE_rate; }
float STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; }
float SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; }
float SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; }
float SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; }
float SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; }
float get_hgt_setpoint() { return _hgt_setpoint; }
float get_hgt_rate_setpoint() { return _hgt_rate_setpoint; }
float get_STE() { return _SPE_estimate + _SKE_estimate; }
float get_STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; }
float get_STE_rate() { return _SPE_rate + _SKE_rate; }
float get_STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; }
float get_SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; }
float get_SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; }
float get_SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; }
float get_SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; }
void tecs_status_publish(const hrt_abstime &now);
/**
* Handle the altitude reset
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
-67
View File
@@ -399,70 +399,6 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const
return constrain(airspeed_demand, adjusted_min_airspeed, _param_fw_airspd_max.get());
}
void
FixedwingPositionControl::tecs_status_publish()
{
tecs_status_s t{};
switch (_tecs.tecs_mode()) {
case TECS::ECL_TECS_MODE_NORMAL:
t.mode = tecs_status_s::TECS_MODE_NORMAL;
break;
case TECS::ECL_TECS_MODE_UNDERSPEED:
t.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
break;
case TECS::ECL_TECS_MODE_BAD_DESCENT:
t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT;
break;
case TECS::ECL_TECS_MODE_CLIMBOUT:
t.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
break;
}
t.altitude_sp = _tecs.hgt_setpoint();
t.altitude_filtered = _tecs.vert_pos_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.equivalent_airspeed_sp = _tecs.get_EAS_setpoint();
t.true_airspeed_derivative_sp = _tecs.TAS_rate_setpoint();
t.true_airspeed_derivative = _tecs.speed_derivative();
t.true_airspeed_derivative_raw = _tecs.speed_derivative_raw();
t.true_airspeed_innovation = _tecs.getTASInnovation();
t.total_energy_error = _tecs.STE_error();
t.total_energy_rate_error = _tecs.STE_rate_error();
t.energy_distribution_error = _tecs.SEB_error();
t.energy_distribution_rate_error = _tecs.SEB_rate_error();
t.total_energy = _tecs.STE();
t.total_energy_rate = _tecs.STE_rate();
t.total_energy_balance = _tecs.SEB();
t.total_energy_balance_rate = _tecs.SEB_rate();
t.total_energy_sp = _tecs.STE_setpoint();
t.total_energy_rate_sp = _tecs.STE_rate_setpoint();
t.total_energy_balance_sp = _tecs.SEB_setpoint();
t.total_energy_balance_rate_sp = _tecs.SEB_rate_setpoint();
t.throttle_integ = _tecs.throttle_integ_state();
t.pitch_integ = _tecs.pitch_integ_state();
t.throttle_sp = _tecs.get_throttle_setpoint();
t.pitch_sp_rad = _tecs.get_pitch_setpoint();
t.timestamp = hrt_absolute_time();
_tecs_status_pub.publish(t);
}
void
FixedwingPositionControl::status_publish()
@@ -612,7 +548,6 @@ FixedwingPositionControl::getManualHeightRateSetpoint()
const float climb_rate_target = _param_climbrate_target.get();
height_rate_setpoint = pitch * climb_rate_target;
}
return height_rate_setpoint;
@@ -2226,8 +2161,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
pitch_min_rad - radians(_param_fw_psp_off.get()),
pitch_max_rad - radians(_param_fw_psp_off.get()),
_param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp);
tecs_status_publish();
}
void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
-2
View File
@@ -76,7 +76,6 @@
#include <uORB/topics/position_controller_landing_status.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -284,7 +283,6 @@ private:
void status_publish();
void landing_status_publish();
void tecs_status_publish();
void abort_landing(bool abort);