From 7a715e245204523dfe922342fdfdc57f7ca18fb4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= <patrickelectric@gmail.com>
Date: Sun, 23 Feb 2025 10:52:46 -0300
Subject: [PATCH] wip
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
---
 src/lib/drivers/rest/control.rs   | 44 ++++++++++++++++++++++++---
 src/lib/drivers/rest/mod.rs       | 50 +++++++++++++++++++++++++++++++
 src/lib/web/routes/v1/rest/mod.rs |  2 ++
 3 files changed, 92 insertions(+), 4 deletions(-)

diff --git a/src/lib/drivers/rest/control.rs b/src/lib/drivers/rest/control.rs
index b01f231..293ea75 100644
--- a/src/lib/drivers/rest/control.rs
+++ b/src/lib/drivers/rest/control.rs
@@ -1,17 +1,34 @@
 use anyhow::Result;
 use lazy_static::lazy_static;
 use mavlink;
+use serde::Serialize;
 
 use std::collections::HashMap;
 use std::sync::{Arc, Mutex};
 
-#[derive(Debug)]
+#[derive(Clone, Debug, Default, Serialize)]
+pub struct Attitude {
+    pub roll: f32,
+    pub pitch: f32,
+    pub yaw: f32,
+}
+
+#[derive(Clone, Debug, Default, Serialize)]
+pub struct Position {
+    pub latitude: f64,
+    pub longitude: f64,
+    pub altitude: f32,
+}
+
+#[derive(Clone, Debug, Default, Serialize)]
 pub struct Vehicle {
     vehicle_id: u8,
     armed: bool,
+    attitude: Attitude,
+    position: Position,
 }
 
-#[derive(Debug, Default)]
+#[derive(Clone, Debug, Default, Serialize)]
 pub struct Vehicles {
     vehicles: HashMap<u8, Vehicle>,
 }
@@ -38,7 +55,7 @@ impl Vehicle {
         }
 
         // Let's only take care of the vehicle and not the components
-        if header.component_id != 1 {
+        if header.component_id != mavlink::ardupilotmega::MavComponent::MAV_COMP_ID_AUTOPILOT1 as u8 {
             return;
         }
 
@@ -50,6 +67,20 @@ impl Vehicle {
 
                 println!("Vehicle {self:#?}");
             }
+            mavlink::ardupilotmega::MavMessage::ATTITUDE(attitude) => {
+                self.attitude = Attitude {
+                    roll: attitude.roll,
+                    pitch: attitude.pitch,
+                    yaw: attitude.yaw,
+                };
+            }
+            mavlink::ardupilotmega::MavMessage::GLOBAL_POSITION_INT(global_position) => {
+                self.position = Position {
+                    latitude: global_position.lat as f64 / 1e7, //degE7
+                    longitude: global_position.lon as f64 / 1e7, //degE7
+                    altitude: global_position.alt as f32 / 1e3, //mm
+                };
+            }
             _ => {}
         }
     }
@@ -100,7 +131,12 @@ pub fn update((header, message): (mavlink::MavHeader, mavlink::ardupilotmega::Ma
         .entry(header.system_id)
         .or_insert(Vehicle {
             vehicle_id: header.system_id,
-            armed: false,
+            ..Default::default()
         });
     vehicle.update(header, message);
 }
+
+pub fn vehicles() -> Vec<Vehicle> {
+    let mut vehicles = DATA.vehicles.lock().unwrap();
+    vehicles.vehicles.values().cloned().collect()
+}
diff --git a/src/lib/drivers/rest/mod.rs b/src/lib/drivers/rest/mod.rs
index 4a062ff..c2e767d 100644
--- a/src/lib/drivers/rest/mod.rs
+++ b/src/lib/drivers/rest/mod.rs
@@ -110,6 +110,49 @@ impl Rest {
         Ok(())
     }
 
+    #[instrument(level = "debug", skip_all)]
+    async fn controller_receive_task(
+        context: &SendReceiveContext,
+        ws_receiver: &mut broadcast::Receiver<String>,
+    ) -> Result<()> {
+        while let Ok(message) = ws_receiver.recv().await {
+            let Ok(content) =
+                json5::from_str::<MAVLinkJSON<mavlink::ardupilotmega::MavMessage>>(&message)
+            else {
+                warn!("Failed to parse message, not a valid MAVLinkMessage: {message:?}");
+                continue;
+            };
+
+            let bus_message = Arc::new(Protocol::from_mavlink_raw(
+                content.header.inner,
+                &content.message,
+                "Ws",
+            ));
+
+            trace!("Received message: {bus_message:?}");
+
+            context.stats.write().await.stats.update_input(&bus_message);
+
+            for future in context.on_message_input.call_all(bus_message.clone()) {
+                if let Err(error) = future.await {
+                    debug!("Dropping message: on_message_input callback returned error: {error:?}");
+                    continue;
+                }
+            }
+
+            if let Err(error) = context.hub_sender.send(bus_message) {
+                error!("Failed to send message to hub: {error:?}");
+                continue;
+            }
+
+            trace!("Message sent to hub");
+        }
+
+        debug!("Driver receiver task stopped!");
+
+        Ok(())
+    }
+
     #[instrument(level = "debug", skip_all)]
     async fn send_task(context: &SendReceiveContext) -> Result<()> {
         let mut hub_receiver = context.hub_sender.subscribe();
@@ -201,6 +244,13 @@ impl Driver for Rest {
                         error!("Error in rest receive task: {e:?}");
                     }
                 }
+                /*
+                result = Rest::controller_receive_task(&context) => {
+                    if let Err(e) = result {
+                        error!("Error in controller receive task: {e:?}");
+                    }
+                }
+                */
             }
         }
     }
diff --git a/src/lib/web/routes/v1/rest/mod.rs b/src/lib/web/routes/v1/rest/mod.rs
index 6802e36..c0ea632 100644
--- a/src/lib/web/routes/v1/rest/mod.rs
+++ b/src/lib/web/routes/v1/rest/mod.rs
@@ -1,4 +1,5 @@
 pub mod mavlink;
+pub mod vehicles;
 pub mod websocket;
 
 use axum::{routing::get, Router};
@@ -18,4 +19,5 @@ pub fn router() -> Router {
             "/mavlink/message_id_from_name/{*name}",
             get(mavlink::message_id_from_name),
         )
+        .route("/vehicles", get(vehicles::vehicles))
 }