feat: simulate posllh

indriApollo · Dec 30, 2024 · 2d1294c · 2d1294c
1 parent 1e73ade
commit 2d1294c
Show file tree

Hide file tree

Showing 2 changed files with 49 additions and 5 deletions.
diff --git a/UbloxSimulator/Program.cs b/UbloxSimulator/Program.cs
@@ -17,4 +17,4 @@
 var ubx = new UbloxSimulator.UbloxSimulator(portName, 38400);
 
 Console.WriteLine("Running");
-await ubx.Run(cts.Token);
+ubx.Run(cts.Token);
diff --git a/UbloxSimulator/UbloxSimulator.cs b/UbloxSimulator/UbloxSimulator.cs
@@ -41,12 +41,40 @@ public UbloxSimulator(string portName, int baudRate)
         _port.WriteTimeout = 1000;
     }
 
-    public async Task Run(CancellationToken cancellationToken)
+    public void Run(CancellationToken cancellationToken)
     {
         _port.Open();
-
+
+        var configured = false;
         while (!cancellationToken.IsCancellationRequested)
         {
+            if (configured)
+            {
+                byte[] posllh = [
+                    UbxSyncChar1,
+                    UbxSyncChar2,
+                    (byte)UbxClass.UbxNav,
+                    (byte)UbxId.UbxNavPosllh,
+                    28, 0, // len
+                    0, 0, 0, 0, // iTOW
+                    1, 0, 0, 0, // lon
+                    2, 0, 0, 0, // lat
+                    3, 0, 0, 0, // height
+                    4, 0, 0, 0, //hMSL
+                    5, 0, 0, 0, // hAcc
+                    6, 0, 0, 0, // vAcc
+                    0x34, 0x86 // ck
+                ];
+                Write(posllh);
+            }
+            else
+            {
+                var garbage = new byte[128];
+                Random.Shared.NextBytes(garbage);
+                Write(garbage);
+            }
+
+
             byte[] data;
             try
             {
@@ -57,12 +85,28 @@ public async Task Run(CancellationToken cancellationToken)
                 continue;
             }
 
-            byte[] valset = [UbxSyncChar1, UbxSyncChar2, (byte)UbxClass.UbxCfg, (byte)UbxId.UbxCfgValset];
+            byte[] valset = [
+                UbxSyncChar1,
+                UbxSyncChar2,
+                (byte)UbxClass.UbxCfg,
+                (byte)UbxId.UbxCfgValset
+            ];
             if (data[..4].SequenceEqual(valset))
             {
                 Console.WriteLine("got valset");
-                byte[] ack = [UbxSyncChar1, UbxSyncChar2, (byte)UbxClass.UbxAck, 1, 2, 0, (byte)UbxClass.UbxCfg, (byte)UbxId.UbxCfgValset, 0x98, 0xc1];
+                byte[] ack = [
+                    UbxSyncChar1,
+                    UbxSyncChar2,
+                    (byte)UbxClass.UbxAck,
+                    1, // ack
+                    2, 0, // len
+                    (byte)UbxClass.UbxCfg,
+                    (byte)UbxId.UbxCfgValset,
+                    0x98, 0xc1
+                ];
                 Write(ack);
+
+                configured = true;
             }
         }