diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index 0ee46b3910..c0c836c0fa 100644 --- a/msg/position_setpoint.msg +++ b/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 diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp index 88fb6378a5..8ec4f83d92 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.cpp +++ b/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) || - (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 + && 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 */ diff --git a/src/lib/l1/ECL_L1_Pos_Controller.hpp b/src/lib/l1/ECL_L1_Pos_Controller.hpp index 8190e3e533..e6e7464859 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.hpp +++ b/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. diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index afb224388b..3f4da2df46 100644 --- a/src/lib/npfg/npfg.cpp +++ b/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); diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp index 2e75164dc5..4d4a5a2772 100644 --- a/src/lib/npfg/npfg.hpp +++ b/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); /* diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index bba68307c4..64cd3d2b42 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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; - int8_t loiter_direction = pos_sp.loiter_direction; + float loiter_radius = pos_sp.loiter_radius * (pos_sp.loiter_direction_counter_clockwise ? -1.f : 1.f); if (fabsf(loiter_radius) < FLT_EPSILON) { loiter_radius = _param_nav_loiter_rad.get(); - loiter_direction = signNoZero(loiter_radius); } - orbit_status.radius = static_cast(loiter_direction) * loiter_radius; + orbit_status.radius = loiter_radius; orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL orbit_status.x = static_cast(pos_sp.lat); orbit_status.y = static_cast(pos_sp.lon); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index aefc19fef7..5946e06a8f 100644 --- a/src/modules/navigator/mission.cpp +++ b/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) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index c93ab99517..2277a3b55e 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/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) { - bearing -= inner_angle; + if (curr_sp.loiter_direction_counter_clockwise) { + 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 diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 565ec28ac1..0396350c26 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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 = curr->current.loiter_direction; - - } else { - rep->current.loiter_direction = 1; - } + rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise; } 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()