1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127

// ------------------------------------------------------------------------------
// Copyright 2018 Uwe Arzt, mail@uwe-arzt.de
// SPDX-License-Identifier: Apache-2.0
// ------------------------------------------------------------------------------

//! Driver for AMS AS5048A Magnetic Rotary Encoder

#![no_std]

use core::fmt;

use embedded_hal as hal;
use hal::blocking::spi::Transfer;
use hal::digital::v2::OutputPin;

/// Error
pub enum Error<SPI, CS>
where
    SPI: Transfer<u8>,
    CS: OutputPin,
{
    Spi(SPI::Error),
    ChipSelect(CS::Error),
}

impl<SPI, CS> fmt::Debug for Error<SPI, CS>
where
    SPI: Transfer<u8>,
    <SPI as Transfer<u8>>::Error: fmt::Debug,
    CS: OutputPin,
    <CS as OutputPin>::Error: fmt::Debug,
{
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            Error::Spi(error) => write!(f, "Spi({:?})", error),
            Error::ChipSelect(error) => write!(f, "ChipSelect({:?})", error),
        }
    }
}

#[allow(dead_code)]
#[derive(Copy, Clone)]
enum Register {
    ClearErrorFlag = 0x0001,
    ProgrammingControl = 0x0003,
    OtpRegisterZeroPosHigh = 0x0016,
    OtpRegisterZeroPosLow = 0x0017,
    DiagAgc = 0x3FFD,
    Magnitude = 0x3FFE,
    Angle = 0x3FFF,
}

/// AS5048A driver
pub struct AS5048A<SPI, CS> {
    spi: SPI,
    cs: CS,
}

impl<SPI, CS, E> AS5048A<SPI, CS>
where
    SPI: Transfer<u8, Error = E>,
    CS: OutputPin,
{
    pub fn new(spi: SPI, cs: CS) -> Self {
        Self { spi, cs }
    }

    pub fn diag_gain(&mut self) -> Result<(u8, u8), Error<SPI, CS>> {
        self.read(Register::DiagAgc)
            .map(|arr| (arr[0] & 0x0f, arr[1]))
    }

    pub fn magnitude(&mut self) -> Result<u16, Error<SPI, CS>> {
        self.read_u16(Register::Magnitude)
    }

    /// Read the rotation angle as u16 (only 14 bits are significant)
    pub fn angle(&mut self) -> Result<u16, Error<SPI, CS>> {
        self.read_u16(Register::Angle)
    }

    fn read_u16(&mut self, reg: Register) -> Result<u16, Error<SPI, CS>> {
        match self.read(reg) {
            Ok(arr) => {
                let y = u16::from_be_bytes(arr);
                Ok(y & 0b0011_1111_1111_1111)
            }
            Err(e) => Err(e),
        }
    }

    fn read(&mut self, reg: Register) -> Result<[u8; 2], Error<SPI, CS>> {
        // send cmd
        let mut cmd: u16 = 0b_0100_0000_0000_0000;
        cmd |= reg as u16;
        cmd = set_parity(cmd);

        let mut bytes = cmd.to_be_bytes();

        self.cs.set_low().map_err(Error::ChipSelect)?;
        self.spi.transfer(&mut bytes).map_err(Error::Spi)?;
        self.cs.set_high().map_err(Error::ChipSelect)?;

        // send nop to get result back
        let mut nop = [0x00, 0x00];
        self.cs.set_low().map_err(Error::ChipSelect)?;
        self.spi.transfer(&mut nop).map_err(Error::Spi)?;
        self.cs.set_high().map_err(Error::ChipSelect)?;

        Ok(nop)
    }
}

const fn set_parity(par: u16) -> u16 {
    let mut x = par;

    x = (x & 0x00FF) ^ (x >> 8);
    x = (x & 0x000F) ^ (x >> 4);
    x = (x & 0x0003) ^ (x >> 2);
    x = (x & 0x0001) ^ (x >> 1);

    if x == 0x0001 {
        par | 0b1000_0000_0000_0000
    } else {
        par
    }
}