orb-mcu-util: control gimbal

mcu-util/src/main.rs

@@ -55,6 +55,9 @@ enum SubCommand {
     /// Stress microcontroller by flooding communication channels
     #[clap(action)]
     Stress(StressOpts),
+    /// Control optics: gimbal
+    #[clap(subcommand)]
+    Optics(OpticsOpts),
     /// Control secure element
     #[clap(subcommand)]
     SecureElement(SecureElement),
@@ -123,6 +126,28 @@ pub enum Mcu {
     Security = 0x02,
 }
 
+/// Optics tests options
+#[derive(Parser, Debug)]
+enum OpticsOpts {
+    /// Auto-home the gimbal
+    #[clap(action)]
+    GimbalHome,
+    /// Set gimbal position: --phi and --theta
+    #[clap(action)]
+    GimbalPosition(OpticsPosition),
+}
+
+/// Optics position
+#[derive(Parser, Debug)]
+struct OpticsPosition {
+    /// Move mirror right/left. Angle in millidegrees. Center is 45000.
+    #[clap(short, long)]
+    phi: u32,
+    /// Move mirror up/down. Angle in millidegrees. Center is 90000.
+    #[clap(short, long)]
+    theta: u32,
+}
+
 /// Commands to the secure element
 #[derive(Parser, Debug)]
 enum SecureElement {
@@ -140,28 +165,26 @@ async fn execute(args: Args) -> Result<()> {
             debug!("{:?}", orb_info);
             println!("{:#}", orb_info);
         }
-        SubCommand::Reboot(mcu) => orb.borrow_mut_mcu(mcu).reboot(None).await?,
+        SubCommand::Reboot(mcu) => orb.board_mut(mcu).reboot(None).await?,
         SubCommand::Dump(DumpOpts {
             mcu,
             duration,
             logs_only,
         }) => {
-            orb.borrow_mut_mcu(mcu)
+            orb.board_mut(mcu)
                 .dump(duration.map(Duration::from_secs), logs_only)
                 .await?
         }
         SubCommand::Stress(StressOpts { duration, mcu }) => {
-            orb.borrow_mut_mcu(mcu)
+            orb.board_mut(mcu)
                 .stress_test(duration.map(Duration::from_secs))
                 .await?
         }
         SubCommand::Image(Image::Switch(mcu)) => {
-            orb.borrow_mut_mcu(mcu).switch_images().await?
+            orb.board_mut(mcu).switch_images().await?
         }
         SubCommand::Image(Image::Update(opts)) => {
-            orb.borrow_mut_mcu(opts.mcu)
-                .update_firmware(&opts.path)
-                .await?
+            orb.board_mut(opts.mcu).update_firmware(&opts.path).await?
         }
         SubCommand::HardwareRevision { filename } => {
             let hw_rev = orb.get_revision().await?;
@@ -206,11 +229,17 @@ async fn execute(args: Args) -> Result<()> {
                 }
             }
         }
+        SubCommand::Optics(opts) => match opts {
+            OpticsOpts::GimbalHome => orb.main_board_mut().gimbal_auto_home().await?,
+            OpticsOpts::GimbalPosition(opts) => {
+                orb.main_board_mut()
+                    .gimbal_set_position(opts.phi, opts.theta)
+                    .await?
+            }
+        },
         SubCommand::SecureElement(opts) => match opts {
             SecureElement::PowerCycle => {
-                orb.borrow_mut_sec_board()
-                    .power_cycle_secure_element()
-                    .await?
+                orb.sec_board_mut().power_cycle_secure_element().await?
             }
         },
     }

mcu-util/src/orb/main_board.rs

@@ -98,6 +98,58 @@ impl MainBoard {
             ))
         }
     }
+
+    pub async fn gimbal_auto_home(&mut self) -> Result<()> {
+        match self
+            .isotp_iface
+            .send(McuPayload::ToMain(
+                main_messaging::jetson_to_mcu::Payload::DoHoming(
+                    main_messaging::PerformMirrorHoming {
+                        homing_mode:
+                            main_messaging::perform_mirror_homing::Mode::OneBlockingEnd
+                                as i32,
+                        angle: main_messaging::perform_mirror_homing::Angle::Both
+                            as i32,
+                    },
+                ),
+            ))
+            .await?
+        {
+            CommonAckError::Success => {
+                info!("✅ Gimbal went back home");
+                Ok(())
+            }
+            ack_err => Err(eyre!("Gimbal auto home failed: ack error: {ack_err}")),
+        }
+    }
+
+    pub async fn gimbal_set_position(
+        &mut self,
+        phi_angle_millidegrees: u32,
+        theta_angle_millidegrees: u32,
+    ) -> Result<()> {
+        match self
+            .isotp_iface
+            .send(McuPayload::ToMain(
+                main_messaging::jetson_to_mcu::Payload::MirrorAngle(
+                    main_messaging::MirrorAngle {
+                        horizontal_angle: 0,
+                        vertical_angle: 0,
+                        angle_type: main_messaging::MirrorAngleType::PhiTheta as i32,
+                        phi_angle_millidegrees,
+                        theta_angle_millidegrees,
+                    },
+                ),
+            ))
+            .await?
+        {
+            CommonAckError::Success => {
+                info!("✅ Gimbal position set");
+                Ok(())
+            }
+            ack_err => Err(eyre!("Gimbal set position failed: ack error: {ack_err}")),
+        }
+    }
 }
 
 #[async_trait]

mcu-util/src/orb/mod.rs

@@ -78,14 +78,18 @@ impl Orb {
         ))
     }
 
-    pub fn borrow_mut_mcu(&mut self, mcu: crate::Mcu) -> &mut dyn Board {
+    pub fn board_mut(&mut self, mcu: crate::Mcu) -> &mut dyn Board {
         match mcu {
             crate::Mcu::Main => &mut self.main_board,
             crate::Mcu::Security => &mut self.sec_board,
         }
     }
 
-    pub fn borrow_mut_sec_board(&mut self) -> &mut SecurityBoard {
+    pub fn main_board_mut(&mut self) -> &mut MainBoard {
+        &mut self.main_board
+    }
+
+    pub fn sec_board_mut(&mut self) -> &mut SecurityBoard {
         &mut self.sec_board
     }