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

FlightTaskAuto: valid target is required and valid reference

Browse Source
This commit is contained in:
Dennis Mannhart
2018-05-24 08:52:43 +02:00
committed by Lorenz Meier
parent 4713f47668
commit bd2de0e585
2 changed files with 23 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
+22 -15
View File
@@ -62,23 +62,21 @@ bool FlightTaskAuto::activate()
bool ret = FlightTask::activate();
_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
_setDefaultConstraints();
// need a valid position and velocity
ret = ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
&& PX4_ISFINITE(_position(2))
&& PX4_ISFINITE(_velocity(0))
&& PX4_ISFINITE(_velocity(1))
&& PX4_ISFINITE(_velocity(2));
return ret;
}
bool FlightTaskAuto::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
_evaluateVehicleGlobalPosition();
return (ret && _evaluateTriplets());
// require valid reference and valid target
ret = ret && _evaluateGlobalReference() && _evaluateTriplets();
// require valid position / velocity in xy
ret = ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
&& PX4_ISFINITE(_velocity(0))
&& PX4_ISFINITE(_velocity(1));
return ret;
}
bool FlightTaskAuto::_evaluateTriplets()
@@ -95,8 +93,8 @@ bool FlightTaskAuto::_evaluateTriplets()
// such as land and takeoff. The navigator should use for auto takeoff/land with flow the position in xy at the moment the
// takeoff/land was initiated. Until then we do this kind of logic here.
if (!_sub_triplet_setpoint->get().current.valid) {
// best we can do is to just set all waypoints to current state
if (!_sub_triplet_setpoint->get().current.valid && !_isFinite(_sub_triplet_setpoint->get().current)) {
// best we can do is to just set all waypoints to current state and return false
_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
_type = WaypointType::position;
return false;
@@ -118,7 +116,7 @@ bool FlightTaskAuto::_evaluateTriplets()
_sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &target(0), &target(1));
target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude);
// check if target is valid
_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
if (_type == WaypointType::follow_target && _sub_triplet_setpoint->get().current.yawspeed_valid) {
@@ -181,7 +179,7 @@ bool FlightTaskAuto::_isFinite(const position_setpoint_s sp)
return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt));
}
void FlightTaskAuto::_evaluateVehicleGlobalPosition()
bool FlightTaskAuto::_evaluateGlobalReference()
{
// check if reference has changed and update.
// Only update if reference timestamp has changed AND no valid reference altitude
@@ -196,6 +194,15 @@ void FlightTaskAuto::_evaluateVehicleGlobalPosition()
_reference_altitude = _sub_vehicle_local_position->get().ref_alt;
_time_stamp_reference = _sub_vehicle_local_position->get().ref_timestamp;
}
if (PX4_ISFINITE(_reference_altitude)
&& PX4_ISFINITE(_sub_vehicle_local_position->get().ref_lat)
&& PX4_ISFINITE(_sub_vehicle_local_position->get().ref_lat)) {
return true;
} else {
return false;
}
}
void FlightTaskAuto::_setDefaultConstraints()