Skip to content

Commit

Permalink
FlightTasks: fix takeoff trigger for offboard
Browse files Browse the repository at this point in the history
  • Loading branch information
MaEtUgR authored and LorenzMeier committed May 22, 2019
1 parent e73218d commit 7c7d980
Show file tree
Hide file tree
Showing 6 changed files with 42 additions and 31 deletions.
11 changes: 0 additions & 11 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 0 additions & 1 deletion src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,6 @@ class FlightTaskAuto : public FlightTask
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. */
Expand Down
29 changes: 28 additions & 1 deletion src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ constexpr uint64_t FlightTask::_timeout;
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};

const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {}};
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};

bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
Expand Down Expand Up @@ -145,3 +145,30 @@ void FlightTask::_setDefaultConstraints()
_constraints.max_distance_to_ground = NAN;
_constraints.want_takeoff = false;
}

bool FlightTask::_checkTakeoff()
{
// position setpoint above the minimum altitude
bool position_triggered_takeoff = false;

if (PX4_ISFINITE(_position_setpoint(2))) {
//minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow
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;
}

position_triggered_takeoff = _position_setpoint(2) < (_position(2) - min_altitude);
}

// upwards velocity setpoint
bool velocity_triggered_takeoff = false;

if (PX4_ISFINITE(_velocity_setpoint(2))) {
velocity_triggered_takeoff = _velocity_setpoint(2) < -0.3f;
}

return position_triggered_takeoff || velocity_triggered_takeoff;
}
15 changes: 6 additions & 9 deletions src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -185,21 +185,18 @@ class FlightTask : public ModuleParams
uORB::Subscription<vehicle_attitude_s> *_sub_attitude{nullptr};
uint8_t _heading_reset_counter{0}; /**< estimator heading reset */

/**
* Reset all setpoints to NAN
*/
/** Reset all setpoints to NAN */
void _resetSetpoints();

/**
* Check and update local position
*/
/** Check and update local position */
void _evaluateVehicleLocalPosition();

/**
* Set constraints to default values
*/
/** Set constraints to default values */
virtual void _setDefaultConstraints();

/** determines when to trigger a takeoff (ignored in flight) */
virtual bool _checkTakeoff();

/* Time abstraction */
static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
float _time = 0; /**< passed time in seconds since the task was activated */
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ class FlightTaskManualAltitude : public FlightTaskManual
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) */
bool _checkTakeoff() override;

/**
* rotates vector into local frame
Expand Down
15 changes: 7 additions & 8 deletions src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -75,17 +75,16 @@ bool FlightTaskOffboard::activate()

bool FlightTaskOffboard::update()
{
// reset setpoint for every loop
_resetSetpoints();

if (!_sub_triplet_setpoint->get().current.valid) {
_resetSetpoints();
_setDefaultConstraints();
_position_setpoint = _position;
return false;
}

// reset setpoint for every loop
_resetSetpoints();

// Yaw / Yaw-speed

if (_sub_triplet_setpoint->get().current.yaw_valid) {
// yaw control required
_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
Expand Down Expand Up @@ -169,7 +168,6 @@ bool FlightTaskOffboard::update()
// 2. position setpoint + velocity setpoint (velocity used as feedforward)
// 3. velocity setpoint
// 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported

const bool position_ctrl_xy = _sub_triplet_setpoint->get().current.position_valid
&& _sub_vehicle_local_position->get().xy_valid;
const bool position_ctrl_z = _sub_triplet_setpoint->get().current.alt_valid
Expand Down Expand Up @@ -225,7 +223,6 @@ bool FlightTaskOffboard::update()
}

// Z-direction

if (feedforward_ctrl_z) {
_position_setpoint(2) = _sub_triplet_setpoint->get().current.z;
_velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz;
Expand All @@ -239,12 +236,14 @@ bool FlightTaskOffboard::update()

// Acceleration
// Note: this is not supported yet and will be mapped to normalized thrust directly.

if (_sub_triplet_setpoint->get().current.acceleration_valid) {
_thrust_setpoint(0) = _sub_triplet_setpoint->get().current.a_x;
_thrust_setpoint(1) = _sub_triplet_setpoint->get().current.a_y;
_thrust_setpoint(2) = _sub_triplet_setpoint->get().current.a_z;
}

// use default conditions of upwards position or velocity to take off
_constraints.want_takeoff = _checkTakeoff();

return true;
}

0 comments on commit 7c7d980

Please sign in to comment.