more cleanup

Signed-off-by: RomanBapst <[email protected]>
PX4 · Jan 8, 2025 · 777a264 · 777a264
1 parent eb68cd0
commit 777a264
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 13 deletions.
diff --git 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
@@ -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
@@ -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