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

ekf2 support SET_GPS_GLOBAL_ORIGIN and remove globallocalconverter usage

Browse Source 
- vehicle_command cmd extended from uint16 to support PX4 internal commands that don't map to mavlink
This commit is contained in:
Daniel Agar
2021-03-05 18:25:14 -05:00
committed by GitHub
parent 12a4b0334f
commit 263b00b65f
21 changed files with 212 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/mocap.cpp
+6 -4
View File
@@ -37,9 +37,11 @@ void BlockLocalPositionEstimator::mocapInit()
_sensorFault &= ~SENSOR_MOCAP;
// get reference for global position
globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt);
_global_ref_timestamp = _timeStamp;
_is_global_cov_init = globallocalconverter_initialized();
_ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
_ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
_ref_alt = _global_local_alt0;
_is_global_cov_init = map_projection_initialized(&_global_local_proj_ref);
if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) {
// initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well)
@@ -53,7 +55,7 @@ void BlockLocalPositionEstimator::mocapInit()
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOriginGlobal = true;
_altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f;
_altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f;
}
}
}
src/modules/local_position_estimator/sensors/vision.cpp
+6 -4
View File
@@ -42,9 +42,11 @@ void BlockLocalPositionEstimator::visionInit()
_sensorFault &= ~SENSOR_VISION;
// get reference for global position
globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt);
_global_ref_timestamp = _timeStamp;
_is_global_cov_init = globallocalconverter_initialized();
_ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
_ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
_ref_alt = _global_local_alt0;
_is_global_cov_init = map_projection_initialized(&_global_local_proj_ref);
if (!_map_ref.init_done && _is_global_cov_init) {
// initialize global origin using the visual estimator reference
@@ -58,7 +60,7 @@ void BlockLocalPositionEstimator::visionInit()
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOriginGlobal = true;
_altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f;
_altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f;
}
}
}