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

FlightTask: check if states are valid. if not valid, set them to NAN

Browse Source
This commit is contained in:
Dennis Mannhart 2018-05-24 08:49:18 +02:00 committed by Lorenz Meier
parent e8620708b3
commit 6a7a7d7ff7
1 changed files with 38 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/lib/FlightTasks/tasks/FlightTask.cpp
View File

@ -57,20 +57,55 @@ void FlightTask::_resetSetpoints()
bool FlightTask::_evaluateVehicleLocalPosition()
{
if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {
_position = matrix::Vector3f(&_sub_vehicle_local_position->get().x);
_velocity = matrix::Vector3f(&_sub_vehicle_local_position->get().vx);
// position
if (_sub_vehicle_local_position->get().xy_valid) {
_position(0) = _sub_vehicle_local_position->get().x;
_position(1) = _sub_vehicle_local_position->get().y;
} else {
_position(0) = _position(1) = NAN;
}
if (_sub_vehicle_local_position->get().z_valid) {
_position(2) = _sub_vehicle_local_position->get().z;
} else {
_position(2) = NAN;
}
// velocity
if (_sub_vehicle_local_position->get().v_xy_valid) {
_velocity(0) = _sub_vehicle_local_position->get().vx;
_velocity(1) = _sub_vehicle_local_position->get().vy;
} else {
_velocity(0) = _velocity(1) = NAN;
}
if (_sub_vehicle_local_position->get().v_z_valid) {
_velocity(2) = _sub_vehicle_local_position->get().vz;
} else {
_velocity(2) = NAN;
}
// yaw
_yaw = _sub_vehicle_local_position->get().yaw;
// distance to bottome
_dist_to_bottom = NAN;
if (_sub_vehicle_local_position->get().dist_bottom_valid) {
_dist_to_bottom = _sub_vehicle_local_position->get().dist_bottom;
}
// We don't check here if states are valid or not.
// Validity checks are done in the sub-classes.
return true;
} else {
_resetSetpoints();
_velocity.zero(); /* default velocity is all zero */
return false;
}
}