fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 14:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: setEkfGlobalOrigin() reset baro and hgt sensor offsets if necessary

Browse Source 
- handle uninitalized _gps_alt_ref
 - add basic lat/lon/alt sanity checks
This commit is contained in:
Daniel Agar
2022-06-16 00:59:54 -04:00
committed by GitHub
parent fc3d88bb67
commit 1980b5c5e8
5 changed files with 71 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+11 -9
View File
@@ -343,18 +343,20 @@ void EKF2::Run()
if (_vehicle_command_sub.update(&vehicle_command)) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
if (!_ekf.control_status_flags().in_air) {
uint64_t origin_time {};
double latitude = vehicle_command.param5;
double longitude = vehicle_command.param6;
float altitude = vehicle_command.param7;
_ekf.setEkfGlobalOrigin(latitude, longitude, altitude);
double latitude = vehicle_command.param5;
double longitude = vehicle_command.param6;
float altitude = vehicle_command.param7;
if (_ekf.setEkfGlobalOrigin(latitude, longitude, altitude)) {
// Validate the ekf origin status.
uint64_t origin_time {};
_ekf.getEkfGlobalOrigin(origin_time, latitude, longitude, altitude);
PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
PX4_INFO("%d - New NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
_instance, latitude, longitude, static_cast<double>(altitude));
} else {
PX4_ERR("%d - Failed to set new NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
_instance, latitude, longitude, static_cast<double>(altitude));
}
}
}