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

FlightTaskAuto: set reference to 0 if auto is requested but no global position available

Browse Source
This commit is contained in:
Dennis Mannhart 2018-08-13 09:24:21 +02:00 committed by Lorenz Meier
parent 3222f583c8
commit 16ca0be4e5
1 changed files with 28 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
View File

@ -311,22 +311,41 @@ bool FlightTaskAuto::_evaluateGlobalReference()
// Only update if reference timestamp has changed AND no valid reference altitude
// is available.
// TODO: this needs to be revisited and needs a more clear implementation
if (_sub_vehicle_local_position->get().ref_timestamp != _time_stamp_reference &&
(_sub_vehicle_local_position->get().z_global && !PX4_ISFINITE(_reference_altitude))) {
map_projection_init(&_reference_position,
_sub_vehicle_local_position->get().ref_lat,
_sub_vehicle_local_position->get().ref_lon);
_reference_altitude = _sub_vehicle_local_position->get().ref_alt;
_time_stamp_reference = _sub_vehicle_local_position->get().ref_timestamp;
if (_sub_vehicle_local_position->get().ref_timestamp == _time_stamp_reference && PX4_ISFINITE(_reference_altitude)) {
// don't need to update anything
return true;
}
double ref_lat = _sub_vehicle_local_position->get().ref_lat;
double ref_lon = _sub_vehicle_local_position->get().ref_lon;
_reference_altitude = _sub_vehicle_local_position->get().ref_alt;
if (!_sub_vehicle_local_position->get().z_global) {
// we have no valid global altitude
// set global reference to local reference
_reference_altitude = 0.0f;
}
if (!_sub_vehicle_local_position->get().xy_global) {
// we have no valid global alt/lat
// set global reference to local reference
ref_lat = 0.0;
ref_lon = 0.0;
}
// init projection
map_projection_init(&_reference_position,
ref_lat,
ref_lon);
// check if everything is still finite
if (PX4_ISFINITE(_reference_altitude)
&& PX4_ISFINITE(_sub_vehicle_local_position->get().ref_lat)
&& PX4_ISFINITE(_sub_vehicle_local_position->get().ref_lat)) {
&& PX4_ISFINITE(_sub_vehicle_local_position->get().ref_lon)) {
return true;
} else {
// no valid reference
return false;
}
}