diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 58e5ab4562..51b4b3aee2 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -129,17 +129,6 @@ void FlightTaskAuto::_limitYawRate() } } -bool FlightTaskAuto::_checkTakeoff() { - // position setpoint above the minimum altitude - float min_altitude = 0.2f; - const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; - if (PX4_ISFINITE(min_distance_to_ground)) { - min_altitude = min_distance_to_ground + 0.05f; - } - - return PX4_ISFINITE(_position_setpoint(2)) && _position_setpoint(2) < (_position(2) - min_altitude); -} - bool FlightTaskAuto::_evaluateTriplets() { // TODO: fix the issues mentioned below diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index 316f3851ff..d68c26571a 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -91,7 +91,6 @@ protected: matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/ void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */ - virtual bool _checkTakeoff(); /**< determines when to trigger a takeoff (ignored in flight) */ 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. */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp index 073649d32f..e79e7d66d1 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -6,7 +6,7 @@ constexpr uint64_t FlightTask::_timeout; // First index of empty_setpoint corresponds to time-stamp and requires a finite number. const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}}; -const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {}}; +const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array) @@ -145,3 +145,30 @@ void FlightTask::_setDefaultConstraints() _constraints.max_distance_to_ground = NAN; _constraints.want_takeoff = false; } + +bool FlightTask::_checkTakeoff() +{ + // position setpoint above the minimum altitude + bool position_triggered_takeoff = false; + + if (PX4_ISFINITE(_position_setpoint(2))) { + //minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow + float min_altitude = 0.2f; + const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; + + if (PX4_ISFINITE(min_distance_to_ground)) { + min_altitude = min_distance_to_ground + 0.05f; + } + + position_triggered_takeoff = _position_setpoint(2) < (_position(2) - min_altitude); + } + + // upwards velocity setpoint + bool velocity_triggered_takeoff = false; + + if (PX4_ISFINITE(_velocity_setpoint(2))) { + velocity_triggered_takeoff = _velocity_setpoint(2) < -0.3f; + } + + return position_triggered_takeoff || velocity_triggered_takeoff; +} diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 8106bd7318..19984514b8 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -185,21 +185,18 @@ protected: uORB::Subscription *_sub_attitude{nullptr}; uint8_t _heading_reset_counter{0}; /**< estimator heading reset */ - /** - * Reset all setpoints to NAN - */ + /** Reset all setpoints to NAN */ void _resetSetpoints(); - /** - * Check and update local position - */ + /** Check and update local position */ void _evaluateVehicleLocalPosition(); - /** - * Set constraints to default values - */ + /** Set constraints to default values */ virtual void _setDefaultConstraints(); + /** determines when to trigger a takeoff (ignored in flight) */ + virtual bool _checkTakeoff(); + /* 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 */ diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 96593bfb60..a0ea75424c 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -56,7 +56,7 @@ protected: void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ virtual void _updateSetpoints(); /**< updates all setpoints */ virtual void _scaleSticks(); /**< scales sticks to velocity in z */ - virtual bool _checkTakeoff(); /**< determines when to trigger a takeoff (ignored in flight) */ + bool _checkTakeoff() override; /** * rotates vector into local frame diff --git a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp index 866f49f4e1..8b05f17b47 100644 --- a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp +++ b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp @@ -75,17 +75,16 @@ bool FlightTaskOffboard::activate() bool FlightTaskOffboard::update() { + // reset setpoint for every loop + _resetSetpoints(); + if (!_sub_triplet_setpoint->get().current.valid) { - _resetSetpoints(); + _setDefaultConstraints(); _position_setpoint = _position; return false; } - // reset setpoint for every loop - _resetSetpoints(); - // Yaw / Yaw-speed - if (_sub_triplet_setpoint->get().current.yaw_valid) { // yaw control required _yaw_setpoint = _sub_triplet_setpoint->get().current.yaw; @@ -169,7 +168,6 @@ bool FlightTaskOffboard::update() // 2. position setpoint + velocity setpoint (velocity used as feedforward) // 3. velocity setpoint // 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported - const bool position_ctrl_xy = _sub_triplet_setpoint->get().current.position_valid && _sub_vehicle_local_position->get().xy_valid; const bool position_ctrl_z = _sub_triplet_setpoint->get().current.alt_valid @@ -225,7 +223,6 @@ bool FlightTaskOffboard::update() } // Z-direction - if (feedforward_ctrl_z) { _position_setpoint(2) = _sub_triplet_setpoint->get().current.z; _velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz; @@ -239,12 +236,14 @@ bool FlightTaskOffboard::update() // Acceleration // Note: this is not supported yet and will be mapped to normalized thrust directly. - if (_sub_triplet_setpoint->get().current.acceleration_valid) { _thrust_setpoint(0) = _sub_triplet_setpoint->get().current.a_x; _thrust_setpoint(1) = _sub_triplet_setpoint->get().current.a_y; _thrust_setpoint(2) = _sub_triplet_setpoint->get().current.a_z; } + // use default conditions of upwards position or velocity to take off + _constraints.want_takeoff = _checkTakeoff(); + return true; }