diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 5cd3a76272..46a0c5e27a 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -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(); diff --git a/src/modules/fw_pos_control/figure_eight/FigureEight.cpp b/src/modules/fw_pos_control/figure_eight/FigureEight.cpp index 5bb339c980..89955b4311 100644 --- a/src/modules/fw_pos_control/figure_eight/FigureEight.cpp +++ b/src/modules/fw_pos_control/figure_eight/FigureEight.cpp @@ -37,8 +37,6 @@ #include "FigureEight.hpp" -#include - #include "lib/geo/geo.h" #include @@ -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 ¶meters) +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 ¶meters, 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 + ¶meters) +{ + 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 ¶meters, 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 ¶meters, 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 ¶meters) { diff --git a/src/modules/fw_pos_control/figure_eight/FigureEight.hpp b/src/modules/fw_pos_control/figure_eight/FigureEight.hpp index 95f75dcb8b..53b296ffe7 100644 --- a/src/modules/fw_pos_control/figure_eight/FigureEight.hpp +++ b/src/modules/fw_pos_control/figure_eight/FigureEight.hpp @@ -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 ¶meters); + /** * @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 ¶meters); + + /** + * @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 ¶meters, FigureEightPatternPoints pattern_points); + /** * @brief Calculate figure eight pattern points *