diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index b1d787c721..a98c4adb61 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -85,6 +85,7 @@ set(msg_files EstimatorStatus.msg EstimatorStatusFlags.msg Event.msg + FigureEightStatus.msg FailsafeFlags.msg FailureDetectorStatus.msg FollowTarget.msg diff --git a/msg/FigureEightStatus.msg b/msg/FigureEightStatus.msg new file mode 100644 index 0000000000..e14d8f0d8a --- /dev/null +++ b/msg/FigureEightStatus.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +float32 major_radius # Major axis radius of the figure eight [m]. Positive values orbit clockwise, negative values orbit counter-clockwise. +float32 minor_radius # Minor axis radius of the figure eight [m]. +float32 orientation # Orientation of the major axis of the figure eight [rad]. +uint8 frame # The coordinate system of the fields: x, y, z. +int32 x # X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. +int32 y # Y coordinate of center point. Coordinate system depends on frame field: local = y position in meters * 1e4, global = latitude in degrees * 1e7. +float32 z # Altitude of center point. Coordinate system depends on frame field. diff --git a/msg/PositionSetpoint.msg b/msg/PositionSetpoint.msg index 2323a67b65..268e4b937d 100644 --- a/msg/PositionSetpoint.msg +++ b/msg/PositionSetpoint.msg @@ -9,6 +9,9 @@ uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) +uint8 LOITER_TYPE_ORBIT=0 # Circular pattern +uint8 LOITER_TYPE_FIGUREEIGHT=1 # Pattern resembling an 8 + bool valid # true if setpoint is valid uint8 type # setpoint type to adjust behavior of position controller @@ -25,8 +28,11 @@ bool yaw_valid # true if yaw setpoint valid 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 +float32 loiter_radius # loiter major axis radius in m +float32 loiter_minor_radius # loiter minor axis radius (used for non-circular loiter shapes) in m bool loiter_direction_counter_clockwise # loiter direction is clockwise by default and can be changed using this field +float32 loiter_orientation # Orientation of the major axis with respect to true north in rad [-pi,pi) +uint8 loiter_pattern # loitern pattern to follow float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 19e2761cdb..9ded909545 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -15,6 +15,7 @@ uint16 VEHICLE_CMD_NAV_LAND = 21 # Land at location |Empty| Empty| Empty| Desi uint16 VEHICLE_CMD_NAV_TAKEOFF = 22 # Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| uint16 VEHICLE_CMD_NAV_PRECLAND = 23 # Attempt a precision landing uint16 VEHICLE_CMD_DO_ORBIT = 34 # Start orbiting on the circumference of a circle defined by the parameters. |Radius [m] |Velocity [m/s] |Yaw behaviour |Empty |Latitude/X |Longitude/Y |Altitude/Z | +uint16 VEHICLE_CMD_DO_FIGUREEIGHT = 35 # Start flying on the outline of a figure eight defined by the parameters. |Major Radius [m] |Minor Radius [m] |Velocity [m/s] |Orientation |Latitude/X |Longitude/Y |Altitude/Z | uint16 VEHICLE_CMD_NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| uint16 VEHICLE_CMD_NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| uint16 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84 # Takeoff from ground / hand and transition to fixed wing |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1397156c91..2b69bbd3b8 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1079,40 +1079,70 @@ Commander::handle_command(const vehicle_command_s &cmd) } break; - case vehicle_command_s::VEHICLE_CMD_DO_ORBIT: + case vehicle_command_s::VEHICLE_CMD_DO_ORBIT: { - transition_result_t main_ret; + transition_result_t main_ret; - if (_vehicle_status.in_transition_mode) { - main_ret = TRANSITION_DENIED; + if (_vehicle_status.in_transition_mode) { + main_ret = TRANSITION_DENIED; - } else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - // for fixed wings the behavior of orbit is the same as loiter - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { - main_ret = TRANSITION_CHANGED; + } else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + // for fixed wings the behavior of orbit is the same as loiter + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { + main_ret = TRANSITION_CHANGED; + + } else { + main_ret = TRANSITION_DENIED; + } } else { - main_ret = TRANSITION_DENIED; + // Switch to orbit state and let the orbit task handle the command further + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT)) { + main_ret = TRANSITION_CHANGED; + + } else { + main_ret = TRANSITION_DENIED; + } } - } else { - // Switch to orbit state and let the orbit task handle the command further - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT)) { - main_ret = TRANSITION_CHANGED; + if (main_ret != TRANSITION_DENIED) { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - main_ret = TRANSITION_DENIED; + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + mavlink_log_critical(&_mavlink_log_pub, "Orbit command rejected"); } } + break; - if (main_ret != TRANSITION_DENIED) { - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + case vehicle_command_s::VEHICLE_CMD_DO_FIGUREEIGHT: { - } else { - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - mavlink_log_critical(&_mavlink_log_pub, "Orbit command rejected"); + if (!((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || (_vehicle_status.is_vtol))) { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + mavlink_log_critical(&_mavlink_log_pub, "Figure 8 command only available for fixed wing and vtol vehicles."); + break; + } + + transition_result_t main_ret = TRANSITION_DENIED; + + if ((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + (!_vehicle_status.in_transition_mode)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { + main_ret = TRANSITION_CHANGED; + + } else { + main_ret = TRANSITION_DENIED; + } + } + + if (main_ret != TRANSITION_DENIED) { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + mavlink_log_critical(&_mavlink_log_pub, "Figure 8 command rejected, Only available in fixed wing mode."); + } } - break; case vehicle_command_s::VEHICLE_CMD_ACTUATOR_TEST: diff --git a/src/modules/fw_pos_control/CMakeLists.txt b/src/modules/fw_pos_control/CMakeLists.txt index 4e63f0b564..c488d967c2 100644 --- a/src/modules/fw_pos_control/CMakeLists.txt +++ b/src/modules/fw_pos_control/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(launchdetection) add_subdirectory(runway_takeoff) +add_subdirectory(figure_eight) px4_add_module( MODULE modules__fw_pos_control @@ -41,6 +42,7 @@ px4_add_module( FixedwingPositionControl.cpp FixedwingPositionControl.hpp DEPENDS + figure_eight launchdetection npfg runway_takeoff diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index a7f5bb5eab..773dbaf9a3 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -53,6 +53,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), + _figure_eight(_npfg, _wind_vel, _eas2tas), _launchDetector(this), _runway_takeoff(this) { @@ -895,7 +896,12 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER || current_sp.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - publishOrbitStatus(current_sp); + if (current_sp.loiter_pattern == position_setpoint_s::LOITER_TYPE_FIGUREEIGHT) { + publishFigureEightStatus(current_sp); + + } else { + publishOrbitStatus(current_sp); + } } switch (position_sp_type) { @@ -914,10 +920,24 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto break; case position_setpoint_s::SETPOINT_TYPE_LOITER: - control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next); + if (current_sp.loiter_pattern == position_setpoint_s::LOITER_TYPE_FIGUREEIGHT) { + controlAutoFigureEight(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp); + + } else { + control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next); + + } + break; } + /* reset loiter state */ + if ((position_sp_type != position_setpoint_s::SETPOINT_TYPE_LOITER) || + ((position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER) && + (current_sp.loiter_pattern != position_setpoint_s::LOITER_TYPE_FIGUREEIGHT))) { + _figure_eight.resetPattern(); + } + /* Copy thrust output for publication, handle special cases */ if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { @@ -1287,6 +1307,63 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons _param_climbrate_target.get()); } +void +FixedwingPositionControl::controlAutoFigureEight(const float control_interval, const Vector2d &curr_pos, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +{ + // airspeed settings + float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, + _param_fw_airspd_min.get(), ground_speed); + + // Lateral Control + + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + + FigureEight::FigureEightPatternParameters params; + params.center_pos_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); + params.loiter_direction_counter_clockwise = pos_sp_curr.loiter_direction_counter_clockwise; + params.loiter_minor_radius = pos_sp_curr.loiter_minor_radius; + 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(); + target_airspeed = _figure_eight.getAirspeedSetpoint(); + _target_bearing = _figure_eight.getTargetBearing(); + _closest_point_on_path = _figure_eight.getClosestPoint(); + + // TECS + float tecs_fw_thr_min; + float tecs_fw_thr_max; + + if (pos_sp_curr.gliding_enabled) { + /* enable gliding with this waypoint */ + _tecs.set_speed_weight(2.0f); + tecs_fw_thr_min = 0.0; + tecs_fw_thr_max = 0.0; + + } else { + tecs_fw_thr_min = _param_fw_thr_min.get(); + tecs_fw_thr_max = _param_fw_thr_max.get(); + } + + tecs_update_pitch_throttle(control_interval, + pos_sp_curr.alt, + target_airspeed, + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + tecs_fw_thr_min, + tecs_fw_thr_max, + _param_sinkrate_target.get(), + _param_climbrate_target.get()); + + // Yaw + _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw +} + void FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) @@ -2834,6 +2911,21 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_ _orbit_status_pub.publish(orbit_status); } +void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp) +{ + figure_eight_status_s figure_eight_status{}; + figure_eight_status.timestamp = hrt_absolute_time(); + figure_eight_status.major_radius = pos_sp.loiter_radius * (pos_sp.loiter_direction_counter_clockwise ? -1.f : 1.f); + figure_eight_status.minor_radius = pos_sp.loiter_minor_radius; + figure_eight_status.orientation = pos_sp.loiter_orientation; + figure_eight_status.frame = 5; //MAV_FRAME_GLOBAL_INT + figure_eight_status.x = static_cast(pos_sp.lat * 1e7); + figure_eight_status.y = static_cast(pos_sp.lon * 1e7); + figure_eight_status.z = pos_sp.alt; + + _figure_eight_status_pub.publish(figure_eight_status); +} + void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, const Vector2f &end_waypoint, const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index e46c14c005..ff04c5aafd 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -47,6 +47,7 @@ #ifndef FIXEDWINGPOSITIONCONTROL_HPP_ #define FIXEDWINGPOSITIONCONTROL_HPP_ +#include "figure_eight/FigureEight.hpp" #include "launchdetection/LaunchDetector.h" #include "runway_takeoff/RunwayTakeoff.h" @@ -94,6 +95,7 @@ #include #include #include +#include #include using namespace launchdetection; @@ -213,6 +215,7 @@ private: uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; uORB::Publication _launch_detection_status_pub{ORB_ID(launch_detection_status)}; uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; + uORB::Publication _figure_eight_status_pub{ORB_ID(figure_eight_status)}; uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; @@ -271,6 +274,9 @@ private: bool _landed{true}; + /* Loitering */ + FigureEight _figure_eight; + // indicates whether the plane was in the air in the previous interation bool _was_in_air{false}; @@ -584,6 +590,18 @@ private: void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next); + /** + * Vehicle control for the autonomous figure 8 mode. + * + * @param control_interval Time since last position control call [s] + * @param curr_pos the current 2D absolute position of the vehicle in [deg]. + * @param ground_speed the 2D ground speed of the vehicle in [m/s]. + * @param pos_sp_prev the previous position setpoint. + * @param pos_sp_curr the current position setpoint. + */ + void controlAutoFigureEight(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); + /** * @brief Controls a desired airspeed, bearing, and height rate. * @@ -700,6 +718,7 @@ private: float airspeed_sp); void publishOrbitStatus(const position_setpoint_s pos_sp); + void publishFigureEightStatus(const position_setpoint_s pos_sp); SlewRate _airspeed_slew_rate_controller; diff --git a/src/modules/fw_pos_control/figure_eight/CMakeLists.txt b/src/modules/fw_pos_control/figure_eight/CMakeLists.txt new file mode 100644 index 0000000000..df249c66c9 --- /dev/null +++ b/src/modules/fw_pos_control/figure_eight/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(figure_eight + FigureEight.cpp +) diff --git a/src/modules/fw_pos_control/figure_eight/FigureEight.cpp b/src/modules/fw_pos_control/figure_eight/FigureEight.cpp new file mode 100644 index 0000000000..8ec3aae217 --- /dev/null +++ b/src/modules/fw_pos_control/figure_eight/FigureEight.cpp @@ -0,0 +1,378 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file FigureEight.cpp + * Helper class for fixed wing position controller when flying a figure 8 loiter pattern. + */ + +#include "FigureEight.hpp" + +#include + +#include "lib/geo/geo.h" +#include + +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}; + +FigureEight::FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas) : + ModuleParams(nullptr), + _npfg(npfg), + _wind_vel(wind_vel), + _eas2tas(eas2tas) +{ + +} + +void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters) +{ + // 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))) { + 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; + + } else if (rel_pos_to_center(0) < -NORMALIZED_MAJOR_RADIUS) { // Far away south + _current_segment = FigureEightSegment::SEGMENT_CIRCLE_SOUTH; + + } else if (ground_speed_rotated.dot(Vector2f{1.0f, 0.0f}) > 0.0f) { // Flying northbound + if (rel_pos_to_center(0) > pattern_points.normalized_north_circle_offset(0)) { // already at north circle + _current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH; + + } else { + _current_segment = FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST; + } + + } else { + if (rel_pos_to_center(0) < pattern_points.normalized_south_circle_offset(0)) { // already at south circle + _current_segment = FigureEightSegment::SEGMENT_CIRCLE_SOUTH; + + } else { + _current_segment = FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST; + } + } + + _active_parameters = parameters; + _pos_passed_circle_center_along_major_axis = false; + } +} + +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) +{ + const float normalized_minor_radius = (parameters.loiter_minor_radius / parameters.loiter_radius) * + NORMALIZED_MAJOR_RADIUS; + const float cos_transition_angle = parameters.loiter_minor_radius / (parameters.loiter_radius - + parameters.loiter_minor_radius); + const float sin_transition_angle = sqrtf(1.0f - cos_transition_angle * cos_transition_angle); + pattern_points.normalized_north_circle_offset = Vector2f{NORMALIZED_MAJOR_RADIUS - normalized_minor_radius, 0.0f}; + pattern_points.normalized_north_entry_offset = Vector2f{NORMALIZED_MAJOR_RADIUS - ((normalized_minor_radius) * (1.0f + cos_transition_angle)), + -normalized_minor_radius * sin_transition_angle}; + pattern_points.normalized_north_exit_offset = Vector2f{NORMALIZED_MAJOR_RADIUS - ((normalized_minor_radius) * (1.0f + cos_transition_angle)), + normalized_minor_radius * sin_transition_angle}; + pattern_points.normalized_south_circle_offset = Vector2f{-NORMALIZED_MAJOR_RADIUS + normalized_minor_radius, 0.0f}; + pattern_points.normalized_south_entry_offset = Vector2f{-NORMALIZED_MAJOR_RADIUS + ((normalized_minor_radius) * (1.0f + cos_transition_angle)), + -normalized_minor_radius * sin_transition_angle}; + pattern_points.normalized_south_exit_offset = Vector2f{-NORMALIZED_MAJOR_RADIUS + ((normalized_minor_radius) * (1.0f + cos_transition_angle)), + normalized_minor_radius * sin_transition_angle}; +} + +void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters ¶meters, + const FigureEightPatternPoints &pattern_points) +{ + Vector2f rel_pos_to_center; + calculatePositionToCenterNormalizedRotated(rel_pos_to_center, 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)) { + _pos_passed_circle_center_along_major_axis = true; + } + + Vector2f vector_to_exit_normalized = pattern_points.normalized_north_exit_offset - rel_pos_to_center; + + /* 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)))) { + _current_segment = FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST; + } + } + break; + + 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; + + /* 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)) { + _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)) { + _pos_passed_circle_center_along_major_axis = true; + } + + Vector2f vector_to_exit_normalized = pattern_points.normalized_south_exit_offset - rel_pos_to_center; + + /* 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)))) { + _current_segment = FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST; + } + + } + break; + + 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; + + /* 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)) { + _current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH; + } + } + break; + + case FigureEightSegment::SEGMENT_UNDEFINED: + default: + break; + } +} + +void FigureEight::applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed, + const FigureEightPatternPoints &pattern_points) +{ + switch (_current_segment) { + case FigureEightSegment::SEGMENT_CIRCLE_NORTH: { + applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local, + ground_speed, parameters, target_airspeed); + } + break; + + case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: { + // Follow path from north-east to south-west + applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset, curr_pos_local, + ground_speed, parameters, target_airspeed); + } + break; + + case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: { + applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local, + ground_speed, parameters, target_airspeed); + } + break; + + case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: { + // follow path from south-east to north-west + applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset, curr_pos_local, + ground_speed, parameters, target_airspeed); + } + break; + + case FigureEightSegment::SEGMENT_UNDEFINED: + default: + break; + } +} + +void FigureEight::calculatePositionToCenterNormalizedRotated(matrix::Vector2f &pos_to_center_normalized_rotated, + const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters ¶meters) const +{ + Vector2f pos_to_center = 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; + + // rotate position with respect to figure eight orientation and direction. + pos_to_center_normalized_rotated = Dcm2f(-calculateRotationAngle(parameters)) * pos_to_center_normalized; +} + +float FigureEight::calculateRotationAngle(const FigureEightPatternParameters ¶meters) const +{ + // rotate position with respect to figure eight orientation and direction. + float yaw_rotation = parameters.loiter_orientation; + + // figure eight pattern is symmetric, changing the direction is the same as a rotation by 180° around center + if (parameters.loiter_direction_counter_clockwise) { + yaw_rotation += M_PI_F; + } + + return yaw_rotation; +} + +void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset, + const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed) +{ + const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f; + + Vector2f circle_offset = normalized_circle_offset * (parameters.loiter_radius / NORMALIZED_MAJOR_RADIUS); + Vector2f circle_offset_rotated = Dcm2f(calculateRotationAngle(parameters)) * circle_offset; + Vector2f circle_center = parameters.center_pos_local + circle_offset_rotated; + + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + + const Vector2f vector_center_to_vehicle = curr_pos_local - circle_center; + const float dist_to_center = vector_center_to_vehicle.norm(); + + Vector2f unit_vec_center_to_closest_pt = vector_center_to_vehicle.normalized(); + if (dist_to_center < 0.1f) { + // the logic breaks down at the circle center, employ some mitigation strategies + // until we exit this region + if (ground_speed.norm() < 0.1f) { + // arbitrarily set the point in the northern top of the circle + unit_vec_center_to_closest_pt = Vector2f{1.0f, 0.0f}; + + } else { + // set the point in the direction we are moving + unit_vec_center_to_closest_pt = ground_speed.normalized(); + } + } + + const Vector2f unit_path_tangent = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)}; + + float path_curvature = loiter_direction_multiplier / parameters.loiter_minor_radius; + _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); + _closest_point_on_path = unit_vec_center_to_closest_pt * parameters.loiter_minor_radius + circle_center; + _npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent, + _closest_point_on_path, path_curvature); + + _roll_setpoint = _npfg.getRollSetpoint(); + _indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas; +} + +void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset, + const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local, + const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters, float target_airspeed) +{ + const Dcm2f rotation_matrix(calculateRotationAngle(parameters)); + + // Calculate start offset depending on radius + const Vector2f start_offset = normalized_line_start_offset * (parameters.loiter_radius / NORMALIZED_MAJOR_RADIUS); + const Vector2f start_offset_rotated = rotation_matrix * start_offset; + const Vector2f line_segment_start_position = parameters.center_pos_local + start_offset_rotated; + + const Vector2f end_offset = normalized_line_end_offset * (parameters.loiter_radius / NORMALIZED_MAJOR_RADIUS); + const Vector2f end_offset_rotated = rotation_matrix * end_offset; + const Vector2f line_segment_end_position = parameters.center_pos_local + end_offset_rotated; + + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + const Vector2f path_tangent = line_segment_end_position - line_segment_start_position; + const Vector2f unit_path_tangent = path_tangent.normalized(); + _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); + const Vector2f vector_A_to_vehicle = curr_pos_local - line_segment_start_position; + _closest_point_on_path = line_segment_start_position + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; + _npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), line_segment_start_position, 0.0f); + _roll_setpoint = _npfg.getRollSetpoint(); + _indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas; +} diff --git a/src/modules/fw_pos_control/figure_eight/FigureEight.hpp b/src/modules/fw_pos_control/figure_eight/FigureEight.hpp new file mode 100644 index 0000000000..95f75dcb8b --- /dev/null +++ b/src/modules/fw_pos_control/figure_eight/FigureEight.hpp @@ -0,0 +1,280 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file FigureEight.hpp + * Helper class for lateral fixed wing position controller when flying a figure 8 loiter pattern. + * + */ + +#ifndef FIGUREEIGHT_HPP_ +#define FIGUREEIGHT_HPP_ + +#include + +#include +#include +#include + +#include "lib/npfg/npfg.hpp" + +class FigureEight : public ModuleParams +{ +public: + /** + * @brief Figure eight pattern points strust + * + * Struct defining all relevant points for the figure eight pattern. + * + */ + struct FigureEightPatternPoints { + matrix::Vector2f normalized_north_circle_offset; + matrix::Vector2f normalized_north_entry_offset; + matrix::Vector2f normalized_north_exit_offset; + matrix::Vector2f normalized_south_circle_offset; + matrix::Vector2f normalized_south_entry_offset; + matrix::Vector2f normalized_south_exit_offset; + }; + struct FigureEightPatternParameters { + matrix::Vector2f center_pos_local; + float loiter_radius; + float loiter_minor_radius; + float loiter_orientation; + bool loiter_direction_counter_clockwise; + }; + /** + * @brief Construct a new Figure Eight object + * + * @param[in] npfg is the reference to the parent npfg object. + * @param[in] wind_vel is the reference to the parent wind velocity [m/s]. + * @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. + * + * Reset the figure eight pattern such that it can be properly initialized on a new figure eight pattern. + * + */ + void resetPattern(); + /** + * @brief Update roll and airspeed setpoint. + * + * @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] target_airspeed is the current targeted indicated airspeed [m/s]. + */ + void updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed); + /** + * @brief Get the roll setpoint + * + * @return the roll setpoint in [rad]. + */ + float getRollSetpoint() const {return _roll_setpoint;}; + /** + * @brief Get the indicated airspeed setpoint + * + * @return the indicated airspeed setpoint in [m/s]. + */ + float getAirspeedSetpoint() const {return _indicated_airspeed_setpoint;}; + /** + * @brief Get the target bearing of current point on figure of eight + * + * @return target bearing in [rad] + */ + float getTargetBearing() const {return _target_bearing;}; + /** + * @brief Get the closest point on the figure of eight + * + * @return Local coordinates of closes point on the figure of eight + */ + matrix::Vector2f getClosestPoint() const {return _closest_point_on_path;}; + + +private: + /** + * @brief Calculate figure eight pattern points + * + * @param[out] pattern_points is the output with the calculated points for the figure eight. + * @param[in] parameters is the parameter set defining the figure eight shape. + */ + void calculateFigureEightPoints(FigureEightPatternPoints &pattern_points, + const FigureEightPatternParameters ¶meters); + /** + * @brief Apply lateral control logic + * + * + * @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] target_airspeed is the current targeted indicated airspeed [m/s]. + * @param[in] pattern_points are the relevant points defining the figure eight pattern. + */ + void applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed, + const FigureEightPatternPoints &pattern_points); + /** + * @brief Update active segment. + * + * @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. + * @param[in] pattern_points are the relevant points defining the figure eight pattern. + */ + void updateSegment(const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters ¶meters, + const FigureEightPatternPoints &pattern_points); + /** + * @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[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, + const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters ¶meters) const; + /** + * @brief Calculate rotation angle. + * + * @param[in] parameters is the parameter set defining the figure eight shape. + * + * @return is the rotation angle of the major axis compared to north in [rad]. + */ + float calculateRotationAngle(const FigureEightPatternParameters ¶meters) const; + /** + * @brief Apply circular lateral control + * + * @param[in] loiter_direction_counter_clockwise flag if the circle direction should be counter-clockwise. + * @param[in] normalized_circle_offset is the normalized position offset of the circle compared to the pattern center. + * @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] target_airspeed is the current targeted indicated airspeed [m/s]. + */ + void applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset, + const matrix::Vector2f &curr_pos_local, + const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters, float target_airspeed); + /** + * @brief Apply path lateral control + * + * @param[in] normalized_line_start_offset is the normalized position offset of the start point of the path compared to the pattern center. + * @param[in] normalized_line_end_offset is the normalized position offset of the end point of the path compared to the pattern center. + * @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] target_airspeed is the current targeted indicated airspeed [m/s]. + */ + void applyLine(const matrix::Vector2f &normalized_line_start_offset, const matrix::Vector2f &normalized_line_end_offset, + const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed); + +private: + /** + * @brief npfg lateral control object. + * + */ + NPFG &_npfg; + + /** + * @brief Wind velocity in [m/s]. + * + */ + const matrix::Vector2f &_wind_vel; + /** + * @brief Conversion factor from indicated to true airspeed. + * + */ + const float &_eas2tas; + /** + * @brief Roll setpoint in [rad]. + * + */ + float _roll_setpoint; + /** + * @brief Indicated airspeed setpoint in [m/s]. + * + */ + float _indicated_airspeed_setpoint; + /** + * @brief active figure eight position setpoint. + * + */ + FigureEightPatternParameters _active_parameters; + + /** + * @brief Target bearing in [rad]. + * + */ + float _target_bearing{0.0f}; + + /** + * @brief Closest point on figure of eight to track + * + */ + matrix::Vector2f _closest_point_on_path; + + enum class FigureEightSegment { + SEGMENT_UNDEFINED, + SEGMENT_CIRCLE_NORTH, + SEGMENT_NORTHEAST_SOUTHWEST, + SEGMENT_CIRCLE_SOUTH, + SEGMENT_SOUTHEAST_NORTHWEST + }; + + /** + * @brief Current active segment of the figure eight pattern. + * + */ + FigureEightSegment _current_segment{FigureEightSegment::SEGMENT_UNDEFINED}; + /** + * @brief flag if vehicle position passed circle center along major axis when on circle segment. + * + */ + bool _pos_passed_circle_center_along_major_axis; + DEFINE_PARAMETERS( + (ParamFloat) _param_fw_airspd_max, + (ParamFloat) _param_nav_loiter_rad + ) +}; + +#endif // FIGUREEIGHT_HPP_ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 80bcc8f5a7..25736269a5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1444,6 +1444,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("DEBUG_VECT", 1.0f); configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); configure_stream_local("LINK_NODE_STATUS", 1.0f); + configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f); #endif // !CONSTRAINED_FLASH break; @@ -1509,6 +1510,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("DEBUG_VECT", 10.0f); configure_stream_local("NAMED_VALUE_FLOAT", 10.0f); configure_stream_local("LINK_NODE_STATUS", 1.0f); + configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f); #endif // !CONSTRAINED_FLASH break; @@ -1570,6 +1572,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("DEBUG_VECT", 1.0f); configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); configure_stream_local("LINK_NODE_STATUS", 1.0f); + configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 2.0f); #endif // !CONSTRAINED_FLASH break; @@ -1663,6 +1666,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("DEBUG_VECT", 50.0f); configure_stream_local("NAMED_VALUE_FLOAT", 50.0f); configure_stream_local("LINK_NODE_STATUS", 1.0f); + configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f); #endif // !CONSTRAINED_FLASH break; @@ -1740,6 +1744,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f); configure_stream_local("DEBUG_VECT", 1.0f); configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); + configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f); #endif // !CONSTRAINED_FLASH break; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index ef8f1c3b55..a7effb4915 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -120,6 +120,7 @@ #include "streams/VFR_HUD.hpp" #include "streams/VIBRATION.hpp" #include "streams/WIND_COV.hpp" +#include "streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp" #if !defined(CONSTRAINED_FLASH) # include "streams/ADSB_VEHICLE.hpp" @@ -431,6 +432,9 @@ static const StreamListItem streams_list[] = { #if defined(ORBIT_EXECUTION_STATUS_HPP) create_stream_list_item(), #endif // ORBIT_EXECUTION_STATUS_HPP +#if defined(FIGURE_EIGHT_EXECUTION_STATUS_HPP) + create_stream_list_item(), +#endif // FIGURE_EIGHT_EXECUTION_STATUS_HPP #if defined(OBSTACLE_DISTANCE_HPP) create_stream_list_item(), #endif // OBSTACLE_DISTANCE_HPP diff --git a/src/modules/mavlink/streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp b/src/modules/mavlink/streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp new file mode 100644 index 0000000000..f0d33b2db9 --- /dev/null +++ b/src/modules/mavlink/streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef FIGURE_EIGHT_EXECUTION_STATUS_HPP +#define FIGURE_EIGHT_EXECUTION_STATUS_HPP + +#include +#include +#include + +class MavlinkStreamFigureEightStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamFigureEightStatus(mavlink); } + + static constexpr const char *get_name_static() { return "FIGURE_EIGHT_EXECUTION_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _figure_eight_status_subs.advertised() ? MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamFigureEightStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _figure_eight_status_subs{ORB_ID::figure_eight_status}; + + bool send() override + { + figure_eight_status_s figure_eight_status; + + if ((_mavlink->get_free_tx_buf() >= get_size()) && _figure_eight_status_subs.update(&figure_eight_status)) { + mavlink_figure_eight_execution_status_t msg_figure_eight_execution_status{}; + + msg_figure_eight_execution_status.time_usec = figure_eight_status.timestamp; + msg_figure_eight_execution_status.major_radius = figure_eight_status.major_radius; + msg_figure_eight_execution_status.minor_radius = figure_eight_status.minor_radius; + msg_figure_eight_execution_status.frame = figure_eight_status.frame; + msg_figure_eight_execution_status.orientation = figure_eight_status.orientation; + msg_figure_eight_execution_status.x = figure_eight_status.x; + msg_figure_eight_execution_status.y = figure_eight_status.y; + msg_figure_eight_execution_status.z = figure_eight_status.z; + + mavlink_msg_figure_eight_execution_status_send_struct(_mavlink->get_channel(), &msg_figure_eight_execution_status); + + return true; + } + + return false; + } +}; + +#endif // FIGURE_EIGHT_EXECUTION_STATUS_HPP diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 100998bc3f..044c17c7cd 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -382,6 +382,27 @@ void Navigator::run() rep->current.loiter_radius = get_loiter_radius(); } + if (PX4_ISFINITE(curr->current.loiter_minor_radius) && fabsf(curr->current.loiter_minor_radius) > FLT_EPSILON) { + rep->current.loiter_minor_radius = curr->current.loiter_minor_radius; + + } else { + rep->current.loiter_minor_radius = NAN; + } + + if (PX4_ISFINITE(curr->current.loiter_orientation) && fabsf(curr->current.loiter_minor_radius) > FLT_EPSILON) { + rep->current.loiter_orientation = curr->current.loiter_orientation; + + } else { + rep->current.loiter_orientation = 0.0f; + } + + if (curr->current.loiter_pattern > 0) { + rep->current.loiter_pattern = curr->current.loiter_pattern; + + } else { + rep->current.loiter_pattern = position_setpoint_s::LOITER_TYPE_ORBIT; + } + rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise; } @@ -505,6 +526,8 @@ void Navigator::run() rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction_counter_clockwise = false; + rep->current.loiter_orientation = 0.0f; + rep->current.loiter_pattern = position_setpoint_s::LOITER_TYPE_ORBIT; rep->current.cruising_throttle = get_cruising_throttle(); // on entering Loiter mode, reset speed setpoint to default @@ -531,6 +554,58 @@ void Navigator::run() mavlink_log_critical(&_mavlink_log_pub, "Orbit is outside geofence"); } + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_FIGUREEIGHT && + get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + // Only valid for fixed wing mode + + bool orbit_location_valid = true; + + vehicle_global_position_s position_setpoint{}; + position_setpoint.lat = PX4_ISFINITE(cmd.param5) ? cmd.param5 : get_global_position()->lat; + position_setpoint.lon = PX4_ISFINITE(cmd.param6) ? cmd.param6 : get_global_position()->lon; + position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt; + + if (have_geofence_position_data) { + orbit_location_valid = geofence_allows_position(position_setpoint); + } + + if (orbit_location_valid) { + position_setpoint_triplet_s *rep = get_reposition_triplet(); + rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + rep->current.loiter_minor_radius = fabsf(get_loiter_radius()); + rep->current.loiter_direction_counter_clockwise = get_loiter_radius() < 0; + rep->current.loiter_orientation = 0.0f; + rep->current.loiter_pattern = position_setpoint_s::LOITER_TYPE_FIGUREEIGHT; + rep->current.cruising_speed = get_cruising_speed(); + + if (PX4_ISFINITE(cmd.param2) && fabsf(cmd.param2) > FLT_EPSILON) { + rep->current.loiter_minor_radius = fabsf(cmd.param2); + } + + rep->current.loiter_radius = 2.5f * rep->current.loiter_minor_radius; + + if (PX4_ISFINITE(cmd.param1)) { + rep->current.loiter_radius = fabsf(cmd.param1); + rep->current.loiter_direction_counter_clockwise = cmd.param1 < 0; + } + + rep->current.loiter_radius = math::max(rep->current.loiter_radius, 2.0f * rep->current.loiter_minor_radius); + + if (PX4_ISFINITE(cmd.param4)) { + rep->current.loiter_orientation = cmd.param4; + } + + rep->current.lat = position_setpoint.lat; + rep->current.lon = position_setpoint.lon; + rep->current.alt = position_setpoint.alt; + + rep->current.valid = true; + rep->current.timestamp = hrt_absolute_time(); + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Figure 8 is outside geofence"); + } + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { position_setpoint_triplet_s *rep = get_takeoff_triplet();