dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

figureEight: Rename relative position variable

This commit is contained in:
Konrad 2023-11-23 10:01:17 +01:00 committed by KonradRudin
parent 1e06ed2ed5
commit 8a68a66203
2 changed files with 36 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
src/modules/fw_pos_control/figure_eight/FigureEight.cpp
View File

@ -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};
@ -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;
@ -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
@ -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
@ -191,8 +190,8 @@ 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;
@ -200,19 +199,19 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi
// 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;
}
}
@ -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;
}
@ -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;
}
}
@ -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

4
src/modules/fw_pos_control/figure_eight/FigureEight.hpp
View File

@ -190,11 +190,11 @@ private:
/**
* @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.