FlightTasks: fix takeoff trigger for offboard

This commit is contained in:
Matthias Grob
2019-05-22 22:05:38 +02:00
committed by Lorenz Meier
parent e73218d665
commit 7c7d980cf0
6 changed files with 42 additions and 31 deletions
@@ -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
@@ -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. */