From 777a264ab49fed23e4e1246616fb9269161f5da7 Mon Sep 17 00:00:00 2001
From: RomanBapst <bapstroman@gmail.com>
Date: Wed, 8 Jan 2025 11:18:03 +0300
Subject: [PATCH] more cleanup

Signed-off-by: RomanBapst <bapstroman@gmail.com>
---
 .../FwLateralLongitudinalControl.cpp                  |  6 +++---
 .../FwLateralLongitudinalControl.hpp                  |  4 ++--
 .../fw_pos_control/FixedwingPositionControl.cpp       | 11 +++--------
 3 files changed, 8 insertions(+), 13 deletions(-)

diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
index fd904c5b212b..7a97f362126f 100644
--- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
+++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
@@ -212,6 +212,7 @@ void FwLateralLongitudinalControl::Run()
 		_fw_lateral_ctrl_sub.copy(&lateral_sp);
 
 		if (lateral_sp.timestamp > 0 && hrt_elapsed_time(&lateral_sp.timestamp) < 1_s) {
+			publish = true;
 			float airspeed_reference_direction{NAN};
 			float lateral_accel_sp {NAN};
 
@@ -258,7 +259,6 @@ void FwLateralLongitudinalControl::Run()
 			};
 
 			_lateral_ctrl_status_pub.publish(status);
-			publish = true;
 		}
 
 
@@ -580,7 +580,7 @@ void FwLateralLongitudinalControl::updateAirspeed() {
 		_airspeed_valid = airspeed_valid;
 	}
 }
-bool FwLateralLongitudinalControl::checkLowHeightConditions()
+bool FwLateralLongitudinalControl::checkLowHeightConditions() const
 {
 	// Are conditions for low-height
 	return _param_fw_t_thr_low_hgt.get() >= 0.f && _local_pos.dist_bottom_valid
@@ -664,7 +664,7 @@ float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float later
 
 	return can_run_factor * (lateral_accel_sp);
 }
-float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) {
+float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const {
 	return  atanf(lateral_acceleration_sp * 1.0f / CONSTANTS_ONE_G);
 }
 
diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
index 8b73d1fae22b..b874a934e94d 100644
--- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
+++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
@@ -214,13 +214,13 @@ class FwLateralLongitudinalControl final : public ModuleBase<FwLateralLongitudin
 
 	void updateAltitudeAndHeightRate();
 
-	float mapLateralAccelerationToRollAngle(float lateral_acceleration_sp);
+	float mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const;
 
 	void updateWind();
 
 	void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt);
 
-	bool checkLowHeightConditions();
+	bool checkLowHeightConditions() const;
 
 	float getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const;
 
diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
index f4266737f685..f0be6aa7eb45 100644
--- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
@@ -743,10 +743,9 @@ void FixedwingPositionControl::control_idle()
 	_lateral_ctrl_limits_pub.publish(lateral_limits);
 
 	fw_longitudinal_control_setpoint_s long_contrl_sp {empty_longitudinal_control_setpoint};
-	long_contrl_sp.timestamp = hrt_absolute_time();
+	long_contrl_sp.timestamp = now;
 	long_contrl_sp.pitch_sp = math::radians(_param_fw_psp_off.get());
 	long_contrl_sp.thrust_sp = 0.0f;
-	long_contrl_sp.timestamp = now;
 	_longitudinal_ctrl_sp_pub.publish(long_contrl_sp);
 
 	longitudinal_control_limits_s longitudinal_control_limits{.timestamp = now};
@@ -899,17 +898,13 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
 	const float acc_rad = _directional_guidance.switchDistance(500.0f);
-	float throttle_min;
-	float throttle_max;
+	float throttle_min = _param_fw_thr_min.get();
+	float throttle_max = _param_fw_thr_max.get();
 
 	if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		throttle_min = 0.0;
 		throttle_max = 0.0;
-
-	} else {
-		throttle_min = _param_fw_thr_min.get();
-		throttle_max = _param_fw_thr_max.get();
 	}
 
 	// waypoint is a plain navigation waypoint