fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 16:17:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FigureEight: Refactor initialization

Browse Source
This commit is contained in:
Konrad
2023-11-17 10:32:43 +01:00
committed by KonradRudin
parent ef20708a98
commit a73409c015
3 changed files with 85 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
-2
View File
@@ -1262,8 +1262,6 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
params.loiter_orientation = pos_sp_curr.loiter_orientation;
params.loiter_radius = pos_sp_curr.loiter_radius;
_figure_eight.initializePattern(curr_pos_local, ground_speed, params);
// Apply control
_figure_eight.updateSetpoint(curr_pos_local, ground_speed, params, target_airspeed);
_att_sp.roll_body = _figure_eight.getRollSetpoint();
src/modules/fw_pos_control/figure_eight/FigureEight.cpp
+51 -51
View File
@@ -37,8 +37,6 @@
#include "FigureEight.hpp"
#include <cmath>
#include "lib/geo/geo.h"
#include <lib/matrix/matrix/math.hpp>
@@ -60,24 +58,62 @@ FigureEight::FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas)
}
void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
const FigureEightPatternParameters &parameters)
void FigureEight::resetPattern()
{
// Initialize the currently active segment, if it hasn't been active yet, or the sp has been changed.
if ((_current_segment == FigureEightSegment::SEGMENT_UNDEFINED) ||
((fabsf(_active_parameters.center_pos_local(0) - parameters.center_pos_local(0)) > FLT_EPSILON) ||
(fabsf(_active_parameters.center_pos_local(1) - parameters.center_pos_local(1)) > FLT_EPSILON) ||
(fabsf(_active_parameters.loiter_radius - parameters.loiter_radius) > FLT_EPSILON) ||
(fabsf(_active_parameters.loiter_minor_radius - parameters.loiter_minor_radius) > FLT_EPSILON) ||
(fabsf(_active_parameters.loiter_orientation - parameters.loiter_orientation) > FLT_EPSILON) ||
(_active_parameters.loiter_direction_counter_clockwise != parameters.loiter_direction_counter_clockwise))) {
// Set the current segment invalid
_current_segment = FigureEightSegment::SEGMENT_UNDEFINED;
_pos_passed_circle_center_along_major_axis = false;
}
void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
const FigureEightPatternParameters &parameters, float target_airspeed)
{
// Sanitize inputs
FigureEightPatternParameters valid_parameters{sanitizeParameters(parameters)};
// Calculate the figure eight pattern points.
FigureEightPatternPoints pattern_points;
calculateFigureEightPoints(pattern_points, valid_parameters);
// Do the figure of eight initialization if needed.
initializePattern(curr_pos_local, ground_speed, valid_parameters, pattern_points);
// Check if we need to switch to next segment
updateSegment(curr_pos_local, valid_parameters, pattern_points);
// Apply control logic based on segment
applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, pattern_points);
}
FigureEight::FigureEightPatternParameters FigureEight::sanitizeParameters(const FigureEightPatternParameters
&parameters)
{
FigureEightPatternParameters valid_parameters{parameters};
if (!PX4_ISFINITE(parameters.loiter_minor_radius)) {
valid_parameters.loiter_minor_radius = fabsf(_param_nav_loiter_rad.get());
}
if (!PX4_ISFINITE(parameters.loiter_radius)) {
valid_parameters.loiter_radius = DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO * valid_parameters.loiter_minor_radius;
valid_parameters.loiter_direction_counter_clockwise = _param_nav_loiter_rad.get() < 0;
}
valid_parameters.loiter_radius = math::max(valid_parameters.loiter_radius,
MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO * valid_parameters.loiter_minor_radius);
return valid_parameters;
}
void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
const FigureEightPatternParameters &parameters, FigureEightPatternPoints pattern_points)
{
// 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 ground_speed_rotated = Dcm2f(-calculateRotationAngle(parameters)) * ground_speed;
FigureEightPatternPoints pattern_points;
calculateFigureEightPoints(pattern_points, parameters);
if (rel_pos_to_center(0) > NORMALIZED_MAJOR_RADIUS) { // Far away north
_current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH;
@@ -106,42 +142,6 @@ void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, cons
}
}
void FigureEight::resetPattern()
{
// Set the current segment invalid
_current_segment = FigureEightSegment::SEGMENT_UNDEFINED;
_pos_passed_circle_center_along_major_axis = false;
}
void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
const FigureEightPatternParameters &parameters, float target_airspeed)
{
// Sanitize inputs
FigureEightPatternParameters valid_parameters{parameters};
if (!PX4_ISFINITE(parameters.loiter_minor_radius)) {
valid_parameters.loiter_minor_radius = fabsf(_param_nav_loiter_rad.get());
}
if (!PX4_ISFINITE(parameters.loiter_radius)) {
valid_parameters.loiter_radius = DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO * valid_parameters.loiter_minor_radius;
valid_parameters.loiter_direction_counter_clockwise = _param_nav_loiter_rad.get() < 0;
}
valid_parameters.loiter_radius = math::max(valid_parameters.loiter_radius,
MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO * valid_parameters.loiter_minor_radius);
// Calculate the figure eight pattern points.
FigureEightPatternPoints pattern_points;
calculateFigureEightPoints(pattern_points, valid_parameters);
// Check if we need to switch to next segment
updateSegment(curr_pos_local, valid_parameters, pattern_points);
// Apply control logic based on segment
applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, pattern_points);
}
void FigureEight::calculateFigureEightPoints(FigureEightPatternPoints &pattern_points,
const FigureEightPatternParameters &parameters)
{
src/modules/fw_pos_control/figure_eight/FigureEight.hpp
+34 -11
View File
@@ -70,7 +70,18 @@ public:
float loiter_minor_radius;
float loiter_orientation;
bool loiter_direction_counter_clockwise;
bool operator!=(const FigureEightPatternParameters &other) const
{
return ((fabsf(center_pos_local(0) - other.center_pos_local(0)) > FLT_EPSILON) ||
(fabsf(center_pos_local(1) - other.center_pos_local(1)) > FLT_EPSILON) ||
(fabsf(loiter_radius - other.loiter_radius) > FLT_EPSILON) ||
(fabsf(loiter_minor_radius - other.loiter_minor_radius) > FLT_EPSILON) ||
(fabsf(loiter_orientation - other.loiter_orientation) > FLT_EPSILON) ||
(loiter_direction_counter_clockwise != other.loiter_direction_counter_clockwise));
};
};
/**
* @brief Construct a new Figure Eight object
*
@@ -79,17 +90,7 @@ public:
* @param[in] eas2tas is the reference to the parent indicated airspeed to true airspeed conversion.
*/
FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas);
/**
* @brief Initialize the figure eight pattern.
*
* Initialize the figure eight pattern by determining the current active segment.
*
* @param[in] curr_pos_local is the current local position of the vehicle in [m].
* @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
* @param[in] parameters is the parameter set defining the figure eight shape.
*/
void initializePattern(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
const FigureEightPatternParameters &parameters);
/**
* @brief reset the figure eight pattern.
*
@@ -97,6 +98,7 @@ public:
*
*/
void resetPattern();
/**
* @brief Update roll and airspeed setpoint.
*
@@ -134,6 +136,27 @@ public:
private:
/**
* @brief
*
* @param[in] parameters are gotten the figure of eight parameters
* @return are the sanitized figure of eight parameters
*/
FigureEightPatternParameters sanitizeParameters(const FigureEightPatternParameters &parameters);
/**
* @brief Initialize the figure eight pattern.
*
* Initialize the figure eight pattern by determining the current active segment.
*
* @param[in] curr_pos_local is the current local position of the vehicle in [m].
* @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
* @param[in] parameters is the parameter set defining the figure eight shape.
* @param[in] pattern_points are the figure of eight pattern points.
*/
void initializePattern(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
const FigureEightPatternParameters &parameters, FigureEightPatternPoints pattern_points);
/**
* @brief Calculate figure eight pattern points
*