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.
This commit is contained in:
Matthias Grob 2019-05-13 21:07:05 +02:00 committed by Lorenz Meier
parent 9ba748e67e
commit e73218d665
8 changed files with 47 additions and 47 deletions

View File

@ -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)

View File

@ -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

View File

@ -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. */

View File

@ -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;
}

View File

@ -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 */

View File

@ -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;
}

View File

@ -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

View File

@ -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);
}
}