AP_AHRS: support set_origin and get_origin for EKF 10

Yukong · May 8, 2017 · b6568b1 · b6568b1
1 parent 47b79eb
commit b6568b1
Showing 1 changed file with 11 additions and 2 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -587,6 +587,14 @@ bool AP_AHRS_NavEKF::set_origin(const Location &loc)
     case EKF_TYPE3:
         return ret3;
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL: {
+        struct SITL::sitl_fdm &fdm = _sitl->state;
+        fdm.home = loc;
+        return true;
+    }
+#endif
+
     default:
         return false;
     }
@@ -1411,8 +1419,9 @@ bool AP_AHRS_NavEKF::get_origin(Location &ret) const
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     case EKF_TYPE_SITL:
-        ret = get_home();
-        return ret.lat != 0 || ret.lng != 0;
+        const struct SITL::sitl_fdm &fdm = _sitl->state;
+        ret = fdm.home;
+        return true;
 #endif
     }
 }