fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 14:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTasks: fix takeoff trigger for offboard

Browse Source
This commit is contained in:
Matthias Grob
2019-05-14 07:57:13 +01:00
committed by Lorenz Meier
parent e73218d665
commit 7c7d980cf0
6 changed files with 42 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+28 -1
View File
@@ -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)
@@ -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;
}