From 04fecf33f873709c415cde77cbc5732675340d84 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sun, 4 Aug 2024 13:53:53 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=AA=B4=20Adds=20webserial=20lidar=20suppo?=
 =?UTF-8?q?rt?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/lib/components/Lidar.svelte           | 32 +++----
 app/src/lib/stores/lidar.ts                   | 29 ++++++
 app/src/routes/menu.svelte                    |  7 ++
 app/src/routes/peripherals/lidar/+page.svelte |  7 ++
 app/src/routes/peripherals/lidar/lidar.svelte | 95 +++++++++++++++++++
 5 files changed, 154 insertions(+), 16 deletions(-)
 create mode 100644 app/src/lib/stores/lidar.ts
 create mode 100644 app/src/routes/peripherals/lidar/+page.svelte
 create mode 100644 app/src/routes/peripherals/lidar/lidar.svelte

diff --git a/app/src/lib/components/Lidar.svelte b/app/src/lib/components/Lidar.svelte
index fdeed2a..8f05a4f 100644
--- a/app/src/lib/components/Lidar.svelte
+++ b/app/src/lib/components/Lidar.svelte
@@ -1,5 +1,6 @@
 <script lang="ts">
 	import { onMount } from "svelte";
+    import { lidar, type LidarPoint } from '$lib/stores/lidar'
 
     function getIntersection(angle:number, size:number):number {
             const sinAngle = Math.sin(angle);
@@ -17,39 +18,39 @@
             return Math.sqrt(x**2 + y**2);
         }
 
-    export let points:[number, number][] = new Array(360)
-    .fill(5000) // 5m in mm as that allow us to use uint16 (2 bytes)
-    .map((distance, angle) => [angle, getIntersection(angle, distance)])
 
     let canvas:HTMLCanvasElement
     let ctx
 
+    const DEG2RAD = 0.017453292519943;
 
     onMount(() => {
         ctx = canvas.getContext("2d")
         resize()      
+        lidar.subscribe(lidar => {
+            draw(lidar.points)
+        })
     })
 
-    const draw = () => {
+    const draw = (points:LidarPoint[]) => {
+        if(!points) return
         const centerX = canvas.width / 2
         const centerY = canvas.height / 2
         
-        const scale = 0.05
-
-        const offsetDistance = 200;
-        const offset = offsetDistance * scale;
+        const scale = 0.01//Math.max(centerX, centerY) / Math.max(...points.map((point) => point.distance))
 
         if (!ctx) return
+        ctx.clearRect(0, 0, canvas.width, canvas.height);
         for (let i = 0; i < points.length; i++){
-            const [angle, distance] = points[i]
-            
-            const startX = centerX + offset * Math.cos(angle);
-            const startY = centerY + offset * Math.sin(angle);
-            const endX = centerX + (offset + distance * scale) * Math.cos(angle);
-            const endY = centerY + (offset + distance * scale) * Math.sin(angle);
+            const angle = points[i].angle
+            const distance = points[i].distance
+            const quality = points[i].quality
+
+            const endX = centerX + (distance * scale) * Math.cos(angle * DEG2RAD);
+            const endY = centerY - (distance * scale) * Math.sin(angle * DEG2RAD);
 
             ctx.beginPath();
-            ctx.moveTo(startX, startY);
+            ctx.moveTo(centerX, centerY);
             ctx.lineTo(endX, endY);
             ctx.strokeStyle = "grey"
             ctx.stroke();
@@ -67,7 +68,6 @@
 			canvas.width = parentElement.clientWidth
             canvas.height = parentElement.clientHeight
 		}
-        draw()
     }
 
 </script>
diff --git a/app/src/lib/stores/lidar.ts b/app/src/lib/stores/lidar.ts
new file mode 100644
index 0000000..424f723
--- /dev/null
+++ b/app/src/lib/stores/lidar.ts
@@ -0,0 +1,29 @@
+import { writable } from 'svelte/store';
+
+export type LidarPoint = {
+	distance: number;
+	angle: number;
+	quality: number;
+};
+
+let lidar_data = {
+	points: <LidarPoint[]>[]
+};
+
+const maxLidarData = 600;
+
+function createLidar() {
+	const { subscribe, update } = writable(lidar_data);
+
+	return {
+		subscribe,
+		addData: (lidarPoint: LidarPoint) => {
+			update((lidar_data) => ({
+				...lidar_data,
+				points: [...lidar_data.points, lidarPoint].slice(-maxLidarData)
+			}));
+		}
+	};
+}
+
+export const lidar = createLidar();
diff --git a/app/src/routes/menu.svelte b/app/src/routes/menu.svelte
index 89e305c..42f041e 100644
--- a/app/src/routes/menu.svelte
+++ b/app/src/routes/menu.svelte
@@ -8,6 +8,7 @@
     import Devices from '~icons/mdi/devices'
     import Camera from '~icons/mdi/camera-outline';
     import Rotate3d from '~icons/mdi/rotate-3d';
+    import MdiLandslideOutline from '~icons/mdi/landslide-outline';
     import MotorOutline from '~icons/mdi/motor-outline';
 	import Health from '~icons/mdi/stethoscope';
 	import Folder from '~icons/mdi/folder-outline';
@@ -83,6 +84,12 @@
 					icon: Rotate3d,
 					href: '/peripherals/imu',
 					feature: $page.data.features.imu || $page.data.features.mag || $page.data.features.bmp,
+				},
+                {
+					title: 'Lidar',
+					icon: MdiLandslideOutline,
+					href: '/peripherals/lidar',
+					feature: true//$page.data.features.lidar,
 				}
 			]
 		},
diff --git a/app/src/routes/peripherals/lidar/+page.svelte b/app/src/routes/peripherals/lidar/+page.svelte
new file mode 100644
index 0000000..22c6c5c
--- /dev/null
+++ b/app/src/routes/peripherals/lidar/+page.svelte
@@ -0,0 +1,7 @@
+<script lang="ts">
+	import Lidar from './lidar.svelte';
+</script>
+
+<div class="mx-0 my-1 flex flex-col space-y-4 sm:mx-8 sm:my-8">
+    <Lidar />
+</div>
diff --git a/app/src/routes/peripherals/lidar/lidar.svelte b/app/src/routes/peripherals/lidar/lidar.svelte
new file mode 100644
index 0000000..84ab5c2
--- /dev/null
+++ b/app/src/routes/peripherals/lidar/lidar.svelte
@@ -0,0 +1,95 @@
+<script lang="ts">
+    import Lidar from "$lib/components/Lidar.svelte";
+	import SettingsCard from "$lib/components/SettingsCard.svelte";
+	import { lidar } from "$lib/stores/lidar";
+	import { onMount } from "svelte";
+	import { writable } from "svelte/store";
+	import { distance } from "three/examples/jsm/nodes/Nodes.js";
+
+    let port;
+    let reader;
+    let inputDone;
+    let inputStream;
+    let isConnected = false;
+    let buffer = '';
+    let lastLine = ""
+
+    onMount(() => {
+        navigator.serial.addEventListener("connect", (e) => {
+            console.log("Connected");
+        });
+
+        navigator.serial.addEventListener("disconnect", (e) => {
+            console.log("Disconnected");
+        });
+
+        navigator.serial.getPorts().then((ports) => {
+        // Initialize the list of available ports with `ports` on page load.
+        });
+    })
+
+    const connect = async () => {
+        try {
+            port = await navigator.serial.requestPort();
+            await port.open({ baudRate: 115200 });
+            const decoder = new TextDecoderStream();
+            inputDone = port.readable.pipeTo(decoder.writable);
+            inputStream = decoder.readable.pipeThrough(new TransformStream(new LineBreakTransformer()));
+            reader = inputStream.getReader();
+            readLoop();
+        } catch (err) {
+            console.error('Failed to open serial port:', err);
+        }
+    }
+
+    async function readLoop() {
+        while (true) {
+            const { value, done } = await reader.read();
+            if (done) {
+                console.log('[readLoop] DONE', done);
+                reader.releaseLock();
+                break;
+            }
+            if (value.split(",").length !== 3) continue
+                
+            const [distance, angle, quality] = value.split(",").map((val:string) => parseFloat(val))
+            const lidarData = { distance, angle, quality }
+            
+            if (distance <1000 || distance > 40000 || quality < 40) continue
+            lidar.addData(lidarData)
+        }
+    }
+
+    class LineBreakTransformer {
+		container: string;
+        constructor() {
+            this.container = '';
+        }
+
+        transform(chunk: any, controller: { enqueue: (arg0: any) => any; }) {
+            let re = /\r\n|\n|\r/gm;
+            this.container += chunk;
+            const lines = this.container.split(re);
+            this.container = lines.pop() || "";
+            lines.forEach(line => controller.enqueue(line));
+        }
+
+        flush(controller: { enqueue: (arg0: string) => void; }) {
+        controller.enqueue(this.container);
+        }
+    }
+</script>
+
+<SettingsCard collapsible={false}>
+    <!-- <MdiConnection slot="icon" class="lex-shrink-0 mr-2 h-6 w-6 self-end" /> -->
+    <span slot="title">Lidar</span>
+    <div>
+        <button on:click={connect} class="btn">Connect</button>
+    </div>
+</SettingsCard>
+
+<div class="h-96 w-96">
+    <div class="w-full h-full">
+        <Lidar />
+    </div>
+</div>
\ No newline at end of file