more fixes

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

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -1179,7 +1179,6 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
 	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();
 
 	const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = {
@@ -1362,6 +1361,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		fw_lateral_ctrl_sp.reset_integral = _runway_takeoff.resetIntegrators();
 		fw_lateral_ctrl_sp.heading_sp_runway_takeoff = _runway_takeoff.controlYaw() ? _runway_takeoff.getYaw(
 					sp.course_setpoint) : NAN;
+
+		// this overrides the roll setpoint, until the vehicle starts the climbout
 		fw_lateral_ctrl_sp.roll_sp = _runway_takeoff.getRoll();
 		_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
 
@@ -1991,8 +1992,11 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
 	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);
 }
 
 void
@@ -2093,14 +2097,15 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 		_yaw_lock_engaged = false;
 
 		const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
-		const DirectionalGuidanceOutput sp = {.lateral_acceleration_feedforward = rollAngleToLateralAccel(roll_body)};
 		fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
 		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;
-
+		fw_lateral_ctrl_sp.lateral_acceleration_setpoint = rollAngleToLateralAccel(roll_body);
 		_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);
 }
 
 float FixedwingPositionControl::rollAngleToLateralAccel(float roll_body) const

src/modules/fw_pos_control/figure_eight/FigureEight.cpp

@@ -51,8 +51,7 @@ static constexpr float MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO{2.0f};
 FigureEight::FigureEight(DirectionalGuidance &npfg, matrix::Vector2f &wind_vel, float &eas2tas) :
 	ModuleParams(nullptr),
 	_directional_guidance(npfg),
-	_wind_vel(wind_vel),
-	_eas2tas(eas2tas)
+	_wind_vel(wind_vel)
 {
 
 }
@@ -398,7 +397,6 @@ DirectionalGuidanceOutput FigureEight::applyCircle(bool loiter_direction_counter
 	float path_curvature = loiter_direction_multiplier / parameters.loiter_minor_radius;
 	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
 	_closest_point_on_path = unit_vec_center_to_closest_pt * parameters.loiter_minor_radius + circle_center;
-	_indicated_airspeed_setpoint = target_airspeed / _eas2tas;
 	return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent,
 			_closest_point_on_path, path_curvature);
 
@@ -424,7 +422,6 @@ DirectionalGuidanceOutput FigureEight::applyLine(const matrix::Vector2f &normali
 	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
 	const Vector2f vector_A_to_vehicle = curr_pos_local - line_segment_start_position;
 	_closest_point_on_path = line_segment_start_position + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
-	_indicated_airspeed_setpoint = target_airspeed / _eas2tas;
 	return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(),
 			line_segment_start_position,
 			0.0f);

src/modules/fw_pos_control/figure_eight/FigureEight.hpp

@@ -109,18 +109,6 @@ class FigureEight : public ModuleParams
 	 */
 	DirectionalGuidanceOutput updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
 			const FigureEightPatternParameters &parameters, float target_airspeed);
-	/**
-	 * @brief Get the roll setpoint
-	 *
-	 * @return the roll setpoint in [rad].
-	 */
-	float getRollSetpoint() const {return _roll_setpoint;};
-	/**
-	 * @brief Get the indicated airspeed setpoint
-	 *
-	 * @return the indicated airspeed setpoint in [m/s].
-	 */
-	float getAirspeedSetpoint() const {return _indicated_airspeed_setpoint;};
 	/**
 	 * @brief Get the target bearing of current point on figure of eight
 	 *
@@ -246,24 +234,9 @@ class FigureEight : public ModuleParams
 	 */
 	const matrix::Vector2f &_wind_vel;
 	/**
-	 * @brief Conversion factor from indicated to true airspeed.
-	 *
-	 */
-	const float &_eas2tas;
-	/**
-	 * @brief Roll setpoint in [rad].
-	 *
-	 */
-	float _roll_setpoint;
-	/**
-	 * @brief Indicated airspeed setpoint in [m/s].
-	 *
-	 */
-	float _indicated_airspeed_setpoint;
-	/**
-	 * @brief active figure eight position setpoint.
-	 *
-	 */
+	* @brief active figure eight position setpoint.
+	*
+	*/
 	FigureEightPatternParameters _active_parameters;
 
 	/**