fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 00:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTask: introduce method for limits and adjust accordingly for all the tasks

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-18 11:20:51 +02:00
committed by Lorenz Meier
parent 73b4f452cc
commit 97be84b0e4
11 changed files with 111 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTask.cpp
+8
View File
@@ -17,6 +17,7 @@ bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
bool FlightTask::activate()
{
_resetSetpoints();
_updateSetpointLimits();
_time_stamp_activate = hrt_absolute_time();
return true;
}
@@ -72,3 +73,10 @@ bool FlightTask::_evaluateVehicleLocalPosition()
return false;
}
}
void FlightTask::_updateSetpointLimits()
{
_limits.speed_NE_max = MPC_XY_VEL_MAX.get();
_limits.speed_up_max = MPC_Z_VEL_MAX_UP.get();
_limits.speed_dn_max = MPC_Z_VEL_MAX_DN.get();
}
src/lib/FlightTasks/tasks/FlightTask.hpp
+34 -7
View File
@@ -105,6 +105,35 @@ public:
}
protected:
uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
/**
* Reset all setpoints to NAN
*/
void _resetSetpoints();
/**
* Update Flighttask limits
*/
void virtual _updateSetpointLimits();
/*
* Check and update local position
*/
bool _evaluateVehicleLocalPosition();
/**
* @brief Setpoint limits
*/
struct Limits {
float speed_NE_max = 1.0f; /**< maximum speed in NE-direction */
float speed_up_max = 1.0f; /**< maximum speed upwards */
float speed_dn_max = 1.0f; /**< maximum speed downwards */
};
Limits _limits;
/* Time abstraction */
static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
float _time = 0; /**< passed time in seconds since the task was activated */
@@ -128,11 +157,9 @@ protected:
float _yawspeed_setpoint;
float _dist_to_bottom;
/**
* Get the output data
*/
void _resetSetpoints();
uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
bool _evaluateVehicleLocalPosition();
DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) MPC_XY_VEL_MAX,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) MPC_Z_VEL_MAX_DN,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) MPC_Z_VEL_MAX_UP
);
};
src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
+15 -3
View File
@@ -94,12 +94,13 @@ bool FlightTaskAuto::_evaluateTriplets()
}
_type = (WaypointType)_sub_triplet_setpoint->get().current.type;
// always update cruise speed since that can change without waypoint changes
_mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed;
if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) {
// use default
_mc_cruise_speed = _mc_cruise_default.get();
if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f) || (_mc_cruise_speed > _limits.speed_NE_max)) {
// use default limit
_mc_cruise_speed = _limits.speed_NE_max;
}
// get target waypoint.
@@ -184,3 +185,14 @@ void FlightTaskAuto::_evaluateVehicleGlobalPosition()
_time_stamp_reference = _sub_vehicle_local_position->get().ref_timestamp;
}
}
void FlightTaskAuto::_updateSetpointLimits()
{
FlightTask::_updateSetpointLimits();
// only adjust limits if the new limit is lower
if (_limits.speed_NE_max >= MPC_XY_CRUISE.get()) {
_limits.speed_NE_max = MPC_XY_CRUISE.get();
}
}
src/lib/FlightTasks/tasks/FlightTaskAuto.hpp
+3 -1
View File
@@ -70,6 +70,8 @@ public:
bool updateInitialize() override;
protected:
void _updateSetpointLimits() override;
matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */
matrix::Vector3f _target{}; /**< Target waypoint (local frame).*/
@@ -84,7 +86,7 @@ private:
uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _mc_cruise_default); /**< Default mc cruise speed.*/
(ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE); /**< Default mc cruise speed.*/
map_projection_reference_s _reference; /**< Reference frame from global to local. */
src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
+19 -19
View File
@@ -126,7 +126,7 @@ void FlightTaskAutoLine::_generateLandSetpoints()
{
// Keep xy-position and go down with landspeed. */
_position_setpoint = Vector3f(_target(0), _target(1), NAN);
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _land_speed.get()));
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, MPC_LAND_SPEED.get()));
}
void FlightTaskAutoLine::_generateTakeoffSetpoints()
@@ -183,7 +183,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
(Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
@@ -206,7 +206,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
(Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
@@ -230,7 +230,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
(Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
@@ -254,7 +254,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
(Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
angle =
Vector2f(&(_destination - _origin)(0)).unit_or_zero()
@@ -269,10 +269,10 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
void FlightTaskAutoLine::_generateXYsetpoints()
{
Vector2f pos_sp_to_dest = Vector2f(&(_target - _position_setpoint)(0));
const bool has_reached_altitude = fabsf(_destination(2) - _position(2)) < _nav_rad.get();
const bool has_reached_altitude = fabsf(_destination(2) - _position(2)) < NAV_ACC_RAD.get();
if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _nav_rad.get()) ||
(!has_reached_altitude && pos_sp_to_dest.length() < _nav_rad.get())) {
if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < NAV_ACC_RAD.get()) ||
(!has_reached_altitude && pos_sp_to_dest.length() < NAV_ACC_RAD.get())) {
// Vehicle reached target in xy and no passing required. Lock position */
_position_setpoint(0) = _destination(0);
@@ -302,9 +302,9 @@ void FlightTaskAutoLine::_generateXYsetpoints()
}
// Compute maximum speed at target threshold */
if (threshold_max > _nav_rad.get()) {
float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _nav_rad.get());
speed_threshold = m * (target_threshold - _nav_rad.get()) + _speed_at_target; // speed at transition
if (threshold_max > NAV_ACC_RAD.get()) {
float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - NAV_ACC_RAD.get());
speed_threshold = m * (target_threshold - NAV_ACC_RAD.get()) + _speed_at_target; // speed at transition
}
// Either accelerate or decelerate
@@ -317,7 +317,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
_speed_at_target = 0.0f;
}
float acceptance_radius = _nav_rad.get();
float acceptance_radius = NAV_ACC_RAD.get();
if (_speed_at_target < 0.01f) {
// If vehicle wants to stop at the target, then set acceptance radius to zero as well.
@@ -355,7 +355,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
}
// If yaw offset is large, only accelerate with 0.5 m/s^2.
float acc_max = (fabsf(yaw_diff) > math::radians(_mis_yaw_error.get())) ? 0.5f : _acc_xy.get();
float acc_max = (fabsf(yaw_diff) > math::radians(MIS_YAW_ERR.get())) ? 0.5f : MPC_ACC_HOR.get();
if (acc_track > acc_max) {
// accelerate towards target
@@ -392,12 +392,12 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
// limit vertical downwards speed (positive z) close to ground
// for now we use the altitude above home and assume that we want to land at same height as we took off
float vel_limit = math::gradual(_alt_above_ground,
_slow_land_alt2.get(), _slow_land_alt1.get(),
_land_speed.get(), _vel_max_down.get());
MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
MPC_LAND_SPEED.get(), _limits.speed_dn_max);
// Speed at threshold is by default maximum speed. Threshold defines
// the point in z at which vehicle slows down to reach target altitude.
float speed_sp = (flying_upward) ? _vel_max_up.get() : vel_limit;
float speed_sp = (flying_upward) ? _limits.speed_up_max : vel_limit;
// Target threshold defines the distance to target(2) at which
// the vehicle starts to slow down to approach the target smoothly.
@@ -426,7 +426,7 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
// we want to accelerate
const float acc = (speed_sp - fabsf(_velocity_setpoint(2))) / _deltatime;
const float acc_max = (flying_upward) ? (_acc_max_up.get() * 0.5f) : (_acc_max_down.get() * 0.5f);
const float acc_max = (flying_upward) ? (MPC_ACC_UP_MAX.get() * 0.5f) : (MPC_ACC_DOWN_MAX.get() * 0.5f);
if (acc > acc_max) {
speed_sp = acc_max * _deltatime + fabsf(_velocity_setpoint(2));
@@ -463,7 +463,7 @@ float FlightTaskAutoLine::_getVelocityFromAngle(const float angle)
// Middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle of 90degrees.
// It needs to be always larger than minimum cruise speed.
float middle_cruise_speed = _cruise_speed_90.get();
float middle_cruise_speed = MPC_CRUISE_90.get();
if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
middle_cruise_speed = min_cruise_speed + SIGMA_NORM;
@@ -514,5 +514,5 @@ void FlightTaskAutoLine::updateParams()
FlightTaskAuto::updateParams();
// make sure that alt1 is above alt2
_slow_land_alt1.set(math::max(_slow_land_alt1.get(), _slow_land_alt2.get()));
MPC_LAND_ALT1.set(math::max(MPC_LAND_ALT1.get(), MPC_LAND_ALT2.get()));
}
src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp
+9 -12
View File
@@ -66,18 +66,15 @@ protected:
State _current_state{State::none};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _land_speed,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _vel_max_up,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _vel_max_down,
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _acc_max_xy,
(ParamFloat<px4::params::MPC_ACC_HOR>) _acc_xy,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _acc_max_up,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _acc_max_down,
(ParamFloat<px4::params::MPC_CRUISE_90>) _cruise_speed_90,
(ParamFloat<px4::params::NAV_ACC_RAD>) _nav_rad,
(ParamFloat<px4::params::MIS_YAW_ERR>) _mis_yaw_error,
(ParamFloat<px4::params::MPC_LAND_ALT1>) _slow_land_alt1,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _slow_land_alt2
(ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED,
(ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees
(ParamFloat<px4::params::NAV_ACC_RAD>) NAV_ACC_RAD, // acceptance radius at which waypoints are updated
(ParamFloat<px4::params::MIS_YAW_ERR>) MIS_YAW_ERR, // yaw-error threshold
(ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed
(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX,
(ParamFloat<px4::params::MPC_ACC_HOR>) MPC_ACC_HOR // acceleration in flight
)
void _generateIdleSetpoints();
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+2 -2
View File
@@ -46,7 +46,7 @@ void FlightTaskManualAltitude::_scaleSticks()
FlightTaskManualStabilized::_scaleSticks();
/* Scale horizontal velocity with expo curve stick input*/
const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get();
const float vel_max_z = (_sticks(2) > 0.0f) ? _limits.speed_dn_max : _limits.speed_up_max;
_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
}
@@ -58,7 +58,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
/* handle position and altitude hold */
const bool apply_brake_z = fabsf(_velocity_setpoint(2)) <= FLT_EPSILON;
const bool stopped_z = (_vel_hold_thr_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _vel_hold_thr_z.get());
const bool stopped_z = (MPC_HOLD_MAX_Z.get() < FLT_EPSILON || fabsf(_velocity(2)) < MPC_HOLD_MAX_Z.get());
if (apply_brake_z && stopped_z && !PX4_ISFINITE(_position_setpoint(2))) {
_position_setpoint(2) = _position(2);
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
+1 -4
View File
@@ -54,10 +54,7 @@ protected:
void _scaleSticks() override; /**< scales sticks to velocity in z */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _vel_max_down, /**< maximum speed allowed to go up */
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _vel_max_up, /**< maximum speed allowed to go down */
(ParamFloat<px4::params::MPC_HOLD_MAX_Z>)
_vel_hold_thr_z /**< velocity threshold to switch back into vertical position hold */
(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z
)
};
src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+12 -3
View File
@@ -55,8 +55,8 @@ void FlightTaskManualPosition::_scaleSticks()
stick_xy = stick_xy.normalized() * mag;
}
/* Scale to velocity.*/
Vector2f vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
// scale velocity to its maximum lmits
Vector2f vel_sp_xy = stick_xy * _limits.speed_NE_max;
/* Rotate setpoint into local frame. */
_rotateIntoHeadingFrame(vel_sp_xy);
@@ -69,7 +69,7 @@ void FlightTaskManualPosition::_updateXYlock()
/* If position lock is not active, position setpoint is set to NAN.*/
const float vel_xy_norm = Vector2f(&_velocity(0)).length();
const bool apply_brake = Vector2f(&_velocity_setpoint(0)).length() < FLT_EPSILON;
const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get());
const bool stopped = (MPC_HOLD_MAX_XY.get() < FLT_EPSILON || vel_xy_norm < MPC_HOLD_MAX_XY.get());
if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint(0) = _position(0);
@@ -88,3 +88,12 @@ void FlightTaskManualPosition::_updateSetpoints()
_thrust_setpoint *= NAN; // don't require any thrust setpoints
_updateXYlock(); // check for position lock
}
void FlightTaskManualPosition::_updateSetpointLimits()
{
FlightTaskManualAltitude::_updateSetpointLimits();
if (_limits.speed_NE_max >= MPC_VEL_MANUAL.get()) {
_limits.speed_NE_max = MPC_VEL_MANUAL.get();
}
}
src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
+4 -6
View File
@@ -53,14 +53,12 @@ protected:
void _updateXYlock(); /**< applies positon lock based on stick and velocity */
void _updateSetpoints() override;
void _scaleSticks() override;
void _updateSetpointLimits() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _vel_xy_manual_max, /**< maximum speed allowed horizontally */
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>)
_acc_xy_max, /**< maximum acceleration horizontally. Only used to compute lock time */
(ParamFloat<px4::params::MPC_HOLD_MAX_XY>)
_vel_hold_thr_xy /**< velocity threshold to switch back into horizontal position hold */
(ParamFloat<px4::params::MPC_VEL_MANUAL>) MPC_VEL_MANUAL,
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) MPC_ACC_HOR_MAX,
(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY
)
private:
};
src/lib/FlightTasks/tasks/FlightTaskSport.hpp
+4 -16
View File
@@ -53,23 +53,11 @@ public:
virtual ~FlightTaskSport() = default;
protected:
void _updateSetpoints() override
void _updateSetpointLimits() override
{
FlightTaskManualPosition::_updateSetpoints(); // get all setpoints from position task
FlightTaskManualPosition::_updateSetpointLimits();
/* Scale horizontal velocity setpoint by maximum allowed velocity. */
if (PX4_ISFINITE(_velocity_setpoint(0)) && Vector2f(&_velocity_setpoint(0)).length() > 0.0f) {
Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
vel_sp_xy = vel_sp_xy.normalized() * _vel_xy_max.get() / _vel_xy_manual_max.get() * vel_sp_xy.length();
_velocity_setpoint(0) = vel_sp_xy(0);
_velocity_setpoint(1) = vel_sp_xy(1);
}
// for sport mode we just increase horizontal speed to maximum speed
_limits.speed_NE_max = MPC_XY_VEL_MAX.get();
}
private:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>)
_vel_xy_max /**< maximal allowed horizontal speed, in sport mode full stick input*/
)
};