diff --git a/mote-api/src/messages/mote_to_host.rs b/mote-api/src/messages/mote_to_host.rs index f9592ee..b96b4b9 100644 --- a/mote-api/src/messages/mote_to_host.rs +++ b/mote-api/src/messages/mote_to_host.rs @@ -23,7 +23,7 @@ pub struct Point { pub struct WheelJointState { pub effort_percent: f32, pub velocity_rad_per_s: f32, - pub postition_rad: f32, + pub position_rad: f32, } #[cfg_attr(feature = "schemars", derive(JsonSchema))] diff --git a/mote-ffi/mote_link/examples/rerun_demo.py b/mote-ffi/mote_link/examples/rerun_demo.py index dc7962d..88c8ed8 100644 --- a/mote-ffi/mote_link/examples/rerun_demo.py +++ b/mote-ffi/mote_link/examples/rerun_demo.py @@ -25,7 +25,7 @@ def _log_drive_base_state(state: DriveBaseState): f"drive_base/{side}/velocity_rad_per_s", rr.Scalars(wheel.velocity_rad_per_s), ) - rr.log(f"drive_base/{side}/position_rad", rr.Scalars(wheel.postition_rad)) + rr.log(f"drive_base/{side}/position_rad", rr.Scalars(wheel.position_rad)) def _log_imu_measurement(imu: IMUMeasurement): diff --git a/mote-ffi/mote_link/link.py b/mote-ffi/mote_link/link.py index 512d9ae..68c9018 100644 --- a/mote-ffi/mote_link/link.py +++ b/mote-ffi/mote_link/link.py @@ -94,7 +94,7 @@ class SetUID: class WheelJointState: effort_percent: float velocity_rad_per_s: float - postition_rad: float + position_rad: float @dataclass diff --git a/mote-ffi/mote_link/tests/test_link.py b/mote-ffi/mote_link/tests/test_link.py index 45f5464..fc2ed37 100644 --- a/mote-ffi/mote_link/tests/test_link.py +++ b/mote-ffi/mote_link/tests/test_link.py @@ -82,12 +82,12 @@ def test_drive_base_state(self): "left": { "effort_percent": 0.5, "velocity_rad_per_s": 1.0, - "postition_rad": 0.0, + "position_rad": 0.0, }, "right": { "effort_percent": 0.3, "velocity_rad_per_s": 0.8, - "postition_rad": 0.1, + "position_rad": 0.1, }, } } diff --git a/mote-ffi/src/c.rs b/mote-ffi/src/c.rs index c9285c7..4586050 100644 --- a/mote-ffi/src/c.rs +++ b/mote-ffi/src/c.rs @@ -3,17 +3,24 @@ use std::ffi::{CStr, CString}; use std::os::raw::{c_char, c_int}; -use mote_api::{MoteLink, messages::host_to_mote}; +use mote_api::{ + MoteLink, + messages::{host_to_mote, mote_to_host}, +}; + +use crate::MoteCommsFFI; + +type MoteLinkFFI = MoteCommsFFI<1400, mote_to_host::Message, host_to_mote::Message>; pub struct MoteLinkHandle { - inner: MoteLink, + inner: MoteLinkFFI, } /// Create a new MoteLink handle. The returned pointer must be freed with `mote_link_free`. #[unsafe(no_mangle)] pub extern "C" fn mote_link_new() -> *mut MoteLinkHandle { Box::into_raw(Box::new(MoteLinkHandle { - inner: MoteLink::new(), + inner: MoteCommsFFI::from(MoteLink::new()), })) } @@ -44,11 +51,7 @@ pub unsafe extern "C" fn mote_link_send( Ok(s) => s, Err(_) => return -1, }; - let msg: host_to_mote::Message = match serde_json::from_str(json) { - Ok(m) => m, - Err(_) => return -1, - }; - match handle.inner.send(msg) { + match handle.inner.send(json) { Ok(_) => 0, Err(_) => -1, } @@ -68,7 +71,7 @@ pub unsafe extern "C" fn mote_link_poll_transmit( buf_len: c_int, ) -> c_int { let handle = unsafe { &mut *handle }; - match handle.inner.poll_transmit() { + match handle.inner.link.poll_transmit() { Some(bytes) => { if bytes.len() > buf_len as usize { return -1; @@ -94,7 +97,7 @@ pub unsafe extern "C" fn mote_link_handle_receive( ) -> c_int { let handle = unsafe { &mut *handle }; let bytes = unsafe { std::slice::from_raw_parts(buf, buf_len as usize) }; - handle.inner.handle_receive(bytes); + handle.inner.link.handle_receive(bytes); 0 } @@ -113,11 +116,7 @@ pub unsafe extern "C" fn mote_link_poll_receive( ) -> c_int { let handle = unsafe { &mut *handle }; match handle.inner.poll_receive() { - Ok(Some(msg)) => { - let json = match serde_json::to_string(&msg) { - Ok(s) => s, - Err(_) => return -1, - }; + Ok(Some(json)) => { let cstr = match CString::new(json) { Ok(s) => s, Err(_) => return -1, diff --git a/mote-ffi/src/lib.rs b/mote-ffi/src/lib.rs index 4524a24..210e06f 100644 --- a/mote-ffi/src/lib.rs +++ b/mote-ffi/src/lib.rs @@ -35,7 +35,7 @@ where in_type: PhantomData, out_type: PhantomData, - link: MoteComms, + pub(crate) link: MoteComms, } impl From> for MoteCommsFFI @@ -62,7 +62,7 @@ where I: Serialize + for<'de> Deserialize<'de>, // Input type O: Serialize + for<'de> Deserialize<'de>, // Output type { - fn new(link: MoteComms) -> Self { + pub(crate) fn new(link: MoteComms) -> Self { Self { link, in_type: PhantomData, @@ -71,12 +71,12 @@ where } /// Queue a message to be sent - fn send(&mut self, json: &str) -> Result<(), Error> { + pub(crate) fn send(&mut self, json: &str) -> Result<(), Error> { let msg: O = serde_json::from_str(json)?; self.link.send(msg).map_err(|e| e.into()) } - fn poll_transmit(&mut self) -> Result, Error> { + pub(crate) fn poll_transmit(&mut self) -> Result, Error> { if let Some(payload) = self.link.poll_transmit() { Ok(Some(serde_json::to_string(&payload)?)) } else { @@ -84,13 +84,13 @@ where } } - fn handle_receive(&mut self, json: &str) -> Result<(), Error> { + pub(crate) fn handle_receive(&mut self, json: &str) -> Result<(), Error> { let packet: Vec = serde_json::from_str(json)?; self.link.handle_receive(&packet); Ok(()) } - fn poll_receive(&mut self) -> Result, Error> { + pub(crate) fn poll_receive(&mut self) -> Result, Error> { if let Some(v) = self.link.poll_receive()? { Ok(Some(serde_json::to_string(&v)?)) } else { diff --git a/mote-firmware/src/tasks/drive_base.rs b/mote-firmware/src/tasks/drive_base.rs index 8c837e9..66c5ad3 100644 --- a/mote-firmware/src/tasks/drive_base.rs +++ b/mote-firmware/src/tasks/drive_base.rs @@ -63,7 +63,7 @@ impl<'d, T: SetDutyCycle, P: Instance, const SM: usize> Motor<'d, T, P, SM> { joint_state: WheelJointState { effort_percent: 0.0, velocity_rad_per_s: 0.0, - postition_rad: 0.0, + position_rad: 0.0, }, } } @@ -102,7 +102,7 @@ impl<'d, T: SetDutyCycle, P: Instance, const SM: usize> Motor<'d, T, P, SM> { } // Update the joint state - self.joint_state.postition_rad = encoder_pulses_to_rad(self.encoder_value); + self.joint_state.position_rad = encoder_pulses_to_rad(self.encoder_value); self.joint_state.velocity_rad_per_s = measurement / dt; self.joint_state.effort_percent = deadband_adjusted_output; }