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 (#26219)

Browse Source 
* FwLateralLongitudinalControl: publish flight phase

* FwLateralLongitudinalControl: consolidate hrt_absolute_time calls

* FwLateralLongitudinalControl: Name time variables correctly

* FwLateralLongitudinalControl: pass current time as argument rather than class member

* FwLateralLongitudinalControl: use local position timestamp
This commit is contained in:
Balduin 2026-01-07 11:10:38 +01:00 committed by GitHub
parent 1ff36422c9
commit 450cf79fc8
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 48 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -141,11 +141,12 @@ void FwLateralLongitudinalControl::Run()
if (_local_pos_sub.update(&_local_pos)) {
const float control_interval = math::constrain((_local_pos.timestamp - _last_time_loop_ran) * 1e-6f,
0.001f, 0.1f);
_last_time_loop_ran = _local_pos.timestamp;
const hrt_abstime now = _local_pos.timestamp_sample;
updateControllerConfiguration();
const float control_interval = math::constrain((now - _last_time_loop_ran) * 1e-6f, 0.001f, 0.1f);
_last_time_loop_ran = now;
updateControllerConfiguration(now);
_tecs.set_speed_weight(_long_configuration.speed_weight);
updateTECSAltitudeTimeConstant(checkLowHeightConditions()
@ -165,8 +166,6 @@ void FwLateralLongitudinalControl::Run()
_landed = landed.landed;
}
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
_vehicle_status_sub.update();
_control_mode_sub.update();
@ -176,7 +175,7 @@ void FwLateralLongitudinalControl::Run()
_flaps_setpoint = flaps_setpoint.normalized_setpoint;
}
update_control_state();
update_control_state(now);
if (_control_mode_sub.get().flag_control_manual_enabled && _control_mode_sub.get().flag_control_altitude_enabled
&& _local_pos.z_reset_counter != _z_reset_counter) {
@ -219,7 +218,8 @@ void FwLateralLongitudinalControl::Run()
_long_configuration.sink_rate_target,
_long_configuration.climb_rate_target,
_long_configuration.disable_underspeed_protection,
_long_control_sp.height_rate
_long_control_sp.height_rate,
now
);
pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_direct) ? _long_control_sp.pitch_direct : _tecs.get_pitch_setpoint();
@ -279,13 +279,13 @@ void FwLateralLongitudinalControl::Run()
lateral_accel_sp = 0.f; // mitigation if no valid setpoint is received: 0 lateral acceleration
}
lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp);
lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp, now);
lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_configuration.lateral_accel_max,
_lateral_configuration.lateral_accel_max);
roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp);
fixed_wing_lateral_status_s fixed_wing_lateral_status{};
fixed_wing_lateral_status.timestamp = hrt_absolute_time();
fixed_wing_lateral_status.timestamp = now;
fixed_wing_lateral_status.lateral_acceleration_setpoint = lateral_accel_sp;
fixed_wing_lateral_status.can_run_factor = _can_run_factor;
@ -307,7 +307,7 @@ void FwLateralLongitudinalControl::Run()
// roll slew rate
roll_body = _roll_slew_rate.update(roll_body, control_interval);
_att_sp.timestamp = hrt_absolute_time();
_att_sp.timestamp = now;
const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
q.copyTo(_att_sp.q_d);
@ -323,16 +323,16 @@ void FwLateralLongitudinalControl::Run()
perf_end(_loop_perf);
}
void FwLateralLongitudinalControl::updateControllerConfiguration()
void FwLateralLongitudinalControl::updateControllerConfiguration(hrt_abstime timestamp)
{
if (_lateral_configuration.timestamp == 0) {
_lateral_configuration.timestamp = _local_pos.timestamp;
_lateral_configuration.timestamp = timestamp;
_lateral_configuration.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G;
}
if (_long_configuration.timestamp == 0) {
setDefaultLongitudinalControlConfiguration();
setDefaultLongitudinalControlConfiguration(timestamp);
}
if (_long_control_configuration_sub.updated() || _parameter_update_sub.updated()) {
@ -361,7 +361,7 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
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)
bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now)
{
bool tecs_is_running = true;
@ -400,7 +400,7 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
_long_control_state.height_rate,
hgt_rate_sp);
tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated);
tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated, now);
if (tecs_is_running && !_vehicle_status_sub.get().in_transition_mode
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
@ -420,14 +420,19 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;
} else {
//We can't infer the flight phase , do nothing, estimation is reset at each step
// 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;
}
_flight_phase_estimation_pub.get().timestamp = now;
_flight_phase_estimation_pub.update();
}
}
void
FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
float true_airspeed_derivative_raw, float throttle_trim)
float true_airspeed_derivative_raw, float throttle_trim, hrt_abstime timestamp)
{
tecs_status_s tecs_status{};
@ -458,7 +463,7 @@ FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent
tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio();
tecs_status.fast_descend_ratio = debug_output.fast_descend;
tecs_status.timestamp = hrt_absolute_time();
tecs_status.timestamp = timestamp;
_tecs_status_pub.publish(tecs_status);
}
@ -531,16 +536,16 @@ fw_lat_lon_control computes attitude and throttle setpoints from lateral and lon
return 0;
}
void FwLateralLongitudinalControl::update_control_state() {
void FwLateralLongitudinalControl::update_control_state(hrt_abstime now) {
updateAltitudeAndHeightRate();
updateAirspeed();
updateAttitude();
updateWind();
updateWind(now);
_lateral_control_state.ground_speed = Vector2f(_local_pos.vx, _local_pos.vy);
}
void FwLateralLongitudinalControl::updateWind() {
void FwLateralLongitudinalControl::updateWind(hrt_abstime now) {
if (_wind_sub.updated()) {
wind_s wind{};
_wind_sub.update(&wind);
@ -549,14 +554,14 @@ void FwLateralLongitudinalControl::updateWind() {
_wind_valid = PX4_ISFINITE(wind.windspeed_north)
&& PX4_ISFINITE(wind.windspeed_east);
_time_wind_last_received = hrt_absolute_time();
_time_wind_last_received = now;
_lateral_control_state.wind_speed(0) = wind.windspeed_north;
_lateral_control_state.wind_speed(1) = wind.windspeed_east;
} else {
// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT;
_wind_valid = _wind_valid && (now - _time_wind_last_received) < WIND_EST_TIMEOUT;
}
if (!_wind_valid) {
@ -735,32 +740,30 @@ float FwLateralLongitudinalControl::getGuidanceQualityFactor(const vehicle_local
return flying_forward_factor * low_ground_speed_factor;
}
float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp)
float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp, hrt_abstime now)
{
// Scale the npfg output to zero if npfg is not certain for correct output
_can_run_factor = math::constrain(getGuidanceQualityFactor(_local_pos, _wind_valid), 0.f, 1.f);
hrt_abstime now{hrt_absolute_time()};
// Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition)
// If the npfg was not running before, reset the user warning variables.
if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) {
if ((now - _time_of_last_npfg_call) > ROLL_WARNING_TIMEOUT) {
_need_report_npfg_uncertain_condition = true;
_time_since_first_reduced_roll = 0U;
_time_of_first_reduced_roll = 0U;
}
if (_vehicle_status_sub.get().in_transition_mode || _can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD || _landed) {
// NPFG reports a good condition or we are in transition, reset the user warning variables.
_need_report_npfg_uncertain_condition = true;
_time_since_first_reduced_roll = 0U;
_time_of_first_reduced_roll = 0U;
} else if (_need_report_npfg_uncertain_condition) {
if (_time_since_first_reduced_roll == 0U) {
_time_since_first_reduced_roll = now;
if (_time_of_first_reduced_roll == 0U) {
_time_of_first_reduced_roll = now;
}
if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) {
if ((now - _time_of_first_reduced_roll) > ROLL_WARNING_TIMEOUT) {
_need_report_npfg_uncertain_condition = false;
events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning,
"Roll command reduced due to uncertain velocity/wind estimates!");
@ -770,7 +773,7 @@ float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float later
// Nothing to do, already reported.
}
_time_since_last_npfg_call = now;
_time_of_last_npfg_call = now;
return _can_run_factor * (lateral_accel_sp);
}
@ -778,8 +781,8 @@ float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float late
return atanf(lateral_acceleration_sp / CONSTANTS_ONE_G);
}
void FwLateralLongitudinalControl::setDefaultLongitudinalControlConfiguration() {
_long_configuration.timestamp = hrt_absolute_time();
void FwLateralLongitudinalControl::setDefaultLongitudinalControlConfiguration(hrt_abstime timestamp) {
_long_configuration.timestamp = timestamp;
_long_configuration.pitch_min = radians(_param_fw_p_lim_min.get());
_long_configuration.pitch_max = radians(_param_fw_p_lim_max.get());
_long_configuration.throttle_min = _param_fw_thr_min.get();

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

@ -195,8 +195,8 @@ private:
matrix::Vector2f wind_speed;
} _lateral_control_state{};
bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed
hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time
hrt_abstime _time_since_last_npfg_call{0U}; ///< absolute time since start when the npfg reduced roll angle calculations was last performed
hrt_abstime _time_of_first_reduced_roll{0U}; ///< absolute time when entering reduced roll angle for the first time
hrt_abstime _time_of_last_npfg_call{0U}; ///< absolute time when the npfg reduced roll angle calculations was last performed
vehicle_attitude_setpoint_s _att_sp{};
bool _landed{false};
float _can_run_factor{0.f};
@ -212,15 +212,15 @@ private:
float _min_airspeed_from_guidance{0.f}; // need to store it bc we only update after running longitudinal controller
void parameters_update();
void update_control_state();
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);
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);
float throttle_trim, hrt_abstime now);
void updateAirspeed();
@ -230,7 +230,7 @@ private:
float mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const;
void updateWind();
void updateWind(hrt_abstime now);
void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt);
@ -238,13 +238,13 @@ private:
float getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const;
float getCorrectedLateralAccelSetpoint(float lateral_accel_sp);
float getCorrectedLateralAccelSetpoint(float lateral_accel_sp, hrt_abstime now);
void setDefaultLongitudinalControlConfiguration();
void setDefaultLongitudinalControlConfiguration(hrt_abstime now);
void updateLongitudinalControlConfiguration(const longitudinal_control_configuration_s &configuration_in);
void updateControllerConfiguration();
void updateControllerConfiguration(hrt_abstime now);
float getLoadFactor() const;