Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

position_setpoint: replaced loiter_direction integer by boolean (#20317)

Browse Source 
* position_setpoint: replaced loiter_direction integer by boolean (loiter_direction_counter_clockwise)

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst
2022-09-29 11:06:10 +02:00
committed by GitHub
parent 5d39fdba6d
commit 20457c5e2e
9 changed files with 36 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/position_setpoint.msg
+1 -1
View File
@@ -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
+10 -5
View File
@@ -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 */
src/lib/l1/ECL_L1_Pos_Controller.hpp
+1 -1
View File
@@ -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
+6 -4
View File
@@ -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
+2 -2
View File
@@ -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
+6 -8
View File
@@ -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<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
+1 -1
View File
@@ -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
+4 -4
View File
@@ -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
src/modules/navigator/navigator_main.cpp
+5 -10
View File
@@ -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()