diff --git a/src/lib/FlightTasks/tasks/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp index 0fbdb220d4..8b66b03b83 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask.cpp @@ -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(); +} diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 9c09088958..e348e0dd29 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -105,6 +105,35 @@ public: } protected: + + + uORB::Subscription *_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 *_sub_vehicle_local_position{nullptr}; - bool _evaluateVehicleLocalPosition(); + DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, + (ParamFloat) MPC_XY_VEL_MAX, + (ParamFloat) MPC_Z_VEL_MAX_DN, + (ParamFloat) MPC_Z_VEL_MAX_UP + ); }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp index 26e08460c4..d23ab4be2d 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp @@ -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(); + } + +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp index a7fe25f672..876dcc8bf5 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -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 *_sub_triplet_setpoint{nullptr}; DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, - (ParamFloat) _mc_cruise_default); /**< Default mc cruise speed.*/ + (ParamFloat) MPC_XY_CRUISE); /**< Default mc cruise speed.*/ map_projection_reference_s _reference; /**< Reference frame from global to local. */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index 716c37841b..1d3ce1e196 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -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())); } diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp index bf25649a2a..a19ffccf0c 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -66,18 +66,15 @@ protected: State _current_state{State::none}; DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, - (ParamFloat) _land_speed, - (ParamFloat) _vel_max_up, - (ParamFloat) _vel_max_down, - (ParamFloat) _acc_max_xy, - (ParamFloat) _acc_xy, - (ParamFloat) _acc_max_up, - (ParamFloat) _acc_max_down, - (ParamFloat) _cruise_speed_90, - (ParamFloat) _nav_rad, - (ParamFloat) _mis_yaw_error, - (ParamFloat) _slow_land_alt1, - (ParamFloat) _slow_land_alt2 + (ParamFloat) MPC_LAND_SPEED, + (ParamFloat) MPC_CRUISE_90, // speed at corner when angle is 90 degrees + (ParamFloat) NAV_ACC_RAD, // acceptance radius at which waypoints are updated + (ParamFloat) MIS_YAW_ERR, // yaw-error threshold + (ParamFloat) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed + (ParamFloat) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed + (ParamFloat) MPC_ACC_UP_MAX, + (ParamFloat) MPC_ACC_DOWN_MAX, + (ParamFloat) MPC_ACC_HOR // acceleration in flight ) void _generateIdleSetpoints(); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index 855f5e2608..e2d0bab5e8 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -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); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp index 3d21c4bb10..f00d0d7403 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp @@ -54,10 +54,7 @@ protected: void _scaleSticks() override; /**< scales sticks to velocity in z */ DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized, - (ParamFloat) _vel_max_down, /**< maximum speed allowed to go up */ - (ParamFloat) _vel_max_up, /**< maximum speed allowed to go down */ - (ParamFloat) - _vel_hold_thr_z /**< velocity threshold to switch back into vertical position hold */ + (ParamFloat) MPC_HOLD_MAX_Z ) }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp index 361adb2333..1de8a825e4 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp @@ -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(); + } +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp index 1a4e533b72..cbd96eafc9 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp @@ -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) _vel_xy_manual_max, /**< maximum speed allowed horizontally */ - (ParamFloat) - _acc_xy_max, /**< maximum acceleration horizontally. Only used to compute lock time */ - (ParamFloat) - _vel_hold_thr_xy /**< velocity threshold to switch back into horizontal position hold */ + (ParamFloat) MPC_VEL_MANUAL, + (ParamFloat) MPC_ACC_HOR_MAX, + (ParamFloat) MPC_HOLD_MAX_XY ) -private: }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskSport.hpp b/src/lib/FlightTasks/tasks/FlightTaskSport.hpp index 7f77d9145e..b146a2492a 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskSport.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskSport.hpp @@ -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) - _vel_xy_max /**< maximal allowed horizontal speed, in sport mode full stick input*/ - ) - };