Update fault handling in driver crates.

- Add parity and motor fault handling
- Add a full diagnostic struct for the A4964

drivers/src/a4910/driver.rs

@@ -30,45 +30,63 @@ where
     /// Read from the specified register, and store the register in the local data copy.
     ///
     /// # Errors
-    /// Returns a `SPI::Error` if the SPI transaction fails.
+    /// Returns a `AllegroError::SpiError` if the SPI transaction fails.
+    /// Stores a message but still returns a `AllegroError::MotorFault` if the fault bit is raised.
     pub fn read_register(&mut self, register: A4910Reg) -> Result<&Self, AllegroError> {
         let mut message = Self::read_request(register);
         self.spi.transfer_in_place(&mut message)?;
-        self.read_response(register, message);
+        self.read_response(register, message)?;
         Ok(self)
     }
 
     /// Write to the specified register, and store the returned diagnostics.
     ///
     /// # Errors
-    /// Returns a `SPI::Error` if the SPI transaction fails.
+    /// Returns a `AllegroError::SpiError` if the SPI transaction fails.
+    /// Stores a message but still returns a `AllegroError::MotorFault` if the fault bit is raised.
     pub fn write_register(&mut self, register: A4910Reg) -> Result<&Self, AllegroError> {
         let mut message = self.write_request(register);
         self.spi.transfer_in_place(&mut message)?;
-        self.write_response(message);
+        self.write_response(message)?;
         Ok(self)
     }
 
+    /// Encode a request to read the desired register.
     fn read_request(register: A4910Reg) -> [u8; 2] {
         let request: u16 = ReadRequest::new(false, register.into()).into();
         request.to_be_bytes()
     }
 
+    /// Encode a request to write to the desired register.
     fn write_request(&self, register: A4910Reg) -> [u8; 2] {
         let reg_contents =
             unsafe { bilge::arbitrary_int::u13::new_unchecked(self.regs[register].value()) };
         let request: u16 = WriteRequest::new(reg_contents, true, register.into()).into();
         request.to_be_bytes()
     }
 
-    fn read_response(&mut self, register: A4910Reg, message: [u8; 2]) {
+    /// Decode the response from a SPI read transaction.
+    /// 
+    /// # Errors
+    /// This function will check for an `AllegroError::MotorFault` and return it
+    /// after parsing.
+    fn read_response(&mut self, register: A4910Reg, message: [u8; 2]) -> Result<(), AllegroError> {
         let response = ReadResponse::from(u16::from_be_bytes(message));
         self.regs[register].set_value(response.register().into());
         self.status.set_header(response.status());
+        if response.status().ff() { return Err(AllegroError::MotorFault) }
+        Ok(())
     }
 
-    fn write_response(&mut self, message: [u8; 2]) {
+    /// Decode the response from a SPI write transaction.
+    /// 
+    /// # Errors
+    /// This function will check for an `AllegroError::MotorFault` and return it
+    /// after parsing.
+    fn write_response(&mut self, message: [u8; 2]) -> Result<(), AllegroError> {
         self.status = WriteResponse::from(u16::from_be_bytes(message));
+        if self.status.header().ff() { return Err(AllegroError::MotorFault) }
+        Ok(())
     }
 }
 
@@ -99,8 +117,8 @@ mod tests {
         let config1_write_req = [0b01_1_1_1_00_0, 0b0_0100000];
         assert_eq!(a4910.write_request(A4910Reg::Config1), config1_write_req);
 
-        let config1_write_resp = [0b00_0_0_0_11_1, 0b1_1011111];
-        a4910.read_response(A4910Reg::Config1, config1_write_resp);
+        let config1_read_resp = [0b00_0_0_0_11_1, 0b1_1011111];
+        a4910.read_response(A4910Reg::Config1, config1_read_resp).unwrap();
         assert_eq!(a4910.regs.cfg.1.vt(), u7::new(0b101_1111).into());
 
         a4910.spi.done();

drivers/src/a4910/regs/mod.rs

@@ -12,7 +12,7 @@ use config::Config;
 use mask::Mask;
 use run::Run;
 
-#[derive(Debug, PartialEq, Clone, Copy)]
+#[derive(Debug, PartialEq, Copy, Clone)]
 pub enum A4910Reg {
     Config0,
     Config1,

drivers/src/a4964/diagnostics.rs

@@ -0,0 +1,13 @@
+//! All diagnostics for the chip
+
+use bilge::prelude::*;
+use crate::a4964::readback::ReadbackDiagnostics;
+use crate::a4964::regs::diagnostic::*;
+
+#[bitsize(24)]
+#[derive(Clone, Copy, DefaultBits, DebugBits, FromBits)]
+pub struct Diagnostics {
+    pub readback_diagnostics: ReadbackDiagnostics,
+    pub status_diagnostics: Data,
+    pub main_diagnostics: Header,
+}

drivers/src/a4964/driver.rs

@@ -6,13 +6,16 @@ use crate::io::Parity;
 use crate::error::AllegroError;
 
 use super::{
-    io::{Diagnostics, ReadOnlyResponse, ReadRequest, ReadResponse, WriteRequest, WriteResponse, WriteOnlyRequest},
-    regs::{A4964Reg, A4964Registers},
+    io::*,
+    regs::{*, readback::ReadbackSelect},
+    readback::Readback,
+    diagnostics::Diagnostics,
 };
 pub struct A4964<SPI> {
     spi: SPI,
     pub regs: A4964Registers,
-    pub status: Diagnostics,
+    pub diagnostics: Diagnostics,
+    pub readback: Readback,
 }
 
 impl<SPI> A4964<SPI>
@@ -25,20 +28,22 @@ where
         Self {
             spi,
             regs: A4964Registers::default(),
-            status: Diagnostics::default(),
+            diagnostics: Diagnostics::default(),
+            readback: Readback::default(),
         }
     }
 
     /// Read from the specified register, and store the register in the local data copy.
     ///
     /// # Errors
-    /// Returns a `MotorFault` if the SPI transaction fails.
+    /// Returns an `AllegroError` if the SPI transaction fails.
+    /// Returns an `InvalidRegister` if the user is trying to read from the write only register
     pub fn read_register(&mut self, register: A4964Reg) -> Result<&Self, AllegroError> {
         if register == A4964Reg::WriteOnly { return Err(AllegroError::InvalidRegister) }
 
         let mut message = Self::read_request(register);
         self.spi.transfer_in_place(&mut message)?;
-        self.read_response(register, message);
+        self.read_response(register, message)?;
         Ok(self)
     }
 
@@ -51,15 +56,17 @@ where
 
         let mut message = self.write_request(register);
         self.spi.transfer_in_place(&mut message)?;
-        self.write_response(message);
+        self.write_response(message)?;
         Ok(self)
     }
 
+    /// Encode a request to read the desired register.
     fn read_request(register: A4964Reg) -> [u8; 2] {
         let request: u16 = ReadRequest::new(false, false, register.into()).into();
         request.to_be_bytes()
     }
 
+    /// Encode a request to write to the desired register.
     fn write_request(&self, register: A4964Reg) -> [u8; 2] {
         let message: u16 = if register == A4964Reg::WriteOnly {
             let reg_contents = unsafe { bilge::arbitrary_int::u10::new_unchecked(self.regs[register].value()) };
@@ -75,20 +82,56 @@ where
         message.to_be_bytes()
     }
 
-    fn read_response(&mut self, register: A4964Reg, message: [u8; 2]) {
-        let register_contents: u16 = if let A4964Reg::ReadOnly = register {
-            let response = ReadOnlyResponse::from(u16::from_be_bytes(message));
-            self.status.set_header(response.status());
-            response.register().into()
+    /// Decode the response from a SPI read transaction.
+    /// 
+    /// # Errors
+    /// If the message has `AllegroError::InvalidParity`, then it is not to be trusted.
+    /// The message is not parsed, and the function returns.
+    /// This function will check for an `AllegroError::MotorFault` and return it
+    /// after parsing.
+    fn read_response(&mut self, register: A4964Reg, message: [u8; 2]) -> Result<(), AllegroError> {
+        let mut result: Result<(), AllegroError> = Ok(());
+        let message = u16::from_be_bytes(message);
+        if !crate::io::parity(message) { return Err(AllegroError::InvalidParity); }
+
+        let register_contents: u16 = if register == A4964Reg::ReadOnly {
+            let response = ReadOnlyResponse::from(message);
+            if response.status().ff() { result = Err(AllegroError::MotorFault) }
+            self.diagnostics.set_main_diagnostics(response.status());
+
+            let register_contents: u16 = response.register().into();
+            self.readback[self.regs.readback_select.rbs()] = register_contents;
+
+            if self.regs.readback_select.rbs() == ReadbackSelect::Diagnostic {
+                self.diagnostics.set_readback_diagnostics(response.register().into());
+            }
+
+            register_contents
         } else {
-            let response = ReadResponse::from(u16::from_be_bytes(message));
-            self.status.set_header(response.status());
+            let response = ReadResponse::from(message);
+            if response.status().ff() { result = Err(AllegroError::MotorFault) }
+
+            self.diagnostics.set_main_diagnostics(response.status());
             response.register().into()
         };
         self.regs[register].set_value(register_contents);
+        result
     }
 
-    fn write_response(&mut self, message: [u8; 2]) {
-        self.status = WriteResponse::from(u16::from_be_bytes(message));
+    /// Decode the response from a SPI write transaction.
+    /// 
+    /// # Errors
+    /// If the message has `AllegroError::InvalidParity`, then it is not to be trusted.
+    /// The message is not parsed, and the function returns.
+    /// This function will check for an `AllegroError::MotorFault` and return it
+    /// after parsing.
+    fn write_response(&mut self, message: [u8; 2]) -> Result<(), AllegroError> {
+        let message = u16::from_be_bytes(message);
+        if !crate::io::parity(message) { return Err(AllegroError::InvalidParity) }
+        let resp = WriteResponse::from(message);
+        self.diagnostics.set_main_diagnostics(resp.header());
+        self.diagnostics.set_status_diagnostics(resp.data());
+        if resp.header().ff() { return Err(AllegroError::MotorFault) }
+        Ok(())
     }
 }

drivers/src/a4964/io.rs

@@ -17,7 +17,7 @@ pub struct ReadRequest {
 #[bitsize(16)]
 #[derive(DebugBits, DefaultBits, PartialEq, FromBits, Parity)]
 pub struct ReadResponse {
-    parity: bool,
+    pub(crate) parity: bool,
     pub register: u9,
     write_read: bool,
     pub status: Header,
@@ -26,7 +26,7 @@ pub struct ReadResponse {
 #[bitsize(16)]
 #[derive(DebugBits, DefaultBits, PartialEq, FromBits, Parity)]
 pub struct ReadOnlyResponse {
-    parity: bool,
+    pub(crate) parity: bool,
     pub register: u10,
     pub status: Header,
 }
@@ -56,5 +56,3 @@ pub struct WriteResponse {
     reserved: u1,
     pub header: Header,
 }
-
-pub type Diagnostics = WriteResponse;

drivers/src/a4964/mod.rs

@@ -3,3 +3,5 @@
 pub mod driver;
 pub mod io;
 pub mod regs;
+pub mod readback;
+pub mod diagnostics;

drivers/src/a4964/readback.rs

@@ -0,0 +1,63 @@
+//! Readback telemetry
+
+use core::ops::{Index, IndexMut};
+use bilge::prelude::*;
+use super::regs::readback::ReadbackSelect;
+
+#[bitsize(10)]
+#[derive(Copy, Clone, PartialEq, DebugBits, DefaultBits, FromBits)]
+pub struct ReadbackDiagnostics {
+    cl: bool,
+    ch: bool,
+    bl: bool,
+    bh: bool,
+    al: bool,
+    ah: bool,
+    bc: bool,
+    bb: bool,
+    ba: bool,
+    osr: bool,
+}
+
+#[derive(Debug, Copy, Clone, PartialEq, Default)]
+pub struct Readback {
+    diagnostic: u16,
+    motor_speed: u16,
+    avg_supply_current: u16,
+    supply_voltage: u16,
+    chip_temperature: u16,
+    demand_input: u16,
+    applied_bridge_peak_duty_cycle: u16,
+    applied_phase_advance: u16,
+}
+
+impl Index<ReadbackSelect> for Readback {
+    type Output = u16;
+    fn index(&self, index: ReadbackSelect) -> &Self::Output {
+        match index {
+            ReadbackSelect::Diagnostic => &self.diagnostic,
+            ReadbackSelect::MotorSpeed => &self.motor_speed,
+            ReadbackSelect::AvgSupplyCurrent => &self.avg_supply_current,
+            ReadbackSelect::SupplyVoltage => &self.supply_voltage,
+            ReadbackSelect::ChipTemperature => &self.chip_temperature,
+            ReadbackSelect::DemandInput => &self.demand_input,
+            ReadbackSelect::AppliedBridgePeakDutyCycle => &self.applied_bridge_peak_duty_cycle,
+            ReadbackSelect::AppliedPhaseAdvance => &self.applied_phase_advance,
+        }
+    }
+}
+
+impl IndexMut<ReadbackSelect> for Readback {
+    fn index_mut(&mut self, index: ReadbackSelect) -> &mut Self::Output {
+        match index {
+            ReadbackSelect::Diagnostic => &mut self.diagnostic,
+            ReadbackSelect::MotorSpeed => &mut self.motor_speed,
+            ReadbackSelect::AvgSupplyCurrent => &mut self.avg_supply_current,
+            ReadbackSelect::SupplyVoltage => &mut self.supply_voltage,
+            ReadbackSelect::ChipTemperature => &mut self.chip_temperature,
+            ReadbackSelect::DemandInput => &mut self.demand_input,
+            ReadbackSelect::AppliedBridgePeakDutyCycle => &mut self.applied_bridge_peak_duty_cycle,
+            ReadbackSelect::AppliedPhaseAdvance => &mut self.applied_phase_advance,
+        }
+    }
+}

drivers/src/a4964/regs/mod.rs

@@ -42,7 +42,7 @@ use vds_monitor::VdsMonitor;
 use watchdog::Watchdog;
 use write_only::WriteOnly;
 
-#[derive(Debug, PartialEq, Clone, Copy)]
+#[derive(Debug, PartialEq, Copy, Clone)]
 pub enum A4964Reg {
     Pwm0,
     Pwm1,

drivers/src/a4964/regs/read_only.rs

@@ -4,7 +4,7 @@ use allegro_motor_derive::AllegroRegister;
 use bilge::prelude::*;
 
 #[bitsize(10)]
-#[derive(DebugBits, PartialEq, DefaultBits, FromBits, Clone, Copy)]
+#[derive(DebugBits, PartialEq, DefaultBits, FromBits, Copy, Clone)]
 pub struct DiagnosticRegister {
     pub osr: bool,
     pub ba: bool,