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() {

ground/src/telemetry.rs

@@ -1,7 +1,7 @@
 use api::client::Client;
 use api::client::telemetry::{TelemetryHandle, TelemetryRegistry};
-use futures::TryFutureExt;
-use futures::future::join_all;
+use futures::future::{Either, join_all, select};
+use futures::{TryFutureExt, pin_mut};
 use log::{error, trace};
 use nautilus_common::on_drop::on_drop;
 use nautilus_common::telemetry::{CommsState, SwitchBank, Telemetry, TelemetryMessage};
@@ -12,7 +12,7 @@ use std::sync::Arc;
 use tokio::net::UdpSocket;
 use tokio::sync::{RwLock, RwLockWriteGuard};
 use tokio::task::yield_now;
-use tokio::{join, pin, select, try_join};
+use tokio::{join, select, try_join};
 use tokio_util::sync::CancellationToken;
 
 pub struct TelemetryHandler<'a> {
@@ -52,7 +52,18 @@ impl<'a> TelemetryHandler<'a> {
             .build()?;
 
         runtime.block_on(async {
-            let mut context = TelemetryContext::new(&self).await;
+            // Allow cancellation while waiting for this to initialize
+            let mut context = {
+                let context = TelemetryContext::new(&self);
+                pin_mut!(context);
+                let cancellation_future = self.cancel.cancelled();
+                pin_mut!(cancellation_future);
+                let init_or_cancel = select(context, cancellation_future).await;
+                match init_or_cancel {
+                    Either::Left((success, _)) => success,
+                    Either::Right(_) => return Ok(()),
+                }
+            };
 
             let mut buffer = [0u8; 512];
 
@@ -75,7 +86,7 @@ impl<'a> TelemetryHandler<'a> {
                                     // Update the value in the lock
                                     *lock = addr;
                                 };
-                                pin!(flight_addr_update);
+                                tokio::pin!(flight_addr_update);
 
                                 // We can do these two operations concurrently
                                 join!(