update library and deal with manual pose setting

TeamCode/build.gradle

@@ -36,8 +36,8 @@ dependencies {
     implementation project(':FtcRobotController')
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
 
-    implementation 'com.github.jdhs-ftc:road-runner-ftc-otos:master-SNAPSHOT'
+    implementation 'com.github.jdhs-ftc:road-runner-ftc-otos:5f1b6c5a05'
     implementation "com.acmerobotics.roadrunner:core:1.0.0-beta8"
     implementation "com.acmerobotics.roadrunner:actions:1.0.0-beta8"
-    implementation "com.acmerobotics.dashboard:dashboard:0.4.14"
+    implementation "com.acmerobotics.dashboard:dashboard:0.4.15"
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SparkFunOTOSDrive.java

@@ -3,6 +3,7 @@
 
 
 import static com.acmerobotics.roadrunner.ftc.OTOSKt.OTOSPoseToRRPose;
+import static com.acmerobotics.roadrunner.ftc.OTOSKt.RRPoseToOTOSPose;
 
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.PoseVelocity2d;
@@ -58,6 +59,7 @@ public static class Params {
 
     public static SparkFunOTOSDrive.Params PARAMS = new SparkFunOTOSDrive.Params();
     public SparkFunOTOS otos;
+    private Pose2d lastOtosPose = pose;
 
     public SparkFunOTOSDrive(HardwareMap hardwareMap, Pose2d pose) {
         super(hardwareMap, pose);
@@ -69,6 +71,8 @@ public SparkFunOTOSDrive(HardwareMap hardwareMap, Pose2d pose) {
         otos.setOffset(PARAMS.offset);
         otos.setLinearScalar(PARAMS.linearScalar);
         otos.setAngularScalar(PARAMS.angularScalar);
+
+        otos.setPosition(RRPoseToOTOSPose(pose));
         // The IMU on the OTOS includes a gyroscope and accelerometer, which could
         // have an offset. Note that as of firmware version 1.0, the calibration
         // will be lost after a power cycle; the OTOS performs a quick calibration
@@ -86,6 +90,16 @@ public SparkFunOTOSDrive(HardwareMap hardwareMap, Pose2d pose) {
     }
     @Override
     public PoseVelocity2d updatePoseEstimate() {
+        if (lastOtosPose != pose) {
+            // rr localizer note:
+            // something other then this function has modified pose
+            // probably the user
+            // so we override otos pose with the new pose
+            // this could potentially cause up to 1 loops worth of drift
+            // I don't really like this solution at all, but it preserves compatibility
+            // the only alternative is to add getter and setters but that breaks compat
+            otos.setPosition(RRPoseToOTOSPose(pose));
+        }
         SparkFunOTOS.Pose2D otosPose = otos.getPosition();
         SparkFunOTOS.Pose2D otosVel = otos.getVelocity();
         pose = OTOSPoseToRRPose(otosPose);