adds initial rcs implementation

ground/src/command.rs

@@ -2,9 +2,13 @@ use anyhow::bail;
 use api::client::Client;
 use api::client::command::CommandRegistry;
 use api::macros::IntoCommandDefinition;
-use chrono::{DateTime, Utc};
+use chrono::{DateTime, TimeDelta, Utc};
 use log::{error, trace};
-use nautilus_common::command::{Command, OwnedCommandHeader, SetPin, ValidPriorityCommand};
+use nautilus_common::command::set_pin::SetPin;
+use nautilus_common::command::set_rcs::SetRcs;
+use nautilus_common::command::valid_priority_command::ValidPriorityCommand;
+use nautilus_common::command::{Command, OwnedCommandHeader};
+use nautilus_common::math::Vector;
 use nautilus_common::udp::tokio::AsyncUdpSocketExt;
 use std::fmt::Debug;
 use std::net::SocketAddr;
@@ -29,6 +33,32 @@ struct SetPinCommand {
     index: u8,
     state: bool,
 }
+impl From<SetPinCommand> for SetPin {
+    fn from(value: SetPinCommand) -> Self {
+        Self {
+            pin: value.index,
+            value: value.state,
+        }
+    }
+}
+
+#[derive(IntoCommandDefinition)]
+struct RcsCommand {
+    translate_x: f64,
+    translate_y: f64,
+    translate_z: f64,
+    rotate_x: f64,
+    rotate_y: f64,
+    rotate_z: f64,
+}
+impl From<RcsCommand> for SetRcs {
+    fn from(value: RcsCommand) -> Self {
+        Self {
+            translation: Vector::new(value.translate_x, value.translate_y, value.translate_z),
+            rotation: Vector::new(value.rotate_x, value.rotate_y, value.rotate_z),
+        }
+    }
+}
 
 impl<'a> CommandHandler<'a> {
     pub fn new(
@@ -69,11 +99,8 @@ impl<'a> CommandHandler<'a> {
                             outgoing_commands_tx.try_send_command(
                                 &command_name,
                                 &ValidPriorityCommand {
-                                    inner: SetPin {
-                                        pin: cmd.index,
-                                        value: cmd.state,
-                                    },
-                                    valid_until: MAX_DATETIME, // header.timestamp + TimeDelta::seconds(5),
+                                    inner: SetPin::from(cmd),
+                                    valid_until: MAX_DATETIME,
                                     priority: 0,
                                 }
                             )?;
@@ -84,13 +111,37 @@ impl<'a> CommandHandler<'a> {
                 })
                 .collect::<Vec<_>>();
 
-            commands.push(self.cmd.register_handler("shutdown", move |_, ()| -> anyhow::Result<_> {
-                trace!("Shutting Down Flight");
+            {
+                let outgoing_commands_tx = outgoing_commands_tx.clone();
+                commands.push(self.cmd.register_handler("shutdown", move |_, ()| -> anyhow::Result<_> {
+                    trace!("Shutting Down Flight");
 
-                outgoing_commands_tx.try_send_command("/shutdown", &())?;
+                    outgoing_commands_tx.try_send_command("/shutdown", &())?;
 
-                Ok("Command Executed Successfully".to_string())
-            }));
+                    Ok("Command Executed Successfully".to_string())
+                }));
+            }
+
+            {
+                let outgoing_commands_tx = outgoing_commands_tx.clone();
+                commands.push(self.cmd.register_handler("rcs.set", move |header, cmd: RcsCommand| -> anyhow::Result<_> {
+                    trace!("Sending Rcs Set Command");
+
+                    outgoing_commands_tx.try_send_command(
+                        "/rcs/set",
+                        &ValidPriorityCommand {
+                            inner: SetRcs::from(cmd),
+                            valid_until: header.timestamp + TimeDelta::seconds(5),
+                            priority: 0,
+                        }
+                    )?;
+
+                    Ok("Command Executed Successfully".to_string())
+                }));
+            }
+
+            // We no longer need this
+            drop(outgoing_commands_tx);
 
             let mut buffer = [0u8; 512];
             while !self.cancel.is_cancelled() {