From 450cf79fc868127ee50f5a438d331dab90738d26 Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 7 Jan 2026 11:10:38 +0100 Subject: [PATCH] FwLateralLongitudinalControl: Publish flight phase (#26219) * 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 --- .../FwLateralLongitudinalControl.cpp | 75 ++++++++++--------- .../FwLateralLongitudinalControl.hpp | 18 ++--- 2 files changed, 48 insertions(+), 45 deletions(-) diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index 53005d8ba5..e6b7ad106a 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -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(); diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp index 87b94b1bf3..9f648c4920 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -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;