mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 05:53:34 +08:00
flight_mode_manager: merge with flight_tasks
This commit is contained in:
Daniel Agar
committed by
Lorenz Meier
Lorenz Meier
parent
b1e442b830
commit
4d7b875ee2
@@ -0,0 +1,39 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018 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(FlightTaskOrbit
|
||||
FlightTaskOrbit.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmoothVel SlewRate)
|
||||
target_include_directories(FlightTaskOrbit PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@@ -0,0 +1,275 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018-2019 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 FlightTaskOrbit.cpp
|
||||
*/
|
||||
|
||||
#include "FlightTaskOrbit.hpp"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
FlightTaskOrbit::FlightTaskOrbit() : _circle_approach_line(_position)
|
||||
{
|
||||
_sticks_data_required = false;
|
||||
}
|
||||
|
||||
bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
|
||||
{
|
||||
bool ret = true;
|
||||
// save previous velocity and roatation direction
|
||||
float v = fabsf(_v);
|
||||
bool clockwise = _v > 0;
|
||||
|
||||
// commanded radius
|
||||
if (PX4_ISFINITE(command.param1)) {
|
||||
clockwise = command.param1 > 0;
|
||||
const float r = fabsf(command.param1);
|
||||
ret = ret && setRadius(r);
|
||||
}
|
||||
|
||||
// commanded velocity, take sign of radius as rotation direction
|
||||
if (PX4_ISFINITE(command.param2)) {
|
||||
v = command.param2;
|
||||
}
|
||||
|
||||
ret = ret && setVelocity(v * (clockwise ? 1.f : -1.f));
|
||||
|
||||
// commanded heading behaviour
|
||||
if (PX4_ISFINITE(command.param3)) {
|
||||
_yaw_behaviour = command.param3;
|
||||
}
|
||||
|
||||
// save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING
|
||||
_initial_heading = _yaw;
|
||||
|
||||
// TODO: apply x,y / z independently in geo library
|
||||
// commanded center coordinates
|
||||
// if(PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
|
||||
// map_projection_global_project(command.param5, command.param6, &_center(0), &_center(1));
|
||||
// }
|
||||
|
||||
// commanded altitude
|
||||
// if(PX4_ISFINITE(command.param7)) {
|
||||
// _position_setpoint(2) = gl_ref.alt - command.param7;
|
||||
// }
|
||||
|
||||
if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6) && PX4_ISFINITE(command.param7)) {
|
||||
if (globallocalconverter_tolocal(command.param5, command.param6, command.param7, &_center(0), &_center(1),
|
||||
&_position_setpoint(2))) {
|
||||
// global to local conversion failed
|
||||
ret = false;
|
||||
}
|
||||
}
|
||||
|
||||
// perpendicularly approach the orbit circle again when new parameters get commanded
|
||||
_in_circle_approach = true;
|
||||
_circle_approach_line.reset();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool FlightTaskOrbit::sendTelemetry()
|
||||
{
|
||||
orbit_status_s orbit_status{};
|
||||
orbit_status.radius = math::signNoZero(_v) * _r;
|
||||
orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
|
||||
orbit_status.yaw_behaviour = _yaw_behaviour;
|
||||
|
||||
if (globallocalconverter_toglobal(_center(0), _center(1), _position_setpoint(2), &orbit_status.x, &orbit_status.y,
|
||||
&orbit_status.z)) {
|
||||
return false; // don't send the message if the transformation failed
|
||||
}
|
||||
|
||||
orbit_status.timestamp = hrt_absolute_time();
|
||||
_orbit_status_pub.publish(orbit_status);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FlightTaskOrbit::setRadius(float r)
|
||||
{
|
||||
// clip the radius to be within range
|
||||
r = math::constrain(r, _radius_min, _radius_max);
|
||||
|
||||
// small radius is more important than high velocity for safety
|
||||
if (!checkAcceleration(r, _v, _acceleration_max)) {
|
||||
_v = sign(_v) * sqrtf(_acceleration_max * r);
|
||||
}
|
||||
|
||||
if (fabs(_r - r) > FLT_EPSILON) {
|
||||
_circle_approach_line.reset();
|
||||
}
|
||||
|
||||
_r = r;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FlightTaskOrbit::setVelocity(const float v)
|
||||
{
|
||||
if (fabs(v) < _velocity_max &&
|
||||
checkAcceleration(_r, v, _acceleration_max)) {
|
||||
_v = v;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool FlightTaskOrbit::checkAcceleration(float r, float v, float a)
|
||||
{
|
||||
return v * v < a * r;
|
||||
}
|
||||
|
||||
bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint)
|
||||
{
|
||||
bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
|
||||
_r = _radius_min;
|
||||
_v = 1.f;
|
||||
_center = Vector2f(_position);
|
||||
_center(0) -= _r;
|
||||
_initial_heading = _yaw;
|
||||
_slew_rate_yaw.setForcedValue(_yaw);
|
||||
_slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get()));
|
||||
|
||||
// need a valid position and velocity
|
||||
ret = ret && PX4_ISFINITE(_position(0))
|
||||
&& PX4_ISFINITE(_position(1))
|
||||
&& PX4_ISFINITE(_position(2))
|
||||
&& PX4_ISFINITE(_velocity(0))
|
||||
&& PX4_ISFINITE(_velocity(1))
|
||||
&& PX4_ISFINITE(_velocity(2));
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool FlightTaskOrbit::update()
|
||||
{
|
||||
// update altitude
|
||||
bool ret = FlightTaskManualAltitudeSmoothVel::update();
|
||||
|
||||
// stick input adjusts parameters within a fixed time frame
|
||||
const float r = _r - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f);
|
||||
const float v = _v - _sticks.getPositionExpo()(1) * _deltatime * (_velocity_max / 4.f);
|
||||
|
||||
setRadius(r);
|
||||
setVelocity(v);
|
||||
|
||||
const Vector2f center_to_position = Vector2f(_position) - _center;
|
||||
|
||||
if (_in_circle_approach) {
|
||||
generate_circle_approach_setpoints(center_to_position);
|
||||
|
||||
} else {
|
||||
generate_circle_setpoints(center_to_position);
|
||||
generate_circle_yaw_setpoints(center_to_position);
|
||||
}
|
||||
|
||||
// Apply yaw smoothing
|
||||
_yaw_setpoint = _slew_rate_yaw.update(_yaw_setpoint, _deltatime);
|
||||
|
||||
// publish information to UI
|
||||
sendTelemetry();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void FlightTaskOrbit::generate_circle_approach_setpoints(const Vector2f ¢er_to_position)
|
||||
{
|
||||
const Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
|
||||
|
||||
if (_circle_approach_line.isEndReached()) {
|
||||
// calculate target point on circle and plan a line trajectory
|
||||
const Vector2f closest_circle_point = Vector2f(_position) + start_to_circle;
|
||||
const Vector3f target = Vector3f(closest_circle_point(0), closest_circle_point(1), _position(2));
|
||||
_circle_approach_line.setLineFromTo(_position, target);
|
||||
_circle_approach_line.setSpeed(_param_mpc_xy_cruise.get());
|
||||
}
|
||||
|
||||
_yaw_setpoint = atan2f(start_to_circle(1), start_to_circle(0));
|
||||
|
||||
// follow the planned line and switch to orbiting once the circle is reached
|
||||
_circle_approach_line.generateSetpoints(_position_setpoint, _velocity_setpoint);
|
||||
_in_circle_approach = !_circle_approach_line.isEndReached();
|
||||
}
|
||||
|
||||
void FlightTaskOrbit::generate_circle_setpoints(const Vector2f ¢er_to_position)
|
||||
{
|
||||
// xy velocity to go around in a circle
|
||||
Vector2f velocity_xy(-center_to_position(1), center_to_position(0));
|
||||
velocity_xy = velocity_xy.unit_or_zero();
|
||||
velocity_xy *= _v;
|
||||
|
||||
// xy velocity adjustment to stay on the radius distance
|
||||
velocity_xy += (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
|
||||
|
||||
_position_setpoint(0) = _position_setpoint(1) = NAN;
|
||||
_velocity_setpoint.xy() = velocity_xy;
|
||||
_acceleration_setpoint.xy() = -center_to_position.unit_or_zero() * _v * _v / _r;
|
||||
}
|
||||
|
||||
void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f ¢er_to_position)
|
||||
{
|
||||
switch (_yaw_behaviour) {
|
||||
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING:
|
||||
// make vehicle keep the same heading as when the orbit was commanded
|
||||
_yaw_setpoint = _initial_heading;
|
||||
_yawspeed_setpoint = NAN;
|
||||
break;
|
||||
|
||||
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED:
|
||||
// no yaw setpoint
|
||||
_yaw_setpoint = NAN;
|
||||
_yawspeed_setpoint = NAN;
|
||||
break;
|
||||
|
||||
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
|
||||
_yaw_setpoint = atan2f(sign(_v) * center_to_position(0), -sign(_v) * center_to_position(1));
|
||||
_yawspeed_setpoint = _v / _r;
|
||||
break;
|
||||
|
||||
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:
|
||||
// inherit setpoint from altitude flight task
|
||||
break;
|
||||
|
||||
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER:
|
||||
default:
|
||||
_yaw_setpoint = atan2f(-center_to_position(1), -center_to_position(0));
|
||||
// yawspeed feed-forward because we know the necessary angular rate
|
||||
_yawspeed_setpoint = _v / _r;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2019 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 FlightTaskOrbit.hpp
|
||||
*
|
||||
* Flight task for orbiting in a circle around a target position
|
||||
*
|
||||
* @author Matthias Grob <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "FlightTaskManualAltitudeSmoothVel.hpp"
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/orbit_status.h>
|
||||
#include <StraightLine.hpp>
|
||||
#include <SlewRateYaw.hpp>
|
||||
|
||||
class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
|
||||
{
|
||||
public:
|
||||
FlightTaskOrbit();
|
||||
virtual ~FlightTaskOrbit() = default;
|
||||
|
||||
bool applyCommandParameters(const vehicle_command_s &command) override;
|
||||
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
|
||||
bool update() override;
|
||||
|
||||
/**
|
||||
* Check the feasibility of orbit parameters with respect to
|
||||
* centripetal acceleration a = v^2 / r
|
||||
* @param r desired radius
|
||||
* @param v desired velocity
|
||||
* @param a maximal allowed acceleration
|
||||
* @return true on success, false if value not accepted
|
||||
*/
|
||||
bool checkAcceleration(float r, float v, float a);
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Send out telemetry information for the log and MAVLink.
|
||||
* @return true on success, false on error
|
||||
*/
|
||||
bool sendTelemetry();
|
||||
|
||||
/**
|
||||
* Change the radius of the circle.
|
||||
* @param r desired new radius
|
||||
* @return true on success, false if value not accepted
|
||||
*/
|
||||
bool setRadius(const float r);
|
||||
|
||||
/**
|
||||
* Change the velocity of the vehicle on the circle.
|
||||
* @param v desired new velocity
|
||||
* @return true on success, false if value not accepted
|
||||
*/
|
||||
bool setVelocity(const float v);
|
||||
|
||||
private:
|
||||
/** generates setpoints to smoothly reach the closest point on the circle when starting from far away */
|
||||
void generate_circle_approach_setpoints(const matrix::Vector2f ¢er_to_position);
|
||||
/** generates xy setpoints to make the vehicle orbit */
|
||||
void generate_circle_setpoints(const matrix::Vector2f ¢er_to_position);
|
||||
/** generates yaw setpoints to control the vehicle's heading */
|
||||
void generate_circle_yaw_setpoints(const matrix::Vector2f ¢er_to_position);
|
||||
|
||||
float _r = 0.f; /**< radius with which to orbit the target */
|
||||
float _v = 0.f; /**< clockwise tangential velocity for orbiting in m/s */
|
||||
matrix::Vector2f _center; /**< local frame coordinates of the center point */
|
||||
|
||||
bool _in_circle_approach = false;
|
||||
StraightLine _circle_approach_line;
|
||||
|
||||
// TODO: create/use parameters for limits
|
||||
const float _radius_min = 1.f;
|
||||
const float _radius_max = 100.f;
|
||||
const float _velocity_max = 10.f;
|
||||
const float _acceleration_max = 2.f;
|
||||
|
||||
/** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */
|
||||
int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER;
|
||||
float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */
|
||||
SlewRateYaw<float> _slew_rate_yaw;
|
||||
|
||||
uORB::Publication<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise, /**< cruise speed for circle approach */
|
||||
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max
|
||||
)
|
||||
};
|
||||
Reference in New Issue
Block a user