fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 14:20: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/BlockLocalPositionEstimator.cpp
+17 -1
View File
@@ -106,7 +106,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// local to global coversion related variables
_is_global_cov_init(false),
_global_ref_timestamp(0.0),
_ref_lat(0.0),
_ref_lon(0.0),
_ref_alt(0.0)
@@ -168,6 +167,23 @@ void BlockLocalPositionEstimator::Run()
return;
}
if (_vehicle_command_sub.updated()) {
vehicle_command_s vehicle_command;
if (_vehicle_command_sub.update(&vehicle_command)) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
const double latitude = vehicle_command.param5;
const double longitude = vehicle_command.param6;
const float altitude = vehicle_command.param7;
map_projection_init_timestamped(&_global_local_proj_ref, latitude, longitude, vehicle_command.timestamp);
_global_local_alt0 = altitude;
PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
}
}
}
sensor_combined_s imu;
if (!_sensors_sub.update(&imu)) {