Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 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
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+28 -9
View File
@@ -311,22 +311,41 @@ bool FlightTaskAuto::_evaluateGlobalReference()
// Only update if reference timestamp has changed AND no valid reference altitude // Only update if reference timestamp has changed AND no valid reference altitude
// is available. // is available.
// TODO: this needs to be revisited and needs a more clear implementation // TODO: this needs to be revisited and needs a more clear implementation
if (_sub_vehicle_local_position->get().ref_timestamp != _time_stamp_reference && if (_sub_vehicle_local_position->get().ref_timestamp == _time_stamp_reference && PX4_ISFINITE(_reference_altitude)) {
(_sub_vehicle_local_position->get().z_global && !PX4_ISFINITE(_reference_altitude))) { // don't need to update anything
return true;
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;
} }
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) 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_lat)) { && PX4_ISFINITE(_sub_vehicle_local_position->get().ref_lon)) {
return true; return true;
} else { } else {
// no valid reference
return false; return false;
} }
} }