From e73218d665a38b8246beed2b4255c209c3dd0ad9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 May 2019 21:07:05 +0200 Subject: [PATCH] mc_pos_control/FlightTasks: trigger takeoff based on task The initial idea of the flight task architecture was that a task can freely set it's setpoints and doesn't have to worry about takeoff and landing. It would just takeoff when it's landed and there's a setpoint to go up and land when it puts a setpoint that pushes into the ground. With the takeoff logic there are some significant interface problems depending on the way a task is implemented: From the setpoint is not high enough to trigger to an unexpected takeoff because of some estimator fluctuation affecting the setpoint. It's easiest to solve this by allowing the task to determine when a takeoff is triggered. If no condition is implemented by default a task is not allowing a takeoff and cannot be used to get the vehicle off the ground. --- msg/vehicle_constraints.msg | 13 +++-- .../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 12 ++++ .../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 1 + .../tasks/FlightTask/FlightTask.cpp | 1 + .../tasks/FlightTask/FlightTask.hpp | 2 +- .../FlightTaskManualAltitude.cpp | 7 +++ .../FlightTaskManualAltitude.hpp | 1 + .../mc_pos_control/mc_pos_control_main.cpp | 57 ++++++------------- 8 files changed, 47 insertions(+), 47 deletions(-) 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); } }