Skip to content

Commit

Permalink
figureEight: Rename relative position variable
Browse files Browse the repository at this point in the history
  • Loading branch information
@KonradRudin
KonradRudin committed Nov 28, 2023
1 parent 1e06ed2 commit 8a68a66
Show file tree
Hide file tree
Showing 2 changed files with 36 additions and 35 deletions.
67 changes: 34 additions & 33 deletions src/modules/fw_pos_control/figure_eight/FigureEight.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,6 @@ using namespace matrix;
static constexpr float NORMALIZED_MAJOR_RADIUS{1.0f};
static constexpr bool NORTH_CIRCLE_IS_COUNTER_CLOCKWISE{false};
static constexpr bool SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE{true};
static constexpr float MINIMUM_MINOR_TO_MAJOR_AXIS_SCALE{2.0f};
static constexpr float DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO{2.5f};
static constexpr float MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO{2.0f};

Expand Down Expand Up @@ -110,20 +109,20 @@ void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, cons
{
// Initialize the currently active segment, if it hasn't been active yet, or the pattern has been changed.
if ((_current_segment == FigureEightSegment::SEGMENT_UNDEFINED) || (_active_parameters != parameters)) {
Vector2f rel_pos_to_center;
calculatePositionToCenterNormalizedRotated(rel_pos_to_center, curr_pos_local, parameters);
Vector2f center_to_pos_local;
calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);
Vector2f ground_speed_rotated = Dcm2f(-calculateRotationAngle(parameters)) * ground_speed;

Vector2f rel_pos_to_north_circle{rel_pos_to_center - pattern_points.normalized_north_circle_offset};
Vector2f rel_pos_to_south_circle{rel_pos_to_center - pattern_points.normalized_south_circle_offset};
const bool north_is_closer = rel_pos_to_north_circle.norm() < rel_pos_to_south_circle.norm();
Vector2f north_center_to_pos_local{center_to_pos_local - pattern_points.normalized_north_circle_offset};
Vector2f south_center_to_pos_local{center_to_pos_local - pattern_points.normalized_south_circle_offset};
const bool north_is_closer = north_center_to_pos_local.norm() < south_center_to_pos_local.norm();

// Get the normalized switch distance.
float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;

//Far away from current figure of eight. Fly towards closer circle

if (rel_pos_to_center.norm() > NORMALIZED_MAJOR_RADIUS + switch_distance_normalized) {
if (center_to_pos_local.norm() > NORMALIZED_MAJOR_RADIUS + switch_distance_normalized) {
if (north_is_closer) {
_current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH;

Expand All @@ -135,7 +134,7 @@ void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, cons

} else {
if (north_is_closer) {
const bool is_circling_counter_clockwise{rel_pos_to_north_circle.cross(ground_speed_rotated) < 0.f};
const bool is_circling_counter_clockwise{north_center_to_pos_local.cross(ground_speed_rotated) < 0.f};

if ((ground_speed_rotated(0) > 0.f) && (is_circling_counter_clockwise == NORTH_CIRCLE_IS_COUNTER_CLOCKWISE)) {
// Flying north and right rotation
Expand All @@ -149,7 +148,7 @@ void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, cons
}

} else {
const bool is_circling_counter_clockwise{rel_pos_to_south_circle.cross(ground_speed_rotated) < 0.f};
const bool is_circling_counter_clockwise{south_center_to_pos_local.cross(ground_speed_rotated) < 0.f};

if ((ground_speed_rotated(0) < 0.f) && (is_circling_counter_clockwise == SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE)) {
// Flying south and right rotation
Expand Down Expand Up @@ -191,28 +190,28 @@ void FigureEight::calculateFigureEightPoints(FigureEightPatternPoints &pattern_p
void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters &parameters,
const FigureEightPatternPoints &pattern_points)
{
Vector2f rel_pos_to_center;
calculatePositionToCenterNormalizedRotated(rel_pos_to_center, curr_pos_local, parameters);
Vector2f center_to_pos_local;
calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);

// Get the normalized switch distance.
float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;

// Update segment if segment exit condition has been reached
switch (_current_segment) {
case FigureEightSegment::SEGMENT_CIRCLE_NORTH: {
if (rel_pos_to_center(0) > pattern_points.normalized_north_circle_offset(0)) {
if (center_to_pos_local(0) > pattern_points.normalized_north_circle_offset(0)) {
_pos_passed_circle_center_along_major_axis = true;
}

Vector2f vector_to_exit_normalized = pattern_points.normalized_north_exit_offset - rel_pos_to_center;
Vector2f vector_to_exit_normalized = pattern_points.normalized_north_exit_offset - center_to_pos_local;

/* Exit condition: Switch distance away from north-east point of north circle and at least once was above the circle center. Failsafe action, if poor tracking,
- switch to next if the vehicle is on the east side and below the north exit point. */
if (_pos_passed_circle_center_along_major_axis &&
((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
((rel_pos_to_center(0) < pattern_points.normalized_north_exit_offset(0)) &&
(rel_pos_to_center(1) > FLT_EPSILON) &&
(rel_pos_to_center.norm() < NORMALIZED_MAJOR_RADIUS)))) {
((center_to_pos_local(0) < pattern_points.normalized_north_exit_offset(0)) &&
(center_to_pos_local(1) > FLT_EPSILON) &&
(center_to_pos_local.norm() < NORMALIZED_MAJOR_RADIUS)))) {
_current_segment = FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST;
}
}
Expand All @@ -221,32 +220,33 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi
case FigureEightSegment::SEGMENT_POINT_SOUTHWEST: // fall through
case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: {
_pos_passed_circle_center_along_major_axis = false;
Vector2f vector_to_exit_normalized = pattern_points.normalized_south_entry_offset - rel_pos_to_center;
Vector2f vector_to_exit_normalized = pattern_points.normalized_south_entry_offset - center_to_pos_local;

/* Exit condition: Switch distance away from south-west point of south circle. Failsafe action, if poor tracking,
switch to next if the vehicle is on the west side and below entry point of the south circle or has left the radius. */
if ((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
((rel_pos_to_center(0) < pattern_points.normalized_south_entry_offset(0)) && (rel_pos_to_center(1) < FLT_EPSILON)) ||
(rel_pos_to_center(0) < -NORMALIZED_MAJOR_RADIUS)) {
((center_to_pos_local(0) < pattern_points.normalized_south_entry_offset(0)) && (center_to_pos_local(1) < FLT_EPSILON))
||
(center_to_pos_local(0) < -NORMALIZED_MAJOR_RADIUS)) {
_current_segment = FigureEightSegment::SEGMENT_CIRCLE_SOUTH;
}
}
break;

case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: {
if (rel_pos_to_center(0) < pattern_points.normalized_south_circle_offset(0)) {
if (center_to_pos_local(0) < pattern_points.normalized_south_circle_offset(0)) {
_pos_passed_circle_center_along_major_axis = true;
}

Vector2f vector_to_exit_normalized = pattern_points.normalized_south_exit_offset - rel_pos_to_center;
Vector2f vector_to_exit_normalized = pattern_points.normalized_south_exit_offset - center_to_pos_local;

/* Exit condition: Switch distance away from south-east point of south circle and at least once was below the circle center. Failsafe action, if poor tracking,
- switch to next if the vehicle is on the east side and above the south exit point. */
if (_pos_passed_circle_center_along_major_axis &&
((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
((rel_pos_to_center(0) > pattern_points.normalized_south_exit_offset(0)) &&
(rel_pos_to_center(1) > FLT_EPSILON) &&
(rel_pos_to_center.norm() < NORMALIZED_MAJOR_RADIUS)))) {
((center_to_pos_local(0) > pattern_points.normalized_south_exit_offset(0)) &&
(center_to_pos_local(1) > FLT_EPSILON) &&
(center_to_pos_local.norm() < NORMALIZED_MAJOR_RADIUS)))) {
_current_segment = FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST;
}

Expand All @@ -256,13 +256,14 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi
case FigureEightSegment::SEGMENT_POINT_NORTHWEST: // Fall through
case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: {
_pos_passed_circle_center_along_major_axis = false;
Vector2f vector_to_exit_normalized = pattern_points.normalized_north_entry_offset - rel_pos_to_center;
Vector2f vector_to_exit_normalized = pattern_points.normalized_north_entry_offset - center_to_pos_local;

/* Exit condition: Switch distance away from north-west point of north circle. Failsafe action, if poor tracking,
switch to next if the vehicle is on the west side and above entry point of the north circle or has left the radius. */
if ((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
((rel_pos_to_center(0) > pattern_points.normalized_north_entry_offset(0)) && (rel_pos_to_center(1) < FLT_EPSILON)) ||
(rel_pos_to_center(0) > NORMALIZED_MAJOR_RADIUS)) {
((center_to_pos_local(0) > pattern_points.normalized_north_entry_offset(0)) && (center_to_pos_local(1) < FLT_EPSILON))
||
(center_to_pos_local(0) > NORMALIZED_MAJOR_RADIUS)) {
_current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH;
}
}
Expand Down Expand Up @@ -328,18 +329,18 @@ void FigureEight::applyControl(const matrix::Vector2f &curr_pos_local, const mat
}
}

void FigureEight::calculatePositionToCenterNormalizedRotated(matrix::Vector2f &pos_to_center_normalized_rotated,
void FigureEight::calculatePositionToCenterNormalizedRotated(matrix::Vector2f &center_to_pos_local_normalized_rotated,
const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters &parameters) const
{
Vector2f pos_to_center = curr_pos_local - parameters.center_pos_local;
Vector2f center_to_pos_local = curr_pos_local - parameters.center_pos_local;

// normalize position with respect to radius
Vector2f pos_to_center_normalized;
pos_to_center_normalized(0) = pos_to_center(0) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
pos_to_center_normalized(1) = pos_to_center(1) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
Vector2f center_to_pos_local_normalized;
center_to_pos_local_normalized(0) = center_to_pos_local(0) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
center_to_pos_local_normalized(1) = center_to_pos_local(1) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;

// rotate position with respect to figure eight orientation and direction.
pos_to_center_normalized_rotated = Dcm2f(-calculateRotationAngle(parameters)) * pos_to_center_normalized;
center_to_pos_local_normalized_rotated = Dcm2f(-calculateRotationAngle(parameters)) * center_to_pos_local_normalized;
}

float FigureEight::calculateRotationAngle(const FigureEightPatternParameters &parameters) const
Expand Down
4 changes: 2 additions & 2 deletions src/modules/fw_pos_control/figure_eight/FigureEight.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,11 +190,11 @@ class FigureEight : public ModuleParams
/**
* @brief calculate normalized and rotated relative vehicle position to pattern center.
*
* @param[out] pos_to_center_normalized_rotated is the calculated normalized and rotated relative vehicle position to pattern center.
* @param[out] center_to_pos_local_normalized_rotated is the calculated normalized and rotated relative vehicle position with respect to the pattern center.
* @param[in] curr_pos_local is the current local position of the vehicle in [m].
* @param[in] parameters is the parameter set defining the figure eight shape.
*/
void calculatePositionToCenterNormalizedRotated(matrix::Vector2f &pos_to_center_normalized_rotated,
void calculatePositionToCenterNormalizedRotated(matrix::Vector2f &center_to_pos_local_normalized_rotated,
const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters &parameters) const;
/**
* @brief Calculate rotation angle.
Expand Down

0 comments on commit 8a68a66

Please sign in to comment.