mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
FixedwingPositionControl: Add support for figure 8 loitering.
The command is sent by a dedicated mavlink command and forwarded to the fixed wing position controller. The pattern is defined by the radius of the major axis, the radius of the minor axis and the orientation. The pattern is then defined by: The upper part of the pattern consist of a clockwise circle with radius defined by the minor axis. The center of the circle is defined by the major axis minus the minor axis away from the pattern center. The lower part of the pattern consist of a counter-clockwise circle with the same definitions as above. In between, the circles are connected with straight lines in a cross configuration. The lines are always tangetial to the circles. The orientation rotates the major axis around the NED down axis. The loitering logic is defined inside its own class used by the fixed wing position control module. It defines which segment (one of the circles or lines) is active and uses the path controller (npfg or l1-control) to determine the desired roll angle. A feedback mavlink message is send with the executed pattern parameters.
This commit is contained in:
Konrad
Daniel Agar
parent
0d6c2c8ce9
commit
e5e66370e7
@ -85,6 +85,7 @@ set(msg_files
|
||||
EstimatorStatus.msg
|
||||
EstimatorStatusFlags.msg
|
||||
Event.msg
|
||||
FigureEightStatus.msg
|
||||
FailsafeFlags.msg
|
||||
FailureDetectorStatus.msg
|
||||
FollowTarget.msg
|
||||
|
||||
8
msg/FigureEightStatus.msg
Normal file
8
msg/FigureEightStatus.msg
Normal file
@ -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.
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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|
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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<int32_t>(pos_sp.lat * 1e7);
|
||||
figure_eight_status.y = static_cast<int32_t>(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)
|
||||
{
|
||||
|
||||
@ -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 <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/wind.h>
|
||||
#include <uORB/topics/orbit_status.h>
|
||||
#include <uORB/topics/figure_eight_status.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
using namespace launchdetection;
|
||||
@ -213,6 +215,7 @@ private:
|
||||
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
|
||||
uORB::Publication<launch_detection_status_s> _launch_detection_status_pub{ORB_ID(launch_detection_status)};
|
||||
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
|
||||
uORB::Publication<figure_eight_status_s> _figure_eight_status_pub{ORB_ID(figure_eight_status)};
|
||||
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
|
||||
uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
|
||||
uORB::Publication<normalized_unsigned_setpoint_s> _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<float> _airspeed_slew_rate_controller;
|
||||
|
||||
|
||||
36
src/modules/fw_pos_control/figure_eight/CMakeLists.txt
Normal file
36
src/modules/fw_pos_control/figure_eight/CMakeLists.txt
Normal file
@ -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
|
||||
)
|
||||
378
src/modules/fw_pos_control/figure_eight/FigureEight.cpp
Normal file
378
src/modules/fw_pos_control/figure_eight/FigureEight.cpp
Normal file
@ -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 <cmath>
|
||||
|
||||
#include "lib/geo/geo.h"
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
|
||||
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;
|
||||
}
|
||||
280
src/modules/fw_pos_control/figure_eight/FigureEight.hpp
Normal file
280
src/modules/fw_pos_control/figure_eight/FigureEight.hpp
Normal file
@ -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 <cstdint>
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/param.h>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
|
||||
#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<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
|
||||
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad
|
||||
)
|
||||
};
|
||||
|
||||
#endif // FIGUREEIGHT_HPP_
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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<MavlinkStreamOrbitStatus>(),
|
||||
#endif // ORBIT_EXECUTION_STATUS_HPP
|
||||
#if defined(FIGURE_EIGHT_EXECUTION_STATUS_HPP)
|
||||
create_stream_list_item<MavlinkStreamFigureEightStatus>(),
|
||||
#endif // FIGURE_EIGHT_EXECUTION_STATUS_HPP
|
||||
#if defined(OBSTACLE_DISTANCE_HPP)
|
||||
create_stream_list_item<MavlinkStreamObstacleDistance>(),
|
||||
#endif // OBSTACLE_DISTANCE_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 <uORB/topics/figure_eight_status.h>
|
||||
#include <mavlink.h>
|
||||
#include <mavlink/mavlink_stream.h>
|
||||
|
||||
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
|
||||
@ -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();
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user