position_setpoint: replaced loiter_direction integer by boolean (#20317)

* position_setpoint: replaced loiter_direction integer by boolean (loiter_direction_counter_clockwise)

Signed-off-by: RomanBapst <bapstroman@gmail.com>

msg/position_setpoint.msg

@@ -32,7 +32,7 @@ float32 yawspeed		# yawspeed (only for multirotors, in rad/s)
 bool yawspeed_valid		# true if yawspeed setpoint valid
 
 float32 loiter_radius		# loiter radius (only for fixed wing), in m
-int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
+bool loiter_direction_counter_clockwise # loiter direction is clockwise by default and can be changed using this field
 
 float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
 

src/lib/l1/ECL_L1_Pos_Controller.cpp

@@ -213,10 +213,12 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
 
 void
 ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
-				       int8_t loiter_direction, const Vector2f &ground_speed_vector)
+				       const bool loiter_direction_counter_clockwise, const Vector2f &ground_speed_vector)
 {
 	_has_guidance_updated = true;
 
+	const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
+
 	/* the complete guidance logic in this section was proposed by [2] */
 
 	/* calculate the gains for the PD loop (circle tracking) */
@@ -274,7 +276,7 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
 	float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
 
 	/* calculate velocity on circle / along tangent */
-	float tangent_vel = xtrack_vel_center * loiter_direction;
+	float tangent_vel = xtrack_vel_center * loiter_direction_multiplier;
 
 	/* prevent PD output from turning the wrong way when in circle mode */
 	const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction
@@ -288,7 +290,8 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
 			(radius + xtrack_err_circle));
 
 	/* add PD control on circle and centripetal acceleration for total circle command */
-	float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);
+	float lateral_accel_sp_circle = loiter_direction_multiplier * (lateral_accel_sp_circle_pd +
+					lateral_accel_sp_circle_centripetal);
 
 	/*
 	 * Switch between circle (loiter) and capture (towards waypoint center) mode when
@@ -296,8 +299,10 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
 	 */
 
 	// XXX check switch over
-	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
+	if ((lateral_accel_sp_center < lateral_accel_sp_circle && !loiter_direction_counter_clockwise
-	    (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
+	     && xtrack_err_circle > 0.0f)
+	    ||
+	    (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction_counter_clockwise && xtrack_err_circle > 0.0f)) {
 		_lateral_accel = lateral_accel_sp_center;
 		_circle_mode = false;
 		/* angle between requested and current velocity vector */

src/lib/l1/ECL_L1_Pos_Controller.hpp

@@ -154,7 +154,7 @@ public:
 	 * @return sets _lateral_accel setpoint
 	 */
 	void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
-			     int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector);
+			     const bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_speed_vector);
 
 	/**
 	 * Navigate on a fixed bearing.

src/lib/npfg/npfg.cpp

@@ -574,12 +574,14 @@ void NPFG::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoin
 } // navigateWaypoints
 
 void NPFG::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos,
-			  float radius, int8_t loiter_direction, const Vector2f &ground_vel, const Vector2f &wind_vel)
+			  float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel)
 {
 	path_type_loiter_ = true;
 
 	radius = math::max(radius, MIN_RADIUS);
 
+	const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
+
 	Vector2f vector_center_to_vehicle = vehicle_pos - loiter_center;
 	const float dist_to_center = vector_center_to_vehicle.norm();
 
@@ -605,12 +607,12 @@ void NPFG::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle
 	}
 
 	// 90 deg clockwise rotation * loiter direction
-	unit_path_tangent_ = float(loiter_direction) * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
+	unit_path_tangent_ = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
 
 	// positive in direction of path normal
-	signed_track_error_ = -loiter_direction * (dist_to_center - radius);
+	signed_track_error_ = -loiter_direction_multiplier * (dist_to_center - radius);
 
-	float path_curvature = float(loiter_direction) / radius;
+	float path_curvature = loiter_direction_multiplier / radius;
 
 	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, path_curvature);
 

src/lib/npfg/npfg.hpp

@@ -240,12 +240,12 @@ public:
 	 * @param[in] loiter_center The position of the center of the loiter circle [m]
 	 * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
 	 * @param[in] radius Loiter radius [m]
-	 * @param[in] loiter_direction Loiter direction: -1=counter-clockwise, 1=clockwise
+	 * @param[in] loiter_direction_counter_clockwise Specifies loiter direction
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
 	void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
-			    float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
+			    float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
 			    const matrix::Vector2f &wind_vel);
 
 	/*

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1303,11 +1303,9 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 	/* waypoint is a loiter waypoint */
 	float loiter_radius = pos_sp_curr.loiter_radius;
-	uint8_t loiter_direction = pos_sp_curr.loiter_direction;
 
 	if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) {
 		loiter_radius = _param_nav_loiter_rad.get();
-		loiter_direction = signNoZero(loiter_radius);
 	}
 
 	const bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
@@ -1336,13 +1334,15 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-		_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed),
+		_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
+				     get_nav_speed_2d(ground_speed),
 				     _wind_vel);
 		_att_sp.roll_body = _npfg.getRollSetpoint();
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 	} else {
-		_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction,
+		_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius,
+					    pos_sp_curr.loiter_direction_counter_clockwise,
 					    get_nav_speed_2d(ground_speed));
 		_att_sp.roll_body = _l1_control.get_roll_setpoint();
 	}
@@ -2739,15 +2739,13 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_
 {
 	orbit_status_s orbit_status{};
 	orbit_status.timestamp = hrt_absolute_time();
-	float loiter_radius = pos_sp.loiter_radius;
+	float loiter_radius = pos_sp.loiter_radius * (pos_sp.loiter_direction_counter_clockwise ? -1.f : 1.f);
-	int8_t loiter_direction = pos_sp.loiter_direction;
 
 	if (fabsf(loiter_radius) < FLT_EPSILON) {
 		loiter_radius = _param_nav_loiter_rad.get();
-		loiter_direction = signNoZero(loiter_radius);
 	}
 
-	orbit_status.radius = static_cast<float>(loiter_direction) * loiter_radius;
+	orbit_status.radius = loiter_radius;
 	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
 	orbit_status.x = static_cast<double>(pos_sp.lat);
 	orbit_status.y = static_cast<double>(pos_sp.lon);

src/modules/navigator/mission.cpp

@@ -1893,7 +1893,7 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
 		(fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) &&
 		(p1->yawspeed_valid == p2->yawspeed_valid) &&
 		(fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) &&
-		(p1->loiter_direction == p2->loiter_direction) &&
+		(p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) &&
 		(fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) &&
 		(fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) &&
 		((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle)

src/modules/navigator/mission_block.cpp

@@ -519,11 +519,11 @@ MissionBlock::is_mission_item_reached_or_completed()
 				float inner_angle = acosf(ratio);
 
 				// Compute "ideal" tangent origin
-				if (curr_sp.loiter_direction > 0) {
+				if (curr_sp.loiter_direction_counter_clockwise) {
-					bearing -= inner_angle;
+					bearing += inner_angle;
 
 				} else {
-					bearing += inner_angle;
+					bearing -= inner_angle;
 				}
 
 				// set typ to position, will get set to loiter in the fw position controller once close
@@ -687,7 +687,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	sp->yaw_valid = PX4_ISFINITE(item.yaw);
 	sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
 			    _navigator->get_loiter_radius();
-	sp->loiter_direction = math::signNoZero(item.loiter_radius);
+	sp->loiter_direction_counter_clockwise = item.loiter_radius < 0;
 
 	if (item.acceptance_radius > 0.001f && PX4_ISFINITE(item.acceptance_radius)) {
 		// if the mission item has a specified acceptance radius, overwrite the default one from parameters

src/modules/navigator/navigator_main.cpp

@@ -342,12 +342,7 @@ void Navigator::run()
 							rep->current.loiter_radius = get_loiter_radius();
 						}
 
-						if (curr->current.loiter_direction == 1 || curr->current.loiter_direction == -1) {
+						rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise;
-							rep->current.loiter_direction = curr->current.loiter_direction;
-
-						} else {
-							rep->current.loiter_direction = 1;
-						}
 					}
 
 					rep->previous.timestamp = hrt_absolute_time();
@@ -385,12 +380,12 @@ void Navigator::run()
 					position_setpoint_triplet_s *rep = get_reposition_triplet();
 					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 					rep->current.loiter_radius = get_loiter_radius();
-					rep->current.loiter_direction = 1;
+					rep->current.loiter_direction_counter_clockwise = false;
 					rep->current.cruising_throttle = get_cruising_throttle();
 
 					if (PX4_ISFINITE(cmd.param1)) {
 						rep->current.loiter_radius = fabsf(cmd.param1);
-						rep->current.loiter_direction = math::signNoZero(cmd.param1);
+						rep->current.loiter_direction_counter_clockwise = cmd.param1 < 0;
 					}
 
 					rep->current.lat = position_setpoint.lat;
@@ -414,7 +409,7 @@ void Navigator::run()
 				rep->previous.alt = get_global_position()->alt;
 
 				rep->current.loiter_radius = get_loiter_radius();
-				rep->current.loiter_direction = 1;
+				rep->current.loiter_direction_counter_clockwise = false;
 				rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
 
 				if (home_global_position_valid()) {
@@ -918,7 +913,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 					rep->current.loiter_radius = get_loiter_radius();
 					rep->current.alt_valid = true;
 					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
-					rep->current.loiter_direction = 1;
 					rep->current.cruising_throttle = get_cruising_throttle();
 					rep->current.acceptance_radius = get_acceptance_radius();
 					rep->current.cruising_speed = get_cruising_speed();
@@ -1077,6 +1071,7 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp)
 	sp.valid = false;
 	sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
 	sp.disable_weather_vane = false;
+	sp.loiter_direction_counter_clockwise = false;
 }
 
 float Navigator::get_cruising_throttle()