diff --git a/services/com/src/api.rs b/services/com/src/api.rs index 8a35ae1b0..36b7afd2d 100644 --- a/services/com/src/api.rs +++ b/services/com/src/api.rs @@ -55,8 +55,8 @@ pub(crate) enum Opcode { /// Turn Boost Mode Off BoostOff, - /// Read the current accelerations off the IMU - //ImuAccelRead, + /// Read the current accelerations off the IMU; this blocks while the read takes place + ImuAccelReadBlocking, /// Power off the SoC PowerOffSoc, diff --git a/services/com/src/lib.rs b/services/com/src/lib.rs index 0d9bf0dcc..83c40359f 100644 --- a/services/com/src/lib.rs +++ b/services/com/src/lib.rs @@ -220,6 +220,22 @@ impl Com { Message::new_scalar(Opcode::RequestCharging.to_usize().unwrap(), 0, 0, 0, 0 )).map(|_| ()) } + + pub fn gyro_read_blocking(&self) -> Result<(u16, u16, u16, u16), xous::Error> { + if let xous::Result::Scalar2(x_y, z_id) = + send_message(self.conn, + Message::new_blocking_scalar(Opcode::ImuAccelReadBlocking.to_usize().unwrap(), 0, 0, 0, 0)).unwrap() { + + let x = (x_y >> 16) as u16; + let y = (x_y & 0xffff) as u16; + let z = (z_id >> 16) as u16; + let id = (z_id & 0xffff) as u16; + Ok((x, y, z, id)) + } else { + Err(xous::Error::InternalError) + } + } + // note to future self: add other event listener registrations (such as network events) here } diff --git a/services/com/src/main.rs b/services/com/src/main.rs index a15a719fb..648a6cda6 100644 --- a/services/com/src/main.rs +++ b/services/com/src/main.rs @@ -384,6 +384,18 @@ fn xmain() -> ! { bl_sec = secondary; com.txrx(ComState::BL_START.verb | (main as u16) & 0x1f | (((secondary as u16) & 0x1f) << 5)); }), + Some(Opcode::ImuAccelReadBlocking) => msg_blocking_scalar_unpack!(msg, _, _, _, _, { + com.txrx(ComState::GYRO_UPDATE.verb); + com.txrx(ComState::GYRO_READ.verb); + let x = com.wait_txrx(ComState::LINK_READ.verb, Some(STD_TIMEOUT)); + let y = com.wait_txrx(ComState::LINK_READ.verb, Some(STD_TIMEOUT)); + let z = com.wait_txrx(ComState::LINK_READ.verb, Some(STD_TIMEOUT)); + let id = com.wait_txrx(ComState::LINK_READ.verb, Some(STD_TIMEOUT)); + xous::return_scalar2(msg.sender, + ((x as usize) << 16) | y as usize, + ((z as usize) << 16) | id as usize + ).expect("coludn't return acceleromotere read data"); + }), Some(Opcode::BattStats) => { info!("batt stats request received"); let stats = com.get_battstats(); diff --git a/services/shellchat/src/cmds.rs b/services/shellchat/src/cmds.rs index a7c639d1f..6c088209e 100644 --- a/services/shellchat/src/cmds.rs +++ b/services/shellchat/src/cmds.rs @@ -87,6 +87,7 @@ mod ssid; use ssid::*; mod ver; use ver::*; mod audio; use audio::*; mod backlight; use backlight::*; +mod accel; use accel::*; mod fcc; use fcc::*; mod pds; // dependency of the FCC file @@ -142,6 +143,7 @@ impl CmdEnv { let mut echo_cmd = Echo {}; // this command has no persistent storage, so we can "create" it every time we call dispatch (but it's a zero-cost absraction so this doesn't actually create any instructions) let mut ver_cmd = Ver{}; let mut backlight_cmd = Backlight{}; + let mut accel_cmd = Accel{}; let commands: &mut [& mut dyn ShellCmdApi] = &mut [ ///// 4. add your command to this array, so that it can be looked up and dispatched &mut echo_cmd, @@ -155,6 +157,7 @@ impl CmdEnv { &mut ver_cmd, &mut self.audio_cmd, &mut backlight_cmd, + &mut accel_cmd, &mut self.fcc_cmd, ]; diff --git a/services/shellchat/src/cmds/accel.rs b/services/shellchat/src/cmds/accel.rs new file mode 100644 index 000000000..8ff65c509 --- /dev/null +++ b/services/shellchat/src/cmds/accel.rs @@ -0,0 +1,31 @@ +use crate::{ShellCmdApi, CommonEnv}; +use xous_ipc::String; + +#[derive(Debug)] +pub struct Accel { +} + +impl<'a> ShellCmdApi<'a> for Accel { + cmd_api!(accel); // inserts boilerplate for command API + + fn process(&mut self, args: String::<1024>, env: &mut CommonEnv) -> Result>, xous::Error> { + use core::fmt::Write; + let mut ret = String::<1024>::new(); + let helpstring = "accel has no options"; + + let mut tokens = args.as_str().unwrap().split(' '); + + if let Some(sub_cmd) = tokens.next() { + match sub_cmd { + _ => { + let (x, y, z, id) = env.com.gyro_read_blocking().unwrap(); + write!(ret, "x: {} y: {} z: {}, id: 0x{:x}", x, y, z, id).unwrap(); + } + } + + } else { + write!(ret, "{}", helpstring).unwrap(); + } + Ok(Some(ret)) + } +}