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

FwLateralLongitudinalControl: publish flight phase also if unknown, and with limited rate (#26251)

Browse Source 
* FwLateralLongitudinalControl: publish uknown flight phase if TECS not running

* FwLateralLongitudinalControl: publish flight phase with lower rate

For this we store the new flight phase in a local variable, which is
returned by tecs_update_pitch_throttle (but initialised outside to
unknown in case TECS does not run).
This commit is contained in:
Balduin 2026-01-12 15:30:26 +01:00 committed by GitHub
parent de49edc428
commit 66e21497a6
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 39 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
View File

@ -166,6 +166,8 @@ void FwLateralLongitudinalControl::Run()
_landed = landed.landed;
}
uint8_t current_flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
_vehicle_status_sub.update();
_control_mode_sub.update();
@ -209,18 +211,18 @@ void FwLateralLongitudinalControl::Run()
// If the both altitude and height rate are set, set altitude setpoint to NAN
const float altitude_sp = PX4_ISFINITE(_long_control_sp.height_rate) ? NAN : _long_control_sp.altitude;
tecs_update_pitch_throttle(control_interval, altitude_sp,
airspeed_sp_eas,
_long_configuration.pitch_min,
_long_configuration.pitch_max,
_long_configuration.throttle_min,
_long_configuration.throttle_max,
_long_configuration.sink_rate_target,
_long_configuration.climb_rate_target,
_long_configuration.disable_underspeed_protection,
_long_control_sp.height_rate,
now
);
current_flight_phase = tecs_update_pitch_throttle(control_interval, altitude_sp,
airspeed_sp_eas,
_long_configuration.pitch_min,
_long_configuration.pitch_max,
_long_configuration.throttle_min,
_long_configuration.throttle_max,
_long_configuration.sink_rate_target,
_long_configuration.climb_rate_target,
_long_configuration.disable_underspeed_protection,
_long_control_sp.height_rate,
now
);
pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_direct) ? _long_control_sp.pitch_direct : _tecs.get_pitch_setpoint();
throttle_sp = PX4_ISFINITE(_long_control_sp.throttle_direct) ? _long_control_sp.throttle_direct :
@ -317,6 +319,16 @@ void FwLateralLongitudinalControl::Run()
}
// Publish flight phase with low rate, but immediately if updated
const bool flight_phase_updated = current_flight_phase != _flight_phase_estimation_pub.get().flight_phase;
const hrt_abstime time_since_last_flightphase_pub = now - _flight_phase_estimation_pub.get().timestamp;
if (flight_phase_updated || time_since_last_flightphase_pub >= 1_s) {
_flight_phase_estimation_pub.get().timestamp = now;
_flight_phase_estimation_pub.get().flight_phase = current_flight_phase;
_flight_phase_estimation_pub.update();
}
_z_reset_counter = _local_pos.z_reset_counter;
}
@ -356,7 +368,7 @@ void FwLateralLongitudinalControl::updateControllerConfiguration(hrt_abstime tim
}
}
void
uint8_t
FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad, float throttle_min,
float throttle_max, const float desired_max_sinkrate,
@ -370,8 +382,7 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|| _vehicle_status_sub.get().in_transition_mode)) {
tecs_is_running = false;
return;
return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
}
const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min,
@ -409,25 +420,23 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
// Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving
if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) &&
fabsf(_long_control_state.altitude_msl - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) {
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL;
return flight_phase_estimation_s::FLIGHT_PHASE_LEVEL;
} else if (((tecs_output.altitude_reference - _long_control_state.altitude_msl) >= _param_nav_fw_alt_rad.get()) ||
(tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB;
return flight_phase_estimation_s::FLIGHT_PHASE_CLIMB;
} else if (((_long_control_state.altitude_msl - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) ||
(tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;
return flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;
} else {
// We can't infer the flight phase , do nothing, estimation is reset at each step
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
// We can't infer the flight phase
return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
}
_flight_phase_estimation_pub.get().timestamp = now;
_flight_phase_estimation_pub.update();
}
return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
}
void

11
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
View File

@ -213,11 +213,12 @@ private:
void parameters_update();
void update_control_state(hrt_abstime now);
void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad, float throttle_min,
float throttle_max, const float desired_max_sinkrate,
const float desired_max_climbrate,
bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now);
uint8_t tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad, float throttle_min,
float throttle_max, const float desired_max_sinkrate,
const float desired_max_climbrate,
bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now);
void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
float throttle_trim, hrt_abstime now);