device.rs

#[cfg(feature = "bootloader")]
use crate::commands::bootloader::{
    CMD_CHANGE_PROGRAMMING_MODE, CMD_ERASE_PAGES, CMD_PROGRAMMING_STATUS, CMD_PROGRAM_DATA,
    CMD_READ_MEMORY, CMD_SET_CHECKSUM, CMD_SET_PROGRAMMING_ADDRESS,
};
use crate::commands::common::{
    CMD_DEVICE_FAMILY, CMD_FIRMWARE_VERSION, CMD_FLASH_SIZE, CMD_MCU_IDCODE,
    CMD_REBOOT_TO_BOOTLOADER, CMD_RESET, CMD_WHO_AM_I,
};
#[cfg(feature = "controller")]
use crate::commands::controller::{
    CMD_CONTROLLED_MOTOR_SPEED, CMD_ENCODER_TICKS, CMD_FIRMWARE_FEATURES,
    CMD_MOTOR_SHUTDOWN_TIMEOUT, CMD_PID_K_D, CMD_PID_K_I, CMD_PID_K_P, CMD_PWM_FREQUENCY,
    CMD_RAW_MOTOR_SPEED, CMD_STATUS,
};
use core::slice;
#[cfg(any(feature = "controller", feature = "bootloader"))]
use core::time::Duration;

const I2C_ADDR: u8 = 0x57;

#[derive(Debug)]
pub enum Error<I2cError> {
    I2c(I2cError),
    #[cfg(feature = "controller")]
    InvalidFrequency(u32),
    #[cfg(feature = "controller")]
    InvalidDuration(Duration),
}

impl<I2CError: core::fmt::Display> core::fmt::Display for Error<I2CError> {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        match self {
            Error::I2c(error) => write!(f, "I2C error: {error}"),
            #[cfg(feature = "controller")]
            Error::InvalidFrequency(freq) => write!(f, "invalid frequency {freq} Hz"),
            #[cfg(feature = "controller")]
            Error::InvalidDuration(d) => write!(f, "invalid duration {d:?}"),
        }
    }
}

impl<I2CError: core::error::Error> core::error::Error for Error<I2CError> {}

pub type Result<T, I2cError> = core::result::Result<T, Error<I2cError>>;

/// The `Device` trait lets us interact with the board and contains methods
/// available both in controller and in bootloader mode.
pub trait Device {
    type I2cError;

    /// Write arbitrary data to the board.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn write(&mut self, buf: impl AsRef<[u8]>) -> Result<(), Self::I2cError>;

    /// Exchange data with the board using a repeated start I²C event.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn write_read(
        &mut self,
        write: impl AsRef<[u8]>,
        read: impl AsMut<[u8]>,
    ) -> Result<(), Self::I2cError>;

    /// Retrieve the firmware version as a (major, minor, patch) tuple.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn firmware_version(&mut self) -> Result<[u8; 3], Self::I2cError> {
        let mut result = [0; 3];
        self.write_read([CMD_FIRMWARE_VERSION], &mut result)?;
        Ok(result)
    }

    /// Identify the program currently running on the board as the
    /// controller, the bootloader, or an unknown program.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn who_am_i(&mut self) -> Result<WhoAmI, Self::I2cError> {
        let mut me: u8 = 0;
        self.write_read([CMD_WHO_AM_I], slice::from_mut(&mut me))?;
        Ok(match me {
            addr if addr == I2C_ADDR | 0x80 => WhoAmI::Bootloader,
            addr if addr == I2C_ADDR => WhoAmI::Controller,
            addr => WhoAmI::Other(addr),
        })
    }

    /// Reset the board.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn reset(&mut self) -> Result<(), Self::I2cError> {
        self.write([CMD_RESET])
    }

    /// Return the identity code and continuation code of the microcontroller
    /// running on the board if available.
    ///
    /// The [`brand_name`] function might help further identifying the
    /// microcontroller.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn device_family(&mut self) -> Result<Option<(u8, u8)>, Self::I2cError> {
        let mut data = [0; 2];
        self.write_read([CMD_DEVICE_FAMILY], &mut data)?;
        Ok((data != [0, 0]).then_some((data[0], data[1])))
    }

    /// Return the device id and revision id from the `IDCODE` field of the
    /// `DBGMCU` register of the microcontroller if available.
    ///
    /// Note that some microcontrollers require that the JTAG or SWD interface
    /// has been used in order for this information to be available.
    ///
    /// This information may be further analyzed using the
    /// [`mcu_kind`] function.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn mcu_kind(&mut self) -> Result<Option<(u16, u16)>, Self::I2cError> {
        let mut data = [0; 4];
        self.write_read([CMD_MCU_IDCODE], &mut data)?;
        Ok((data != [0; 4]).then_some((
            u16::from_le_bytes([data[0], data[1]]),
            u16::from_le_bytes([data[2], data[3]]),
        )))
    }

    /// Retrieve the flash size, in KiB, from the microcontroller
    /// present on the board.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn flash_size(&mut self) -> Result<u16, Self::I2cError> {
        let mut data = [0; 2];
        self.write_read([CMD_FLASH_SIZE], &mut data)?;
        Ok(u16::from_le_bytes([data[0], data[1]]))
    }

    /// Force the currently running program to switch to bootloader mode
    /// even when an application is present.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn switch_to_bootloader(&mut self) -> Result<(), Self::I2cError> {
        self.write([CMD_REBOOT_TO_BOOTLOADER])
    }
}

/// Commands available when the board is in controller mode.
#[cfg(feature = "controller")]
pub trait Controller: Device {
    /// Set the PWM frequency used to drive the motors in Hz, between
    /// `1` and `100_000`.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails, or
    /// if the frequency is out of the allowed range.
    fn set_pwm_frequency(&mut self, frequency_hz: u32) -> Result<(), Self::I2cError> {
        if frequency_hz > 100_000 {
            return Err(Error::InvalidFrequency(frequency_hz));
        }
        self.write((u32::from(CMD_PWM_FREQUENCY) + (frequency_hz << 8)).to_le_bytes())
    }

    /// Get the PWM frequency used to drive the motors in Hz.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_pwm_frequency(&mut self) -> Result<u32, Self::I2cError> {
        let mut data = [0; 4];
        self.write_read([CMD_PWM_FREQUENCY], &mut data[..3])?;
        Ok(u32::from_le_bytes(data))
    }

    /// Set the raw PID coefficients used for the controlled mode.
    /// The coefficients will be scaled by the controller as fixed
    /// point number values with [`FRACTIONAL_BITS`] bits.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn set_raw_pid_coefficients(
        &mut self,
        k_p: i32,
        k_i: i32,
        k_d: i32,
    ) -> Result<(), Self::I2cError> {
        let mut buf = [0; 5];
        for (command, data) in [(CMD_PID_K_P, k_p), (CMD_PID_K_I, k_i), (CMD_PID_K_D, k_d)] {
            buf[0] = command;
            buf[1..5].copy_from_slice(&data.to_le_bytes());
            self.write(buf)?;
        }
        Ok(())
    }

    /// Get the raw PID coefficients used for the controlled mode.
    /// The coefficients have been scaled by the controller as fixed
    /// point number values with [`FRACTIONAL_BITS`] bits.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_raw_pid_coefficients(&mut self) -> Result<(i32, i32, i32), Self::I2cError> {
        let mut result = [0i32; 3];
        let mut buf = [0; 4];
        for (command, data) in [CMD_PID_K_P, CMD_PID_K_I, CMD_PID_K_D]
            .into_iter()
            .zip(result.iter_mut())
        {
            self.write_read([command], &mut buf)?;
            *data = i32::from_le_bytes(buf);
        }
        Ok(result.into())
    }

    /// Set the timeout after which motors will shut down if no
    /// further I²C communication takes place.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn set_motor_shutdown_timeout(&mut self, delay: Duration) -> Result<(), Self::I2cError> {
        let timeout = delay.as_millis();
        if timeout > 10_000 {
            return Err(Error::InvalidDuration(delay));
        }
        let timeout = u8::try_from((timeout + 50) / 100).unwrap();
        self.write([CMD_MOTOR_SHUTDOWN_TIMEOUT, timeout])
    }

    /// Retrieve the current motors shutdown timeout.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_motor_shutdown_timeout(&mut self) -> Result<Duration, Self::I2cError> {
        let mut delay = 0;
        self.write_read([CMD_MOTOR_SHUTDOWN_TIMEOUT], slice::from_mut(&mut delay))?;
        Ok(Duration::from_millis(u64::from(delay) * 100))
    }

    /// Set the raw motor speed between -127 and 127 for each. `None` means
    /// that the speed of the corresponding motor is not modified, unless
    /// both sides use `None` in which case the motors will be put in standby
    /// mode.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    #[allow(clippy::cast_sign_loss)]
    fn set_raw_speed(&mut self, left: Option<i8>, right: Option<i8>) -> Result<(), Self::I2cError> {
        let (left, right) = (left.map(|v| v as u8), right.map(|v| v as u8));
        self.write([
            CMD_RAW_MOTOR_SPEED,
            left.unwrap_or(0x80),
            right.unwrap_or(0x80),
        ])
    }

    /// Get the raw motor speed, or `None` if the motors are in standby mode.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    #[allow(clippy::cast_possible_wrap)]
    fn get_raw_speed(&mut self) -> Result<Option<(i8, i8)>, Self::I2cError> {
        let mut buf = [0; 2];
        self.write_read([CMD_RAW_MOTOR_SPEED], &mut buf)?;
        Ok((buf != [0x80, 0x80]).then_some((buf[0] as i8, buf[1] as i8)))
    }

    /// Set the left and right motor speeds. The speed will be controlled
    /// using the internal PID.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn set_motor_speed(&mut self, left: i16, right: i16) -> Result<(), Self::I2cError> {
        let mut buf = [CMD_CONTROLLED_MOTOR_SPEED, 0, 0, 0, 0];
        buf[1..3].copy_from_slice(&left.to_le_bytes());
        buf[3..5].copy_from_slice(&right.to_le_bytes());
        self.write(buf)
    }

    /// Get the left and right motor speeds. If the motors are in
    /// standby mode, or if a raw speed has been explicitly set,
    /// this method will return `None`.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_motor_speed(&mut self) -> Result<Option<(i16, i16)>, Self::I2cError> {
        let mut buf = [0; 4];
        self.write_read([CMD_CONTROLLED_MOTOR_SPEED], &mut buf)?;
        Ok((buf != [0x80, 0x00, 0x80, 0x00]).then(|| {
            (
                i16::from_le_bytes(buf[0..2].try_into().unwrap()),
                i16::from_le_bytes(buf[2..4].try_into().unwrap()),
            )
        }))
    }

    /// Retrive the number of left and right encoder ticks since the
    /// last retrieval.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_encoder_ticks(&mut self) -> Result<(i16, i16), Self::I2cError> {
        let mut buf = [0; 4];
        self.write_read([CMD_ENCODER_TICKS], &mut buf)?;
        Ok((
            i16::from_le_bytes(buf[0..2].try_into().unwrap()),
            i16::from_le_bytes(buf[2..4].try_into().unwrap()),
        ))
    }

    /// Get the current status of the board.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_status(&mut self) -> Result<Status, Self::I2cError> {
        let mut status = 0;
        self.write_read([CMD_STATUS], slice::from_mut(&mut status))?;
        Ok(Status(status))
    }

    /// Retrieve the firmware features implemented on the controller.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn firmware_features(&mut self) -> Result<FirmwareFeatures, Self::I2cError> {
        let mut features = 0;
        self.write_read([CMD_FIRMWARE_FEATURES], slice::from_mut(&mut features))?;
        Ok(FirmwareFeatures(features))
    }
}

#[cfg(feature = "controller")]
impl<D: Device> Controller for D {}

/// Commands available when the board is in bootloader mode.
#[cfg(feature = "bootloader")]
pub trait Bootloader: Device {
    /// Write command, data, and a xor of all data bytes. The xor
    /// computation is returned.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn write_xored(&mut self, command: u8, data: impl AsRef<[u8]>) -> Result<u8, Self::I2cError> {
        let data = data.as_ref();
        let data_len = data.len();
        let mut buf = [0; 10];
        buf[0] = command;
        buf[1..=data_len].copy_from_slice(data);
        let xor = data.iter().fold(0, |a, &b| a ^ b);
        buf[data_len + 1] = xor;
        self.write(&buf[..data_len + 2])?;
        Ok(xor)
    }

    /// Get the programming status of the bootloader.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_programming_status(&mut self) -> Result<ProgrammingStatus, Self::I2cError> {
        let mut buf = [0; 2];
        self.write_read([CMD_PROGRAMMING_STATUS], &mut buf)?;
        Ok(ProgrammingStatus {
            status: buf[0],
            checksum: buf[1],
        })
    }

    /// Enter programming mode by sending the required sequence.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn enter_programming_mode(&mut self) -> Result<(), Self::I2cError> {
        self.write([CMD_CHANGE_PROGRAMMING_MODE, 0x17, 0x27, 0x65, 0x40])
    }

    /// Leave the programming mode, and mark or not the application has having been
    /// successfully written.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn leave_programming_mode(&mut self, commit: bool) -> Result<(), Self::I2cError> {
        self.write([CMD_CHANGE_PROGRAMMING_MODE, u8::from(commit)])
    }

    /// Erase `count` application pages starting at `index`.
    ///
    /// Return a duration for which is it advised to wait before sending other
    /// operations.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn erase_pages(&mut self, index: u8, count: u8) -> Result<Duration, Self::I2cError> {
        self.write_xored(CMD_ERASE_PAGES, [index, count])?;
        Ok(Duration::from_millis(u64::from(count + 1) * 30))
    }

    /// Set the current programming address in the application space.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn set_programming_address(&mut self, address: u32) -> Result<(), Self::I2cError> {
        self.write_xored(CMD_SET_PROGRAMMING_ADDRESS, address.to_le_bytes())?;
        Ok(())
    }

    /// Program data starting at the current programming address.
    /// The address will advance automatically.x
    /// The checksum will be updated with a xor of the data.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn program_data(&mut self, data: &[u8], checksum: &mut u8) -> Result<(), Self::I2cError> {
        let xor = self.write_xored(CMD_PROGRAM_DATA, data)?;
        *checksum ^= xor;
        Ok(())
    }

    /// Read memory starting from the current programming address.
    /// The address will advance automatically.
    /// Zeroes will be returned for data outside the application memory.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn read_memory(&mut self) -> Result<[u8; 8], Self::I2cError> {
        let mut result = [0; 8];
        self.write_read([CMD_READ_MEMORY], &mut result)?;
        Ok(result)
    }

    /// Set the current value of the running checksum.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn set_checksum(&mut self, checksum: u8) -> Result<(), Self::I2cError> {
        self.write([CMD_SET_CHECKSUM, checksum])
    }
}

#[cfg(feature = "bootloader")]
impl<D: Device> Bootloader for D {}

impl<I2c: embedded_hal::i2c::I2c> Device for I2c {
    type I2cError = I2c::Error;

    fn write(&mut self, buf: impl AsRef<[u8]>) -> Result<(), Self::I2cError> {
        (self as &mut I2c)
            .write(I2C_ADDR, buf.as_ref())
            .map_err(Error::I2c)
    }

    fn write_read(
        &mut self,
        write: impl AsRef<[u8]>,
        mut read: impl AsMut<[u8]>,
    ) -> Result<(), Self::I2cError> {
        (self as &mut I2c)
            .write_read(I2C_ADDR, write.as_ref(), read.as_mut())
            .map_err(Error::I2c)
    }
}

/// Controller status
#[cfg(feature = "controller")]
#[derive(Clone, Copy)]
pub struct Status(u8);

#[cfg(feature = "controller")]
impl Status {
    /// Check if any of the motors are moving.
    #[must_use]
    pub fn is_moving(self) -> bool {
        self.0 & 1 != 0
    }

    /// Check if the speed is currently controlled by the internal PID process.
    #[must_use]
    pub fn is_controlled(self) -> bool {
        self.0 & 2 != 0
    }
}

/// Bootloader programming status
#[cfg(feature = "bootloader")]
#[derive(Clone, Copy, Debug)]
pub struct ProgrammingStatus {
    status: u8,
    checksum: u8,
}

#[cfg(feature = "bootloader")]
impl ProgrammingStatus {
    /// Check if the bootloader is in programming mode.
    #[must_use]
    pub fn programming_mode(self) -> bool {
        self.status & 1 != 0
    }

    /// Check if an error has been detected since the last time the
    /// programming mode has been entered.
    #[must_use]
    pub fn error_detected(self) -> bool {
        self.status & 2 != 0
    }

    /// Check if a valid application is marked present.
    #[must_use]
    pub fn application_present(self) -> bool {
        self.status & 4 != 0
    }

    /// Current running checksum value.
    #[must_use]
    pub fn checksum(self) -> u8 {
        self.checksum
    }
}

/// Firmware features enabled in the controller.
#[cfg(feature = "controller")]
#[derive(Clone, Copy)]
pub struct FirmwareFeatures(u8);

#[cfg(feature = "controller")]
impl FirmwareFeatures {
    /// Check if the controller has been compiled with bootloader
    /// support (`true`), or if it is compiled as a standalone
    /// application (`false`).
    #[must_use]
    pub fn has_bootloader_support(self) -> bool {
        self.0 & 1 != 0
    }
}

/// Currently running program on the board.
pub enum WhoAmI {
    Bootloader,
    Controller,
    Other(u8),
}

/// Decode identity code and continuation code into a brand name.
#[must_use]
pub fn brand_name(identity_code: u8, continuation_code: u8) -> Option<&'static str> {
    match (identity_code, continuation_code) {
        (0x20, 0x00) => Some("STMicroelectronics"),
        (0x3b, 0x04) => Some("ARM generic (APM32?)"),
        _ => None,
    }
}

/// Decode device id and revision id obtained from the IDCODE field of the DBGMCU register
/// into a density information, and a model variant.
#[must_use]
pub fn mcu_kind(dev_id: u16, rev_id: u16) -> (&'static str, &'static str) {
    match (dev_id, rev_id) {
        (0x412, 0x1000) => ("low-density", "A"),
        (0x412, _) => ("low-density", "?"),
        (0x410, 0x0000) => ("medium-density", "A"),
        (0x410, 0x2000) => ("medium-density", "B"),
        (0x410, 0x2001) => ("medium-density", "Z"),
        (0x410, 0x2003) => ("medium-density", "1/2/3/X/Y"),
        (0x410, _) => ("medium-density", "?"),
        (0x414, 0x1000) => ("high-density", "1/A"),
        (0x414, 0x1001) => ("high-density", "Z"),
        (0x414, 0x1003) => ("high-density", "1/2/3/X/Y"),
        (0x414, _) => ("high-density", "?"),
        (0x430, 0x1000) => ("XL-density", "1/A"),
        (0x430, _) => ("XL-density", "?"),
        (0x418, 0x1000) => ("connectivity", "A"),
        (0x418, 0x1001) => ("connectivity", "Z"),
        (0x418, _) => ("connectivity", "?"),
        _ => ("?", "?"),
    }
}

/// Number of bits by which the PID coefficients are scaled internally
/// by the microcontroller to make a fixed point numbers from an integral
/// number.
pub const FRACTIONAL_BITS: usize = 8;