diff --git a/msg/vehicle_constraints.msg b/msg/vehicle_constraints.msg index 50a145cf9a..da8c39aa24 100644 --- a/msg/vehicle_constraints.msg +++ b/msg/vehicle_constraints.msg @@ -1,12 +1,13 @@ # Local setpoint constraints in NED frame # setting something to NaN means that no limit is provided -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -float32 yawspeed # in radians/sec -float32 speed_xy # in meters/sec -float32 speed_up # in meters/sec -float32 speed_down # in meters/sec -float32 tilt # in radians [0, PI] +float32 yawspeed # in radians/sec +float32 speed_xy # in meters/sec +float32 speed_up # in meters/sec +float32 speed_down # in meters/sec +float32 tilt # in radians [0, PI] float32 min_distance_to_ground # in meters float32 max_distance_to_ground # in meters +bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 8e08a51990..58e5ab4562 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -106,6 +106,7 @@ bool FlightTaskAuto::updateFinalize() // If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value // it will see its setpoint constrained here _limitYawRate(); + _constraints.want_takeoff = _checkTakeoff(); return true; } @@ -128,6 +129,17 @@ 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 d68c26571a..316f3851ff 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -91,6 +91,7 @@ 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 4517644d10..073649d32f 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -143,4 +143,5 @@ void FlightTask::_setDefaultConstraints() _constraints.tilt = math::radians(_param_mpc_tiltmax_air.get()); _constraints.min_distance_to_ground = NAN; _constraints.max_distance_to_ground = NAN; + _constraints.want_takeoff = false; } diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 38fe00c701..8106bd7318 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -198,7 +198,7 @@ protected: /** * Set constraints to default values */ - virtual void _setDefaultConstraints(); + virtual void _setDefaultConstraints(); /* Time abstraction */ static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */ diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index deae1751d5..5fdd58ce3b 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -345,10 +345,17 @@ void FlightTaskManualAltitude::_updateSetpoints() _respectGroundSlowdown(); } +bool FlightTaskManualAltitude::_checkTakeoff() +{ + // stick is deflected above the middle 15% of the range + return _sticks(2) < -0.3f; +} + bool FlightTaskManualAltitude::update() { _scaleSticks(); _updateSetpoints(); + _constraints.want_takeoff = _checkTakeoff(); return true; } diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 2993f4d38f..96593bfb60 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -56,6 +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) */ /** * rotates vector into local frame diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4b33d4a4a6..2d1ef1ed30 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -232,12 +232,11 @@ private: /** * Update smooth takeoff vertical velocity ramp to bring the vehicle off the ground without step - * @param z_sp position setpoint in the z-Direction * @param vz_sp velocity setpoint in the z-Direction - * @param jz_sp jerk setpoint in the z-Direction - * @param min_ground_clearance minimal distance to the ground in which e.g. optical flow works correctly + * @param z_sp position setpoint in the z-Direction + * @param vz_constraint velocity to ramp to when there's only a position setpoint */ - void update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground); + void update_takeoff_ramp(const bool want_takeoff, const float vz_sp, const float z_sp, const float vz_constraint); /** * Adjust the setpoint during landing. @@ -647,7 +646,7 @@ MulticopterPositionControl::run() // do smooth takeoff after delay if there's a valid vertical velocity or position if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) { - update_takeoff_ramp(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground); + update_takeoff_ramp(constraints.want_takeoff, setpoint.vz, setpoint.z, constraints.speed_up); } // disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards @@ -978,62 +977,40 @@ MulticopterPositionControl::start_flight_task() } void -MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground) +MulticopterPositionControl::update_takeoff_ramp(const bool want_takeoff, const float vz_sp, const float z_sp, const float vz_constraint) { const float takeoff_ramp_initialization = -0.7f; // check if takeoff is triggered - if (_vehicle_land_detected.landed && !_in_takeoff_ramp) { - // vehicle is still landed and no takeoff was initiated yet, check if takeoff is triggered - // minimum takeoff altitude either 20cm or if valid above minimum altitude specified by flight task/estimator - float min_altitude = 0.2f; - if (PX4_ISFINITE(min_distance_to_ground)) { - min_altitude = min_distance_to_ground + 0.05f; - } - - // upwards velocity setpoint larger than a minimal speed - const bool velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.3f; - // upwards jerk setpoint - const bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON; - // position setpoint above the minimum altitude - _position_triggered_takeoff = PX4_ISFINITE(z_sp) && (z_sp < (_states.position(2) - min_altitude)); - - if (velocity_triggered_takeoff || jerk_triggered_takeoff || _position_triggered_takeoff) { - // takeoff was triggered, start velocity ramp - _takeoff_ramp_velocity = takeoff_ramp_initialization; - _in_takeoff_ramp = true; - _takeoff_reference_z = _states.position(2); - } + if (_vehicle_land_detected.landed && !_in_takeoff_ramp && want_takeoff) { + // takeoff was triggered, start velocity ramp + _takeoff_ramp_velocity = takeoff_ramp_initialization; + _in_takeoff_ramp = true; + _takeoff_reference_z = _states.position(2); } // if in smooth takeoff limit upwards velocity setpoint to a smooth ramp if (_in_takeoff_ramp) { float takeoff_desired_velocity = -vz_sp; - // if takeoff was triggered by a position setpoint, ramp to the takeoff speed parameter - if (_position_triggered_takeoff) { - takeoff_desired_velocity = _param_mpc_tko_speed.get(); + // if there's only a position setpoint we go up with the configured takeoff speed + if (!PX4_ISFINITE(vz_sp) && PX4_ISFINITE(z_sp)) { + takeoff_desired_velocity = vz_constraint; } - // ramp up vrtical velocity in TKO_RAMP_T seconds + if (_param_mpc_tko_ramp_t.get() > _dt) { _takeoff_ramp_velocity += (takeoff_desired_velocity - takeoff_ramp_initialization) * _dt / _param_mpc_tko_ramp_t.get(); - } else { - // no ramp, directly go to desired takeoff speed + // no ramp time, directly jump to desired velocity _takeoff_ramp_velocity = takeoff_desired_velocity; } // make sure we cannot overshoot the desired setpoint with the ramp _takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, takeoff_desired_velocity); - // smooth takeoff is achieved once desired altitude/velocity setpoint is reached - if (_position_triggered_takeoff) { - _in_takeoff_ramp = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get()); - - } else { - _in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp); - } + // smooth takeoff is finished once desired velocity setpoint is reached + _in_takeoff_ramp = (_takeoff_ramp_velocity < takeoff_desired_velocity); } }