fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 13:39:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: add flight_phase field in tecs_status (Level, Climb, Descend)

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-11-08 17:54:36 +01:00
parent e76734fa5e
commit 99e1e9deac
3 changed files with 47 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
msg/tecs_status.msg
View File

@ -42,3 +42,9 @@ float32 throttle_sp
float32 pitch_sp_rad
uint8 mode
uint8 flight_phase
uint8 TECS_FLIGHT_PHASE_LEVEL = 0 # In level flight (no altitude setpoint change)
uint8 TECS_FLIGHT_PHASE_CLIMB = 1 # Climb is triggered (altitude setpoint is increasing)
uint8 TECS_FLIGHT_PHASE_DESCEND = 2 # Descend is triggered (altitude setpoint is descreasing)

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

@ -452,6 +452,38 @@ void TECS::_updateTrajectoryGenerationConstraints()
_velocity_control_traj_generator.setMaxVelDown(_max_climb_rate); // different convention for FW than for MC
}
void TECS::_updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint)
{
const bool input_is_height_rate = PX4_ISFINITE(height_rate_setpoint);
// update flight phase state (are we flying level, climbing or descending)
if (input_is_height_rate) {
if (PX4_ISFINITE(_velocity_control_traj_generator.getCurrentPosition())) {
// we have a valid altitude setpoint which means that we are flying level
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_LEVEL;
} else if (_velocity_control_traj_generator.getCurrentVelocity() > FLT_EPSILON) {
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_CLIMB;
} else {
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_DESCEND;
}
} else {
// stay in flight phase level if only small altitude changes are demanded (<10m)
if (altitude_sp_amsl - _alt_control_traj_generator.getCurrentPosition() > 10.0f) {
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_CLIMB;
} else if (altitude_sp_amsl - _alt_control_traj_generator.getCurrentPosition() < -10.0f) {
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_DESCEND;
} else {
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_LEVEL;
}
}
}
void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
float target_sinkrate, float altitude_amsl)
{
@ -594,6 +626,8 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
// Calculate the demanded true airspeed
_update_speed_setpoint();
_updateFlightPhase(hgt_setpoint, hgt_rate_sp);
_calculateHeightRateSetpoint(hgt_setpoint, hgt_rate_sp, target_climbrate, target_sinkrate, baro_altitude);
// Calculate the specific energy values required by the control loop
@ -672,6 +706,7 @@ TECS::tecs_status_publish(const hrt_abstime &now)
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.flight_phase = _flight_phase;
tecs_status.height_rate_setpoint = _hgt_rate_setpoint;
tecs_status.height_rate = _vert_vel_state;
tecs_status.pitch_integ = _pitch_integ_state;

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

@ -155,6 +155,7 @@ public:
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; }
int get_flight_phase() { return _flight_phase; }
void tecs_status_publish(const hrt_abstime &now);
@ -280,6 +281,9 @@ private:
bool _states_initialized{false}; ///< true when TECS states have been iniitalized
bool _in_air{false}; ///< true when the vehicle is flying
// flight phase
int _flight_phase{tecs_status_s::TECS_FLIGHT_PHASE_LEVEL};
/**
* Update the airspeed internal state using a second order complementary filter
*/
@ -323,6 +327,8 @@ private:
void _updateTrajectoryGenerationConstraints();
void _updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint);
void _calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
float target_sinkrate, float altitude_amsl);