Store the last pose on update (#1207)

* Store the last pose on update * Don't clear lastPose if pose isn't calculated --------- Co-authored-by: Mohammad Durrani <[email protected]>
PhotonVision · Feb 5, 2024 · 0eb0a4e · 0eb0a4e
1 parent 7666f15
commit 0eb0a4e
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -408,8 +408,8 @@ private Optional<EstimatedRobotPose> update(
                 return Optional.empty();
         }
 
-        if (estimatedPose.isEmpty()) {
-            lastPose = null;
+        if (estimatedPose.isPresent()) {
+            lastPose = estimatedPose.get().estimatedPose;
         }
 
         return estimatedPose;

diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
@@ -186,6 +186,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
       ret = std::nullopt;
   }
 
+  if (ret) {
+    lastPose = ret.value().estimatedPose;
+  }
   return ret;
 }