more cleanup

Signed-off-by: RomanBapst <[email protected]>

src/lib/npfg/AirspeedReferenceController.cpp

@@ -31,7 +31,6 @@
  *
  ****************************************************************************/
 
-/*
 #include "AirspeedReferenceController.hpp"
 #include <matrix/math.hpp>
 #include <lib/mathlib/mathlib.h>

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -738,6 +738,10 @@ void FixedwingPositionControl::control_idle()
 	lateral_ctrl_sp.lateral_acceleration_setpoint = 0.0f;
 	_lateral_ctrl_sp_pub.publish(lateral_ctrl_sp);
 
+	lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()};
+	lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get()));
+	_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.pitch_sp = math::radians(_param_fw_psp_off.get());
@@ -789,15 +793,17 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 	longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()};
 	setDefaultLongControlLimits(longitudinal_control_limits);
 	longitudinal_control_limits.throttle_max = throttle_max;
-	longitudinal_control_limits.climb_rate_max = _param_climbrate_target.get();
-	longitudinal_control_limits.sink_rate_max = _param_sinkrate_target.get();
 	_longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits);
 
 	fw_lateral_control_setpoint_s lateral_ctrl_sp = empty_lateral_control_setpoint;
 	lateral_ctrl_sp.timestamp = hrt_absolute_time();
 	const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
 	lateral_ctrl_sp.lateral_acceleration_setpoint = rollAngleToLateralAccel(roll_body);
 	_lateral_ctrl_sp_pub.publish(lateral_ctrl_sp);
+
+	lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()};
+	lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get()));
+	_lateral_ctrl_limits_pub.publish(lateral_limits);
 }
 
 void
@@ -819,15 +825,21 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
 	_longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp);
 
 	longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()};
-	longitudinal_control_limits.pitch_min = radians(_param_fw_p_lim_min.get());
-	longitudinal_control_limits.pitch_max = radians(_param_fw_p_lim_max.get());
-	longitudinal_control_limits.throttle_min = _param_fw_thr_min.get();
+	setDefaultLongControlLimits(longitudinal_control_limits);
 	longitudinal_control_limits.throttle_max = (_landed
 			|| !_local_pos.v_z_valid) ? _param_fw_thr_min.get() : _param_fw_thr_max.get();
-	longitudinal_control_limits.climb_rate_max = _param_climbrate_target.get();
-	longitudinal_control_limits.sink_rate_max = _param_sinkrate_target.get();
 
 	_longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits);
+
+	fw_lateral_control_setpoint_s lateral_ctrl_sp = empty_lateral_control_setpoint;
+	lateral_ctrl_sp.timestamp = hrt_absolute_time();
+	const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
+	lateral_ctrl_sp.lateral_acceleration_setpoint = rollAngleToLateralAccel(roll_body);
+	_lateral_ctrl_sp_pub.publish(lateral_ctrl_sp);
+
+	lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()};
+	lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get()));
+	_lateral_ctrl_limits_pub.publish(lateral_limits);
 }
 
 uint8_t
@@ -972,12 +984,12 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 	lateral_ctrl_sp.timestamp = hrt_absolute_time();
 	lateral_ctrl_sp.course_setpoint = sp.course_setpoint;
 	lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward;
+	_lateral_ctrl_sp_pub.publish(lateral_ctrl_sp);
 
 	lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()};
 	lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get()));
 	_lateral_ctrl_limits_pub.publish(lateral_limits);
 
-	_lateral_ctrl_sp_pub.publish(lateral_ctrl_sp);
 }
 
 void
@@ -998,9 +1010,12 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 	fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
 	fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint;
 	fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward;
-
 	_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
 
+	lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()};
+	lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get()));
+	_lateral_ctrl_limits_pub.publish(lateral_limits);
+
 	const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = {
 		.timestamp = hrt_absolute_time(),
 		.height_rate_setpoint = pos_sp_curr.vz,
@@ -1164,6 +1179,11 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
 	fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward;
 
 	_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
+
+	lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()};
+	lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get()));
+	_lateral_ctrl_limits_pub.publish(lateral_limits);
+
 	target_airspeed = _figure_eight.getAirspeedSetpoint();
 	_closest_point_on_path = _figure_eight.getClosestPoint();
 
@@ -1229,9 +1249,12 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
 	fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
 	fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint;
 	fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward;
-
 	_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
 
+	lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()};
+	lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get()));
+	_lateral_ctrl_limits_pub.publish(lateral_limits);
+
 	const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = {
 		.timestamp = hrt_absolute_time(),
 		.height_rate_setpoint = NAN,