Complex takeoff height pattern

victoryforphil · Nov 18, 2024 · 8cef9c2 · 8cef9c2
1 parent b876543
commit 8cef9c2
Show file tree

Hide file tree

Showing 5 changed files with 152 additions and 91 deletions.
diff --git a/lil-gcs/src/main.rs b/lil-gcs/src/main.rs
@@ -38,9 +38,7 @@ pub struct TCPNodeSubscriber {
 }
 
 impl DataStoreListener for TCPNodeSubscriber {
-    fn on_datapoint(&mut self, datapoint: &victory_data_store::datapoints::Datapoint) {
-
-    }
+    fn on_datapoint(&mut self, datapoint: &victory_data_store::datapoints::Datapoint) {}
 
     fn on_raw_datapoint(&mut self, datapoint: &victory_data_store::datapoints::Datapoint) {
         let topic = datapoint.topic.clone();
@@ -50,11 +48,8 @@ impl DataStoreListener for TCPNodeSubscriber {
         self.update
             .insert(topic.display_name(), format!("{:?}", datapoint.value));
     }
-
-
-    fn on_bucket_update(&mut self, bucket: &victory_data_store::buckets::BucketHandle) {
-
-    }
+
+    fn on_bucket_update(&mut self, bucket: &victory_data_store::buckets::BucketHandle) {}
 }
 
 #[derive(Serialize, Deserialize, Debug)]
@@ -148,17 +143,17 @@ async fn main() {
         .unwrap()
         .setup_sync(sync_config, client_handle);
 
-
-    let mut listener = TCPNodeSubscriber {
+    let listener = TCPNodeSubscriber {
         map: BTreeMap::new(),
         update: BTreeMap::new(),
     };
 
     let listener_handle = Arc::new(Mutex::new(listener));
 
-    datastore.lock().unwrap().add_listener(&topic_key, listener_handle.clone());
-
-
+    let _ = datastore
+        .lock()
+        .unwrap()
+        .add_listener(&topic_key, listener_handle.clone());
 
     let subscriber_handle_clone = listener_handle.clone();
     let tcp_tx_clone = tcp_tx.clone();
@@ -234,16 +229,21 @@ async fn main() {
                             .unwrap();
                     }
                     "TAKEOFF" => {
-                        info!("Takeoff requested");
-                        // Hard coded to 10.0 for now.
-                        let arm_request = QuadTakeoffRequest::new(10.0);
-                        datastore
-                            .add_struct(
-                                &TopicKey::from_str("cmd/takeoff"),
-                                Timepoint::now(),
-                                arm_request,
-                            )
-                            .unwrap();
+                        if params.len() == 1 {
+                            if let Ok(set_val) = params[0].parse::<f32>() {
+                                let mode_req = QuadTakeoffRequest::new(set_val);
+                                datastore
+                                    .add_struct(
+                                        &TopicKey::from_str("cmd/takeoff"),
+                                        Timepoint::now(),
+                                        mode_req,
+                                    )
+                                    .unwrap();
+                                info!("Takeoff requested at {0} feet", set_val);
+                            }
+                        } else {
+                            warn!("Wrong number of commands sent to the MODE command");
+                        }
                     }
                     "LAND" => {
                         info!("Land Requested");
@@ -306,7 +306,7 @@ async fn main() {
             tokio::time::sleep(Duration::from_millis(250)).await;
             {
                 let mut map = subscriber_handle_clone.lock().unwrap();
-                let updates=  map.update.clone();
+                let updates = map.update.clone();
                 map.update.clear();
                 drop(map);
                 if !updates.is_empty() {
@@ -318,7 +318,6 @@ async fn main() {
                         })
                         .collect::<Vec<DataLine>>();
 
-
                     let message = WebMessage {
                         timestamp: get_current_timestamp(),
                         data,
@@ -330,8 +329,6 @@ async fn main() {
                         warn!("Failed to MsgPack the DataStore Map")
                     }
                 }
-
-
             }
         }
     });

diff --git a/lil-interface/src/components/big_number.tsx b/lil-interface/src/components/big_number.tsx
@@ -1,9 +1,22 @@
-export default function BigNumber(props: { val: number, name: string }) {
-  return (
-    <div className='flex flex-col items-center justify-between bg-[#1f1f1f] p-4 rounded-md w-28 flex-1'>
-        <div className='text-green-500 text-xs font-mono w-fit ml-auto mb-2'>+16</div>
-        <div className='font-mono text-3xl'>{props.val}</div>
-        <div className="opacity-60 text-sm font-extralight">{props.name}</div>
-    </div>
-  )
+import useVictoryValue, { MapValue } from "@/hooks/useVictoryValue";
+import { LOS } from "./sidebar";
+
+export default function BigNumber(props: { val: number; name?: string; victory_id: string }) {
+	const [val] = useVictoryValue(props.victory_id);
+
+    const get_val = (map_val: MapValue | undefined) => {
+        if (map_val) {
+            return (val as number).toFixed(2)
+        } else {
+            return (<LOS />)
+        }
+    }
+
+	return (
+		<div className="flex flex-col items-center justify-between bg-[#1f1f1f] p-4 rounded-md w-28 flex-1">
+			<div className="text-green-500 text-xs font-mono w-fit ml-auto mb-2">+16</div>
+			<div className="font-mono text-3xl">{get_val(val)}</div>
+			<div className="opacity-60 text-sm font-extralight">{props.name ?? props.victory_id}</div>
+		</div>
+	);
 }
diff --git a/lil-interface/src/components/charts.tsx b/lil-interface/src/components/charts.tsx
@@ -1,18 +1,26 @@
-import useVictoryValue from '@/hooks/useVictoryValue';
+import useDroneStore from '@/state/drone';
 import { LineChart } from '@mantine/charts';
 import { useEffect, useState } from 'react';
 
-export default function RealtimeLine(props: { element_limit: number }) {
-	const [pose_z] = useVictoryValue('pose/ned/position/z');
+function roundFloat(value: number, decimalPlaces: number): number {
+	const multiplier = Math.pow(10, decimalPlaces);
+	return Math.round(value * multiplier) / multiplier;
+}
+
+export default function RealtimeLine(props: { element_limit: number; title: string; victory_id: string }) {
+	// FULL MAP -- NO REDRAW. KTHX BIY
+	const drone_store = useDroneStore.getState().data;
 
-	const [points, setPoints] = useState<Array<{ z: number; timestamp: number }>>([]);
+	const [points, setPoints] = useState<Array<{ z: number; x: number }>>([]);
 
 	useEffect(() => {
 		const intervalId = setInterval(() => {
-			if (pose_z !== undefined) {
+			const drone_val = drone_store.get(props.victory_id);
+
+			if (drone_val) {
 				setPoints((prevPoints) => {
 					const now = Date.now();
-					const updatedPoints = [...prevPoints, { z: pose_z as number, timestamp: now }];
+					const updatedPoints = [...prevPoints, { z: roundFloat(drone_val as number, 3), x: now }];
 
 					if (updatedPoints.length > props.element_limit) {
 						updatedPoints.shift();
@@ -24,7 +32,7 @@ export default function RealtimeLine(props: { element_limit: number }) {
 				const randomZ = Math.random() * 10;
 				setPoints((prevPoints) => {
 					const now = Date.now();
-					const updatedPoints = [...prevPoints, { z: randomZ, timestamp: now }];
+					const updatedPoints = [...prevPoints, { z: randomZ, x: now }];
 
 					if (updatedPoints.length > props.element_limit) {
 						updatedPoints.shift();
@@ -33,21 +41,21 @@ export default function RealtimeLine(props: { element_limit: number }) {
 					return updatedPoints;
 				});
 			}
-		}, 100); // 0.5 seconds interval
+		}, 100);
 
 		return () => {
 			clearInterval(intervalId);
 		};
-	}, [pose_z, props.element_limit]);
+	}, [drone_store, props.element_limit, props.victory_id]);
 
 	return (
 		<div className="w-[48%] bg-[#1f1f1f] shadow-sm rounded-md">
-			<div className="w-fit p-4">Z index</div>
+			<div className="w-fit p-4 font-mono">{props.victory_id}</div>
 			<hr className="h-px mb-2 bg-gray-200 border-0 dark:bg-zinc-700"></hr>
 			<div>
 				<LineChart
 					h={300}
-					data={points.map((point) => ({ x: point.timestamp, z: point.z }))}
+					data={points}
 					// xAxisLabel="Time"
 					// yAxisLabel="Z Position"
 					series={[{ name: 'z', color: 'indigo.6' }]}
@@ -60,8 +68,7 @@ export default function RealtimeLine(props: { element_limit: number }) {
 					withXAxis={false}
 					withYAxis={false}
 					withDots={false}
-
-                    gridAxis="none"
+					gridAxis="none"
 				/>
 			</div>
 		</div>

diff --git a/lil-interface/src/components/largeView.tsx b/lil-interface/src/components/largeView.tsx
@@ -102,13 +102,20 @@ function DataPage() {
 		}
 	}, [pose_x, pose_y, extents]);
 
+	/*
+
+	const [pose_x] = useVictoryValue('pose/attitude/rpy_radians/x');
+	const [pose_y] = useVictoryValue('pose/attitude/rpy_radians/y');
+	const [pose_z] = useVictoryValue('pose/attitude/rpy_radians/z');
+	*/
+
 	return (
 		<div className="flex h-full w-full justify-between items-center p-2 flex-wrap gap-2">
 			<div className='w-full flex gap-5 justify-center mb-2'>
-				<BigNumber val={15} name={'Altitude'} />
-				<BigNumber val={15} name={'Pose/Y'} />
-				<BigNumber val={15} name={'Pose/X'} />
-				<BigNumber val={15} name={'Battery'} />
+				<BigNumber val={15} victory_id={'pose/attitude/rpy_radians/x'} />
+				<BigNumber val={15} victory_id={'pose/attitude/rpy_radians/x'}/>
+				<BigNumber val={15} victory_id={'pose/attitude/rpy_radians/x'}/>
+				<BigNumber val={15} name={'Battery'} victory_id={'status/battery'}/>
 			</div>
 			<div className="w-[50%]">
 				<ScatterChart
@@ -131,11 +138,9 @@ function DataPage() {
 					]}
 				/>
 			</div>
-			<RealtimeLine element_limit={30} />
-			<RealtimeLine element_limit={45} />
-			<RealtimeLine element_limit={29} />
-			<RealtimeLine element_limit={12} />
-			<RealtimeLine element_limit={30} />
+			<RealtimeLine element_limit={45} title={'NED Z'} victory_id={'pose/ned/position/z'} />
+			<RealtimeLine element_limit={45} title={'NED X'} victory_id={'pose/ned/position/y'} />
+			<RealtimeLine element_limit={45} title={'NED Y'} victory_id={'pose/ned/position/x'} />
 		</div>
 	);
 }