forked from Archive/PX4-Autopilot
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:
parent
9ba748e67e
commit
e73218d665
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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. */
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue