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

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,
+		const hrt_abstime now = _local_pos.timestamp_sample;
-					       0.001f, 0.1f);
-		_last_time_loop_ran = _local_pos.timestamp;
 
-		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) {
+		if (_time_of_first_reduced_roll == 0U) {
-			_time_since_first_reduced_roll = now;
+			_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() {
+void FwLateralLongitudinalControl::setDefaultLongitudinalControlConfiguration(hrt_abstime timestamp) {
-	_long_configuration.timestamp = hrt_absolute_time();
+	_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();

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_of_first_reduced_roll{0U};       ///< absolute time 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_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;