diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml new file mode 100644 index 0000000..dbec5fb --- /dev/null +++ b/.github/workflows/ccpp.yml @@ -0,0 +1,68 @@ +name: Library Compile +on: push +jobs: + build: + name: Test compile + runs-on: ubuntu-latest + strategy: + matrix: + arduino-boards-fqbn: + - arduino:avr:uno # arudino uno + - arduino:sam:arduino_due_x # arduino due + - arduino:samd:nano_33_iot # samd21 + - adafruit:samd:adafruit_metro_m4 # samd51 + - esp32:esp32:esp32 # esp32 + - esp32:esp32:esp32s2 # esp32s2 + - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill + - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - arduino:mbed_rp2040:pico # rpi pico + include: + - arduino-boards-fqbn: arduino:avr:uno + sketch-names: '**.ino' + required-libraries: Simple FOC + - arduino-boards-fqbn: arduino:sam:arduino_due_x + required-libraries: Simple FOC + sketch-names: '**.ino' + - arduino-boards-fqbn: arduino:samd:nano_33_iot + required-libraries: Simple FOC + sketch-names: '**.ino' + - arduino-boards-fqbn: arduino:mbed_rp2040:pico + required-libraries: Simple FOC + sketch-names: '**.ino' + - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 + platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json + required-libraries: Simple FOC + sketch-names: '**.ino' + # - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 + # platform-url: https://dl.espressif.com/dl/package_esp32_index.json + # required-libraries: Simple FOC + # sketch-names: '**.ino' + - arduino-boards-fqbn: esp32:esp32:esp32 # esp32 + platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json + required-libraries: Simple FOC + sketch-names: '**.ino' + - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 + platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json + required-libraries: Simple FOC + sketch-names: '**.ino' + - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + required-libraries: Simple FOC + sketch-names: '**.ino' + - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + required-libraries: Simple FOC + sketch-names: '**.ino' + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} diff --git a/README.md b/README.md index fb40dbf..e5d9a97 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,9 @@ # SimpleFOC Driver and Support Library +![Library Compile](https://github.com/simplefoc/Arduino-FOC-drivers/workflows/Library%20Compile/badge.svg) +[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) + + This library contains an assortment of drivers and supporting code for SimpleFOC. The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, understand and port to different platforms. In addition to this core, there are various drivers and supporting code which has grown around SimpleFOC, and which we would like to make available to the community. @@ -17,6 +21,9 @@ What's here? See the sections below. Each driver or function should come with it - [AS5048A SPI driver](src/encoders/as5048a/) - SPI driver for the AMS AS5048A absolute position magnetic rotary encoder IC. - [AS5047 SPI driver](src/encoders/as5047/) - SPI driver for the AMS AS5047P and AS5047D absolute position magnetic rotary encoder ICs. - [MA730 SPI driver](src/encoders/ma730/) - SPI driver for the MPS MagAlpha MA730 absolute position magnetic rotary encoder IC. + - [MA730 SSI driver](src/encoders/ma730/) - SSI driver for the MPS MagAlpha MA730 absolute position magnetic rotary encoder IC. + - [AS5145 SSI driver](src/encoders/as5145/) - SSI driver for the AMS AS5145 and AS5045 absolute position magnetic rotary encoder ICs. + - [TLE5012B SPI driver](src/encoders/tle5012b/) - SPI (half duplex) driver for TLE5012B absolute position magnetic rotary encoder IC. - [STM32 Hardware Encoder](src/encoders/stm32hwencoder/) - Hardware timer based encoder driver for ABI type quadrature encoders. ### Communications diff --git a/examples/drivers/drv8316/drv8316_3pwm.ino b/examples/drivers/drv8316/3pwm/drv8316_3pwm.ino similarity index 95% rename from examples/drivers/drv8316/drv8316_3pwm.ino rename to examples/drivers/drv8316/3pwm/drv8316_3pwm.ino index be9dfe1..be89c74 100644 --- a/examples/drivers/drv8316/drv8316_3pwm.ino +++ b/examples/drivers/drv8316/3pwm/drv8316_3pwm.ino @@ -4,14 +4,14 @@ #include "Arduino.h" #include #include -#include +#include #include "drivers/drv8316/drv8316.h" BLDCMotor motor = BLDCMotor(11); -DRV8316Driver3PWM driver = DRV8316Driver3PWM(A3,A4,2,7,false); // MKR1010 3-PWM +DRV8316Driver3PWM driver = DRV8316Driver3PWM(2,3,4,7,false); // use the right pins for your setup! #define ENABLE_A 0 #define ENABLE_B 1 #define ENABLE_C 6 diff --git a/examples/drivers/drv8316/drv8316_6pwm.ino b/examples/drivers/drv8316/6pwm/drv8316_6pwm.ino similarity index 95% rename from examples/drivers/drv8316/drv8316_6pwm.ino rename to examples/drivers/drv8316/6pwm/drv8316_6pwm.ino index bce3900..2eb0dd3 100644 --- a/examples/drivers/drv8316/drv8316_6pwm.ino +++ b/examples/drivers/drv8316/6pwm/drv8316_6pwm.ino @@ -4,14 +4,14 @@ #include "Arduino.h" #include #include -#include +#include #include "drivers/drv8316/drv8316.h" BLDCMotor motor = BLDCMotor(11); -DRV8316Driver6PWM driver = DRV8316Driver6PWM(A3,0,A4,1,2,6,7,false); // MKR1010 6-PWM +DRV8316Driver6PWM driver = DRV8316Driver6PWM(0,1,2,3,4,6,7,false); // use the right pins for your setup! diff --git a/keywords.txt b/keywords.txt index 1c7c07f..e99910a 100644 --- a/keywords.txt +++ b/keywords.txt @@ -1,2 +1,9 @@ SimpleFOC KEYWORD1 -DRV8316 KEYWORD1 +DRV8316 KEYWORD1 +AS5048A KEYWORD1 +AS5047 KEYWORD1 +AS5145 KEYWORD1 +MA730 KEYWORD1 +TLE5012B KEYWORD1 +I2CCommander KEYWORD1 + diff --git a/library.properties b/library.properties index 3b86d64..be32b51 100644 --- a/library.properties +++ b/library.properties @@ -7,5 +7,5 @@ paragraph=SimpleFOC runs BLDC and Stepper motors using the FOC algorithm. This l category=Device Control url=https://docs.simplefoc.com architectures=* -includes=DRV8316.h +includes=SimpleFOCDrivers.h depends=Simple FOC \ No newline at end of file diff --git a/src/comms/i2c/I2CCommander.cpp b/src/comms/i2c/I2CCommander.cpp index 59b332b..6ef5068 100644 --- a/src/comms/i2c/I2CCommander.cpp +++ b/src/comms/i2c/I2CCommander.cpp @@ -23,21 +23,6 @@ void I2CCommander::addMotor(FOCMotor* motor){ -I2CCommander_Motor_Status I2CCommander::getMotorStatus(uint8_t motorNum){ - if (motorNumzero_electric_angle==NOT_SET) // TODO detect open-loop uninitialized state! - return I2CCommander_Motor_Status::MOT_UNINITIALIZED; - if (motors[motorNum]->enabled==0) - return I2CCommander_Motor_Status::MOT_DISABLED; - if (motors[motorNum]->shaft_velocity >= I2CCOMMANDER_MIN_VELOCITY_FOR_MOTOR_MOVING) - return I2CCommander_Motor_Status::MOT_MOVING; - return I2CCommander_Motor_Status::MOT_IDLE; - } - return I2CCommander_Motor_Status::MOT_UNKNOWN; -}; - - - bool I2CCommander::readBytes(void* valueToSet, uint8_t numBytes){ if (_wire->available()>=numBytes){ @@ -85,10 +70,19 @@ void I2CCommander::onRequest(){ +/* +Reads values from I2C bus and updates the motor's values. + +Currently this isn't really thread-safe, but works ok in practice on 32-bit MCUs. +Do not use on 8-bit architectures where the 32 bit writes may not be atomic! + +Plan to make this safe: the writes should be buffered, and not actually executed +until in the main loop by calling commander->run(); +the run() method disables interrupts while the updates happen. +*/ bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int numBytes) { int val; - float floatval; switch (registerNum) { case REG_MOTOR_ADDRESS: val = _wire->read(); // reading one more byte is definately ok, since numBytes>1 @@ -230,7 +224,23 @@ bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int nu +/* + Reads values from motor/sensor and writes them to I2C bus. Intended to be run + from the Wire.onRequest interrupt. + + Assumes atomic 32 bit reads. On 8-bit arduino this assumption does not hold and this + code is not safe on those platforms. You might read "half-written" floats. + + A solution might be to maintain a complete set of shadow registers in the commander + class, and update them in the run() method (which runs with interrupts off). Not sure + of the performance impact of all those 32 bit read/writes though. In any case, since + I use only 32 bit MCUs I'll leave it as an excercise to the one who needs it. ;-) + On 32 bit platforms the implication is that reads will occur atomically, so data will + be intact, but they can occur at any time during motor updates, so different values might + not be in a fully consistent state (i.e. phase A current might be from the current iteration + but phase B current from the previous iteration). +*/ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) { // read the current register switch(registerNum) { @@ -239,8 +249,7 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) { _wire->write((uint8_t)lastcommandregister); _wire->write((uint8_t)lastcommanderror+1); for (int i=0;(iwrite(status); + _wire->write(motors[motorNum]->motor_status); } break; case REG_MOTOR_ADDRESS: @@ -377,7 +386,8 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) { writeFloat(motors[motorNum]->current_limit); break; case REG_MOTION_DOWNSAMPLE: - _wire->write((uint32_t)motors[motorNum]->motion_downsample); + _wire->write((int)motors[motorNum]->motion_downsample); // TODO int will have different sizes on different platforms + // but using uint32 doesn't compile clean on all, e.g. RP2040 break; case REG_ZERO_ELECTRIC_ANGLE: @@ -393,11 +403,12 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) { writeFloat(motors[motorNum]->phase_resistance); break; case REG_POLE_PAIRS: - _wire->write((uint32_t)motors[motorNum]->pole_pairs); + _wire->write((int)motors[motorNum]->pole_pairs); break; case REG_SYS_TIME: - _wire->write(millis()); + // TODO how big is millis()? Same on all platforms? + _wire->write((int)millis()); break; case REG_NUM_MOTORS: _wire->write(numMotors); diff --git a/src/comms/i2c/I2CCommander.h b/src/comms/i2c/I2CCommander.h index 57c0e43..2cb467a 100644 --- a/src/comms/i2c/I2CCommander.h +++ b/src/comms/i2c/I2CCommander.h @@ -26,8 +26,6 @@ class I2CCommander { void onReceive(int numBytes); void onRequest(); - I2CCommander_Motor_Status getMotorStatus(uint8_t motorNum); - protected: void writeFloat(float value); bool readBytes(void* valueToSet, uint8_t numBytes); diff --git a/src/comms/i2c/I2CCommanderRegisters.h b/src/comms/i2c/I2CCommanderRegisters.h index 0977e89..cee8b9b 100644 --- a/src/comms/i2c/I2CCommanderRegisters.h +++ b/src/comms/i2c/I2CCommanderRegisters.h @@ -1,4 +1,3 @@ - #ifndef __I2CCOMMANDERREGISTERS_H__ #define __I2CCOMMANDERREGISTERS_H__ @@ -62,23 +61,6 @@ typedef enum : uint8_t { -// TODO stati are work in progress. For example, it would be good to differentiate between not moving because stalled, -// and not moving because target set-point has been reached. It would be good to be able to track initialization state -// and error state. -typedef enum : uint8_t { - MOT_UNINITIALIZED = 0x00, // Motor is not initialized - MOT_INITIALIZING = 0x01, // TODO - this status is not used at the moment, can't detect it - - MOT_DISABLED = 0x02, // motor is disabled - MOT_IDLE = 0x03, // motor is enabled, but not moving (current_velocity==target==0 in velocity mode, - // or position==target in position mode) - MOT_MOVING = 0x04, // motor is moving - - MOT_ERROR = 0x05, // TODO - motor is disabled because an error has occurred - MOT_INIT_FAILED = 0x0F, // motor initialization failed - MOT_UNKNOWN = 0xFF // incorrect motor number -} I2CCommander_Motor_Status; - #endif \ No newline at end of file diff --git a/src/comms/i2c/README.md b/src/comms/i2c/README.md index 98cf1eb..eac9e34 100644 --- a/src/comms/i2c/README.md +++ b/src/comms/i2c/README.md @@ -9,6 +9,10 @@ This code takes the point of view that the motor driver (the "muscle") is the I2 This is new code, and has not been extensively tested. Your milage may vary. That said, basic use cases have been tested, and we would certainly appreciate feedback and help with testing it out. +In particular, there are concurrency issues with reading/writing the SimpleFOC motor values from I2C while the motor is running. These should be solved soon in an upcoming version. + +**Do not run on 8-bit MCUs!** The code currently assumes atomic 32 bit reads, so running on Arduino UNO or Nano is unfortunately a no-go. + ## Using As would be expected for I2C, each target device needs a unique I2C address on its bus, and setting up and discovering these addresses is out-of-scope for I2CCommander. Setting up and configuring the TwoWire objects (which pins, speed, etc...) is also out of scope and finished, initialized TwoWire objects must be passed to I2CCommander. If you don't specify a different reference, the standard *Wire* object is assumed. diff --git a/src/drivers/drv8316/drv8316.h b/src/drivers/drv8316/drv8316.h index 7f09cd0..dfec32e 100644 --- a/src/drivers/drv8316/drv8316.h +++ b/src/drivers/drv8316/drv8316.h @@ -21,8 +21,8 @@ enum DRV8316_PWMMode { enum DRV8316_SDOMode { - OpenDrain = 0b0, - PushPull = 0b1 + SDOMode_OpenDrain = 0b0, + SDOMode_PushPull = 0b1 }; diff --git a/src/encoders/aeat8800q24/AEAT8800Q24.cpp b/src/encoders/aeat8800q24/AEAT8800Q24.cpp new file mode 100644 index 0000000..5c9c235 --- /dev/null +++ b/src/encoders/aeat8800q24/AEAT8800Q24.cpp @@ -0,0 +1,102 @@ + +#include "AEAT8800Q24.h" + + +AEAT8800Q24::AEAT8800Q24(int nCS, int pinNSL, SPISettings spiSettings, SPISettings ssiSettings) : nCS(nCS), pinNSL(pinNSL), spiSettings(spiSettings), ssiSettings(ssiSettings) { +}; +AEAT8800Q24::~AEAT8800Q24(){ +}; + +void AEAT8800Q24::init(SPIClass* _spi){ + spi = _spi; + spi->beginTransaction(ssiSettings); +}; + + +float AEAT8800Q24::getCurrentAngle() { + return ((float)readRawAngle())/(float)AEAT8800Q24_CPR * 2.0f * (float)PI; +}; // angle in radians, return current value + + +// reads angle via ssi +uint16_t AEAT8800Q24::readRawAngle() { + // digitalWrite(pinNSL, LOW); //TODO maybe we don't need it... + // // 300ns delay + // delayMicroseconds(1); + uint16_t value = spi->transfer16(0x0000); + uint8_t status = spi->transfer(0x00); + //digitalWrite(pinNSL, HIGH); + // // 200ns delay before next read... + // delayMicroseconds(1); + lastStatus.reg = status; + return value; +}; + +AEAT8800Q24_Status_t AEAT8800Q24::getLastStatus() { + return lastStatus; +} + +uint16_t AEAT8800Q24::getZero(){ + uint8_t value = readRegister(AEAT8800Q24_REG_ZERO_MSB); + return (value<<8)|readRegister(AEAT8800Q24_REG_ZERO_LSB); +}; +void AEAT8800Q24::setZero(uint16_t value){ + writeRegister(AEAT8800Q24_REG_ZERO_MSB, (value>>8)&0xFF); + writeRegister(AEAT8800Q24_REG_ZERO_LSB, value&0xFF); +}; + +AEAT8800Q24_CONF0_t AEAT8800Q24::getConf0(){ + AEAT8800Q24_CONF0_t result; + result.reg = readRegister(AEAT8800Q24_REG_CONF0); + return result; +}; +void AEAT8800Q24::setConf0(AEAT8800Q24_CONF0_t value){ + writeRegister(AEAT8800Q24_REG_CONF0, value.reg); +}; + +AEAT8800Q24_CONF1_t AEAT8800Q24::getConf1(){ + AEAT8800Q24_CONF1_t result; + result.reg = readRegister(AEAT8800Q24_REG_CONF1); + return result; +}; +void AEAT8800Q24::setConf1(AEAT8800Q24_CONF1_t value){ + writeRegister(AEAT8800Q24_REG_CONF1, value.reg); +}; + +AEAT8800Q24_CONF2_t AEAT8800Q24::getConf2(){ + AEAT8800Q24_CONF2_t result; + result.reg = readRegister(AEAT8800Q24_REG_CONF2); + return result; +}; +void AEAT8800Q24::setConf2(AEAT8800Q24_CONF2_t value){ + writeRegister(AEAT8800Q24_REG_CONF2, value.reg); +}; + + + + +uint16_t AEAT8800Q24::transfer16SPI(uint16_t outValue) { + // delay 1us between switching the CS line to SPI + delayMicroseconds(1); + if (nCS >= 0) + digitalWrite(nCS, LOW); + spi->endTransaction(); + spi->beginTransaction(spiSettings); + uint16_t value = spi->transfer16(outValue); + spi->endTransaction(); + if (nCS >= 0) + digitalWrite(nCS, HIGH); + // delay 1us between switching the CS line to SSI + delayMicroseconds(1); + spi->beginTransaction(ssiSettings); + return value; +}; +uint8_t AEAT8800Q24::readRegister(uint8_t reg) { + uint16_t cmd = 0x8000 | ((reg&0x001F)<<8); + uint16_t value = transfer16SPI(cmd); + return value&0x00FF; +}; +void AEAT8800Q24::writeRegister(uint8_t reg, uint8_t value) { + uint16_t cmd = 0x4000 | ((reg&0x1F)<<8) | value; + uint16_t result = transfer16SPI(cmd); +}; \ No newline at end of file diff --git a/src/encoders/aeat8800q24/AEAT8800Q24.h b/src/encoders/aeat8800q24/AEAT8800Q24.h new file mode 100644 index 0000000..4acfe6d --- /dev/null +++ b/src/encoders/aeat8800q24/AEAT8800Q24.h @@ -0,0 +1,119 @@ +#ifndef __AEAT8800Q24_H__ +#define __AEAT8800Q24_H__ + + +#include "Arduino.h" +#include "SPI.h" + +#define AEAT8800Q24_CPR 65536 + +#define AEAT8800Q24_REG_CUST0 0x00 +#define AEAT8800Q24_REG_CUST1 0x01 +#define AEAT8800Q24_REG_ZERO_LSB 0x02 +#define AEAT8800Q24_REG_ZERO_MSB 0x03 +#define AEAT8800Q24_REG_CONF0 0x04 +#define AEAT8800Q24_REG_CONF1 0x05 +#define AEAT8800Q24_REG_CONF2 0x06 +#define AEAT8800Q24_REG_CONF_OTP 0x13 +#define AEAT8800Q24_REG_CAL_OTP 0x1B +#define AEAT8800Q24_REG_CUST_OTP 0x11 +#define AEAT8800Q24_REG_CAL 0x17 + +#define AEAT8800Q24_CONF_OTP_ON 0xA3 +#define AEAT8800Q24_CAL_OTP_ON 0xA5 +#define AEAT8800Q24_CUST_OTP_ON 0xA1 +#define AEAT8800Q24_CAL_ON 0x02 +#define AEAT8800Q24_CAL_OFF 0x00 + + +typedef union { + struct { + uint8_t uvw_pwm_mode:1; + uint8_t pwm:2; + uint8_t iwidth:2; + uint8_t uvw:3; + }; + uint8_t reg; +} AEAT8800Q24_CONF0_t; + + +typedef union { + struct { + uint8_t cpr1:4; + uint8_t hys:4; + }; + uint8_t reg; +} AEAT8800Q24_CONF1_t; + + +typedef union { + struct { + uint8_t dir:1; + uint8_t zero_latency:1; + uint8_t abs_res:2; + uint8_t cpr2:4; + }; + uint8_t reg; +} AEAT8800Q24_CONF2_t; + + + +typedef union { + struct { + uint8_t mhi:1; + uint8_t mlo:1; + uint8_t ready:1; + uint8_t parity:1; + uint8_t :4; + }; + uint8_t reg; +} AEAT8800Q24_Status_t; + + +#ifndef MSBFIRST +#define MSBFIRST BitOrder::MSBFIRST +#endif + +static SPISettings AEAT8800Q24SPISettings(1000000, MSBFIRST, SPI_MODE3); // @suppress("Invalid arguments") +static SPISettings AEAT8800Q24SSISettings(4000000, MSBFIRST, SPI_MODE2); // @suppress("Invalid arguments") + + +class AEAT8800Q24 { +public: + AEAT8800Q24(int nCS, int pinNSL = MOSI, SPISettings spiSettings = AEAT8800Q24SPISettings, SPISettings ssiSettings = AEAT8800Q24SSISettings); + virtual ~AEAT8800Q24(); + + virtual void init(SPIClass* _spi = &SPI); + + float getCurrentAngle(); // angle in radians, return current value + + uint16_t readRawAngle(); // 14bit angle value + AEAT8800Q24_Status_t getLastStatus(); // get status associated with last angle read + + uint16_t getZero(); + void setZero(uint16_t); + + AEAT8800Q24_CONF0_t getConf0(); + void setConf0(AEAT8800Q24_CONF0_t); + + AEAT8800Q24_CONF1_t getConf1(); + void setConf1(AEAT8800Q24_CONF1_t); + + AEAT8800Q24_CONF2_t getConf2(); + void setConf2(AEAT8800Q24_CONF2_t); + +private: + int nCS = -1; + int pinNSL = -1; + SPISettings spiSettings; + SPISettings ssiSettings; + SPIClass* spi; + AEAT8800Q24_Status_t lastStatus; + + uint16_t transfer16SPI(uint16_t outValue); + uint8_t readRegister(uint8_t reg); + void writeRegister(uint8_t reg, uint8_t value); +}; + + +#endif \ No newline at end of file diff --git a/src/encoders/aeat8800q24/MagneticSensorAEAT8800Q24.cpp b/src/encoders/aeat8800q24/MagneticSensorAEAT8800Q24.cpp new file mode 100644 index 0000000..f1ad32c --- /dev/null +++ b/src/encoders/aeat8800q24/MagneticSensorAEAT8800Q24.cpp @@ -0,0 +1,24 @@ +#include "./MagneticSensorAEAT8800Q24.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" + +MagneticSensorAEAT8800Q24::MagneticSensorAEAT8800Q24(int nCS, int pinNSL, SPISettings spiSettings, SPISettings ssiSettings) : AEAT8800Q24(nCS, pinNSL, spiSettings, ssiSettings) { +}; + + +MagneticSensorAEAT8800Q24::~MagneticSensorAEAT8800Q24(){ +}; + + +void MagneticSensorAEAT8800Q24::init(SPIClass* _spi) { + this->AEAT8800Q24::init(_spi); + this->Sensor::init(); +}; + + +float MagneticSensorAEAT8800Q24::getSensorAngle() { + float angle_data = readRawAngle(); + angle_data = ( angle_data / (float)AEAT8800Q24_CPR) * _2PI; + // return the shaft angle + return angle_data; +}; diff --git a/src/encoders/aeat8800q24/MagneticSensorAEAT8800Q24.h b/src/encoders/aeat8800q24/MagneticSensorAEAT8800Q24.h new file mode 100644 index 0000000..01d6e04 --- /dev/null +++ b/src/encoders/aeat8800q24/MagneticSensorAEAT8800Q24.h @@ -0,0 +1,18 @@ +#ifndef __MAGNETICSENSORAEAT8800Q24_H__ +#define __MAGNETICSENSORAEAT8800Q24_H__ + + +#include "common/base_classes/Sensor.h" +#include "./AEAT8800Q24.h" + +class MagneticSensorAEAT8800Q24 : public Sensor, public AEAT8800Q24 { +public: + MagneticSensorAEAT8800Q24(int nCS, int pinNSL = MOSI, SPISettings spiSettings = AEAT8800Q24SPISettings, SPISettings ssiSettings = AEAT8800Q24SSISettings); + virtual ~MagneticSensorAEAT8800Q24(); + + virtual float getSensorAngle() override; + + virtual void init(SPIClass* _spi = &SPI); +}; + +#endif \ No newline at end of file diff --git a/src/encoders/aeat8800q24/README.md b/src/encoders/aeat8800q24/README.md new file mode 100644 index 0000000..0afa47d --- /dev/null +++ b/src/encoders/aeat8800q24/README.md @@ -0,0 +1,75 @@ +# AEAT-8800-Q24 SimpleFOC driver + +SPI/SSI driver for the absolute position magnetic rotary encoder. This encoder is not supported by the +normal SimpleFOC drivers due to its mixed SPI/SSI mode. + +- access to the configuration registers of the AEAT-8800-Q24, enabling you to set parameters +- angles are read via SSI, with 16 bit (!) precision +- currently only 16 bit resolution is supported, don't lower the resolution + + +## Hardware setup + +Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver. nCS pin is +required. + +Note that due to the way SSI and SPI share pins, you can normally only run one of these sensors per SPI bus. + +## Software setup + +Its actually easier to use than the standard SPI sensor class, because it is less generic: + +```c++ +#include "Arduino.h" +#include "Wire.h" +#include "SPI.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "encoders/aeat8800q24/MagneticSensorAEAT8800Q24.h" + +#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin +MagneticSensorAEAT8800Q24 sensor1(SENSOR1_CS); + + +void setup() { + sensor1.init(); +} +``` + +Set some options: + +```c++ +MagneticSensorAEAT8800Q24 sensor1(SENSOR1_CS, MOSI_pin, mySPISettings, mySSISettings); +``` + +Use another SPI bus: + +```c++ +void setup() { + sensor1.init(SPI2); +} +``` + +Here's how you can use it: + +```c++ + // get the angle, in radians, including full rotations + float a1 = sensor1.getAngle(); + + // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work + float v1 = sensor1.getVelocity(); + + // get the angle, in radians, no full rotations + float a2 = sensor1.getCurrentAngle(); + + // get the raw 16 bit value + uint16_t raw = sensor1.readRawAngle(); + + // check status + float angle = sensor1.getSensorAngle(); + AEAT8800Q24_Status_t status = sensor1.getLastStatus(); + if (status.mlo) + Serial.println("Sensor magnet low error"); + if (status.mhi) + Serial.println("Sensor magnet high error"); +``` diff --git a/src/encoders/as5047/AS5047.h b/src/encoders/as5047/AS5047.h index 4046c73..59b33a3 100644 --- a/src/encoders/as5047/AS5047.h +++ b/src/encoders/as5047/AS5047.h @@ -94,11 +94,7 @@ struct AS5047Error { #define AS5047_RESULT_MASK 0x3FFF -#if defined(ESP32) #define AS5047_BITORDER MSBFIRST -#else -#define AS5047_BITORDER BitOrder::MSBFIRST -#endif static SPISettings AS5047SPISettings(8000000, AS5047_BITORDER, SPI_MODE1); // @suppress("Invalid arguments") diff --git a/src/encoders/as5048a/AS5048A.h b/src/encoders/as5048a/AS5048A.h index afbc18a..00a4fe6 100644 --- a/src/encoders/as5048a/AS5048A.h +++ b/src/encoders/as5048a/AS5048A.h @@ -47,11 +47,7 @@ struct AS5048Error { #define AS5048A_RESULT_MASK 0x3FFF -#if defined(ESP32) #define AS5048_BITORDER MSBFIRST -#else -#define AS5048_BITORDER BitOrder::MSBFIRST -#endif static SPISettings AS5048SPISettings(8000000, AS5048_BITORDER, SPI_MODE1); // @suppress("Invalid arguments") diff --git a/src/encoders/as5145/MagneticSensorAS5145.cpp b/src/encoders/as5145/MagneticSensorAS5145.cpp new file mode 100644 index 0000000..6d60000 --- /dev/null +++ b/src/encoders/as5145/MagneticSensorAS5145.cpp @@ -0,0 +1,34 @@ +#include "./MagneticSensorAS5145.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" + +MagneticSensorAS5145::MagneticSensorAS5145(SPISettings settings) : settings(settings) { + +} + + +MagneticSensorAS5145::~MagneticSensorAS5145() { + +} + +void MagneticSensorAS5145::init(SPIClass* _spi) { + this->spi=_spi; + this->Sensor::init(); +} + +// check 40us delay between each read? +float MagneticSensorAS5145::getSensorAngle() { + float angle_data = readRawAngleSSI(); + angle_data = ( (float)angle_data / AS5145_CPR ) * _2PI; + // return the shaft angle + return angle_data; +} + + +uint16_t MagneticSensorAS5145::readRawAngleSSI() { + spi->beginTransaction(settings); + uint16_t value = spi->transfer16(0x0000); + //uint16_t parity = spi->transfer(0x00); + spi->endTransaction(); + return (value>>3)&0x1FFF; // TODO this isn't what I expected from the datasheet... maybe there's a leading 0 bit? +}; // 12bit angle value diff --git a/src/encoders/as5145/MagneticSensorAS5145.h b/src/encoders/as5145/MagneticSensorAS5145.h new file mode 100644 index 0000000..ee1f34a --- /dev/null +++ b/src/encoders/as5145/MagneticSensorAS5145.h @@ -0,0 +1,39 @@ + +#ifndef __MAGNETIC_SENSOR_AS5145_H__ +#define __MAGNETIC_SENSOR_AS5145_H__ + +#include "Arduino.h" +#include "SPI.h" +#include "common/base_classes/Sensor.h" + +#ifndef MSBFIRST +#define MSBFIRST BitOrder::MSBFIRST +#endif + +#define AS5145_BITORDER MSBFIRST +#define AS5145_CPR 4096.0f +#define _2PI 6.28318530718f + + +static SPISettings AS5145SSISettings(1000000, AS5145_BITORDER, SPI_MODE2); // @suppress("Invalid arguments") + + +class MagneticSensorAS5145 : public Sensor { +public: + MagneticSensorAS5145(SPISettings settings = AS5145SSISettings); + virtual ~MagneticSensorAS5145(); + + virtual float getSensorAngle() override; + + virtual void init(SPIClass* _spi = &SPI); + + uint16_t readRawAngleSSI(); + +private: + SPIClass* spi; + SPISettings settings; +}; + + + +#endif \ No newline at end of file diff --git a/src/encoders/as5145/README.md b/src/encoders/as5145/README.md new file mode 100644 index 0000000..f0474b9 --- /dev/null +++ b/src/encoders/as5145/README.md @@ -0,0 +1,50 @@ +# AS5145 SimpleFOC driver + +SSI protocol driver for the AMS AS5145 magnetic encoder. Any of the A, B or H variants should work. AS5045 encoders should also be supported. + +Only angle reading is supported, might get to the status bits at a later time. +The SSI protocol is "emulated" using the SPI peripheral. + +Tested with AS5145A on STM32G491 so far. + +## Hardware setup + +Wire the sensor's data (DO) line to the MISO (CIPO) pin, nCS, SCK as normal. Leave the MOSI pin unconnected. + +## Software setup + +``` +#include +#include +#include +#include "encoders/as5145/MagneticSensorAS5145.h" + +MagneticSensorAS5145 sensor; +SPIClass spi_ssi(PB15, PB14, PB13, PB12); + +long ts; + +void setup() { + Serial.begin(115200); + while (!Serial) ; + delay(2000); + + Serial.println("Initializing sensor..."); + + spi_ssi.begin(); + sensor.init(&spi_ssi); + + Serial.println("Sensor initialized."); + + ts = millis(); +} + +void loop() { + sensor.update(); + if (millis() - ts > 1000) { + Serial.println(sensor.getAngle(), 3); + ts = millis(); + } + delay(1); +} +``` \ No newline at end of file diff --git a/src/encoders/ma730/MA730.cpp b/src/encoders/ma730/MA730.cpp index 17979d7..885e335 100644 --- a/src/encoders/ma730/MA730.cpp +++ b/src/encoders/ma730/MA730.cpp @@ -16,12 +16,12 @@ void MA730::init(SPIClass* _spi) { }; float MA730::getCurrentAngle() { - return ((float)readRawAngle())/(float)MA730_CPR * 2.0f * (float)PI; + return (readRawAngle() * _2PI)/MA730_CPR; }; // angle in radians, return current value uint16_t MA730::readRawAngle() { uint16_t angle = transfer16(0x0000); - return angle>>2; + return angle; }; // 14bit angle value uint16_t MA730::getZero() { diff --git a/src/encoders/ma730/MA730.h b/src/encoders/ma730/MA730.h index e32c37d..00c6e48 100644 --- a/src/encoders/ma730/MA730.h +++ b/src/encoders/ma730/MA730.h @@ -13,7 +13,8 @@ enum FieldStrength { }; -#define MA730_CPR 16384 +#define _2PI 6.28318530718f +#define MA730_CPR 65536.0f #define MA730_REG_ZERO_POSITION_LSB 0x00 #define MA730_REG_ZERO_POSITION_MSB 0x01 @@ -25,13 +26,10 @@ enum FieldStrength { #define MA730_REG_RD 0x09 #define MA730_REG_MGH_MGL 0x1B -#if defined(ESP32) #define MA730_BITORDER MSBFIRST -#else -#define MA730_BITORDER BitOrder::MSBFIRST -#endif -static SPISettings MA730SPISettings(8000000, MA730_BITORDER, SPI_MODE3); // @suppress("Invalid arguments") +static SPISettings MA730SPISettings(1000000, MA730_BITORDER, SPI_MODE3); // @suppress("Invalid arguments") +static SPISettings MA730SSISettings(4000000, MA730_BITORDER, SPI_MODE1); // @suppress("Invalid arguments") class MA730 { @@ -44,6 +42,7 @@ class MA730 { float getCurrentAngle(); // angle in radians, return current value uint16_t readRawAngle(); // 14bit angle value + uint16_t readRawAngleSSI(); // 14bit angle value uint16_t getZero(); uint8_t getBiasCurrentTrimming(); diff --git a/src/encoders/ma730/MagneticSensorMA730SSI.cpp b/src/encoders/ma730/MagneticSensorMA730SSI.cpp new file mode 100644 index 0000000..bf87b2b --- /dev/null +++ b/src/encoders/ma730/MagneticSensorMA730SSI.cpp @@ -0,0 +1,34 @@ +#include "./MagneticSensorMA730SSI.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" + +MagneticSensorMA730SSI::MagneticSensorMA730SSI(SPISettings settings) : settings(settings) { + +} + + +MagneticSensorMA730SSI::~MagneticSensorMA730SSI() { + +} + +void MagneticSensorMA730SSI::init(SPIClass* _spi) { + this->spi=_spi; + this->Sensor::init(); +} + +// check 40us delay between each read? +float MagneticSensorMA730SSI::getSensorAngle() { + float angle_data = readRawAngleSSI(); + angle_data = ( angle_data / (float)MA730_CPR ) * _2PI; + // return the shaft angle + return angle_data; +} + + +uint16_t MagneticSensorMA730SSI::readRawAngleSSI() { + spi->beginTransaction(settings); + uint16_t value = spi->transfer16(0x0000); + //uint16_t parity = spi->transfer(0x00); + spi->endTransaction(); + return (value<<1); //>>1)&0x3FFF; +}; // 14bit angle value diff --git a/src/encoders/ma730/MagneticSensorMA730SSI.h b/src/encoders/ma730/MagneticSensorMA730SSI.h new file mode 100644 index 0000000..f45071f --- /dev/null +++ b/src/encoders/ma730/MagneticSensorMA730SSI.h @@ -0,0 +1,25 @@ +#ifndef __MAGNETIC_SENSOR_MA730SSI_H__ +#define __MAGNETIC_SENSOR_MA730SSI_H__ + + +#include "common/base_classes/Sensor.h" +#include "./MA730.h" + +class MagneticSensorMA730SSI : public Sensor { +public: + MagneticSensorMA730SSI(SPISettings settings = MA730SSISettings); + virtual ~MagneticSensorMA730SSI(); + + virtual float getSensorAngle() override; + + virtual void init(SPIClass* _spi = &SPI); + + uint16_t readRawAngleSSI(); + +private: + SPIClass* spi; + SPISettings settings; +}; + + +#endif \ No newline at end of file diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp index 6ee03ca..f911e00 100644 --- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp +++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp @@ -112,7 +112,9 @@ void STM32HWEncoder::init() { gpio.Mode = GPIO_MODE_AF_PP; gpio.Pull = GPIO_NOPULL; gpio.Speed = GPIO_SPEED_FREQ_MEDIUM; + #ifndef STM32F1xx_HAL_GPIO_H gpio.Alternate = pinmap_function(pinA, PinMap_PWM); + #endif HAL_GPIO_Init(digitalPinToPort(_pinA), &gpio); // lets assume pinB is on the same timer as pinA... otherwise it can't work but the API currently doesn't allow us to fail gracefully @@ -120,11 +122,13 @@ void STM32HWEncoder::init() { gpio.Mode = GPIO_MODE_AF_PP; gpio.Pull = GPIO_NOPULL; gpio.Speed = GPIO_SPEED_FREQ_MEDIUM; + #ifndef STM32F1xx_HAL_GPIO_H gpio.Alternate = pinmap_function(digitalPinToPinName(_pinB), PinMap_PWM); + #endif HAL_GPIO_Init(digitalPinToPort(_pinB), &gpio); // set up timer for encoder - encoder_handle.Init.Period = ticks_per_overflow; + encoder_handle.Init.Period = ticks_per_overflow - 1; encoder_handle.Init.Prescaler = 0; encoder_handle.Init.ClockDivision = 0; encoder_handle.Init.CounterMode = TIM_COUNTERMODE_UP; diff --git a/src/encoders/tle5012b/MagneticSensorTLE5012B.cpp b/src/encoders/tle5012b/MagneticSensorTLE5012B.cpp new file mode 100644 index 0000000..5947209 --- /dev/null +++ b/src/encoders/tle5012b/MagneticSensorTLE5012B.cpp @@ -0,0 +1,18 @@ + +#include "./MagneticSensorTLE5012B.h" + +#if defined(_STM32_DEF_) + +MagneticSensorTLE5012B::MagneticSensorTLE5012B(int data, int sck, int nCS) : TLE5012B(data, sck, nCS) { }; +MagneticSensorTLE5012B::~MagneticSensorTLE5012B(){ }; + +void MagneticSensorTLE5012B::init() { + this->TLE5012B::init(); + this->Sensor::init(); +}; + +float MagneticSensorTLE5012B::getSensorAngle() { + return getCurrentAngle(); +}; + +#endif diff --git a/src/encoders/tle5012b/MagneticSensorTLE5012B.h b/src/encoders/tle5012b/MagneticSensorTLE5012B.h new file mode 100644 index 0000000..712c896 --- /dev/null +++ b/src/encoders/tle5012b/MagneticSensorTLE5012B.h @@ -0,0 +1,25 @@ + +#ifndef __MAGNETIC_SENSOR_TLE5012B_H__ +#define __MAGNETIC_SENSOR_TLE5012B_H__ + +#include "./STM32TLE5012B.h" + +#if defined(_STM32_DEF_) + +#include "common/base_classes/Sensor.h" + + + + +class MagneticSensorTLE5012B : public Sensor, public TLE5012B { + public: + MagneticSensorTLE5012B(int data, int sck, int nCS); + ~MagneticSensorTLE5012B(); + virtual void init() override; + virtual float getSensorAngle() override; +}; + + + +#endif +#endif diff --git a/src/encoders/tle5012b/README.md b/src/encoders/tle5012b/README.md new file mode 100644 index 0000000..3ce22c2 --- /dev/null +++ b/src/encoders/tle5012b/README.md @@ -0,0 +1,91 @@ +# SimpleFOC Driver for TLE5012B encoder for STM32 MCUs + +This driver code is compatible with STM32 MCUs. Based loosely on code from [here](https://github.com/xerootg/btt-s42b-simplefoc), with many thanks to @xerootg + +## Hardware setup + +Connect the data pin of the sensor to the MOSI (COPI) line of the MCU. The SCLK and nCS lines are connected as normal. + +## Software setup + +Sample code: + +``` +#include +#include +#include +#include "encoders/tle5012b/MagneticSensorTLE5012B.h" + +MagneticSensorTLE5012B sensor(PB15,PB13,PB12); + +long ts; + +void setup() { + Serial.begin(115200); + while (!Serial) ; + delay(2000); + + Serial.println("Initializing sensor..."); + + sensor.init(); + + Serial.println("Sensor initialized."); + + ts = millis(); +} + +void loop() { + sensor.update(); + if (millis() - ts > 1000) { + Serial.println(sensor.getAngle(), 3); + ts = millis(); + } + delay(1); +} +``` + +## Other MCUs + +Infineon provides a driver library compatible with Atmel AVR (Arduino UNO, Nano, etc...) and Infineon XMC MCUs. If you have one of those MCUs, you can use this library in conjunction with the GenericSensor class in SimpleFOC. + +``` +#include +#include + +Tle5012Ino Tle5012Sensor = Tle5012Ino(); +errorTypes checkError = NO_ERROR; + +float readMySensor(){ + // read my sensor + // return the angle value in radians in between 0 and 2PI + + Tle5012Sensor.getAngleValue(d); + d = (d + 180); + if ( d + 40.9 > 360 ) { + d = (d + 40.9) - 360; + } + else + d = d + 40.9; + + d = d * 0.0174533; + return d; +} +void initMySensor(){ + // do the init + checkError = Tle5012Sensor.begin(); +} + +// empty constructor +GenericSensor sensor = GenericSensor(readMySensor, initMySensor); + +void setup(){ +... + //init sensor and use it further with foc... + sensor.init(); +... +} +``` + +Other MCUs are currently not supported. The problem is that the sensor uses SPI in half-duplex mode, which is not supported by Arduino framework. So for each MCU type a custom driver has to be written to get the half-duplex SPI communication to work. There are currently no plans to support other MCUs on our side, but we will gladly accept pull requests :-) + +If you can find other libraries for other MCUs, you can use the GenericSensor approach to integrate them. \ No newline at end of file diff --git a/src/encoders/tle5012b/STM32TLE5012B.cpp b/src/encoders/tle5012b/STM32TLE5012B.cpp new file mode 100644 index 0000000..7fd89c7 --- /dev/null +++ b/src/encoders/tle5012b/STM32TLE5012B.cpp @@ -0,0 +1,159 @@ + +#include "STM32TLE5012B.h" + +#if defined(_STM32_DEF_) + + +#include "utility/spi_com.h" +extern "C" uint32_t spi_getClkFreqInst(SPI_TypeDef *spi_inst); + + + +TLE5012B::TLE5012B(int data, int sck, int nCS, uint32_t freq) { + _data = data; + _sck = sck; + _nCS = nCS; + _freq = freq; +}; + +TLE5012B::~TLE5012B() { + +}; + + +void TLE5012B::init() { + pinMode(_nCS, OUTPUT); + digitalWrite(_nCS, HIGH); + + // initialize pins + GPIO_InitTypeDef gpio; + gpio.Pin = digitalPinToBitMask(_data); + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(digitalPinToPort(_data), &gpio); + gpio.Pin = digitalPinToBitMask(_sck); + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(digitalPinToPort(_sck), &gpio); + + SPI_TypeDef *spi_data = (SPI_TypeDef*)pinmap_peripheral(digitalPinToPinName(_data), PinMap_SPI_MOSI); + SPI_TypeDef *spi_sclk = (SPI_TypeDef*)pinmap_peripheral(digitalPinToPinName(_sck), PinMap_SPI_SCLK); + SPI_TypeDef *spi_inst = (SPI_TypeDef*)pinmap_merge_peripheral(spi_data, spi_sclk); + + pinmap_pinout(digitalPinToPinName(_data), PinMap_SPI_MOSI); + pinmap_pinout(digitalPinToPinName(_sck), PinMap_SPI_SCLK); + + #if defined SPI1_BASE + if (spi_inst == SPI1) { + __HAL_RCC_SPI1_CLK_ENABLE(); + __HAL_RCC_SPI1_FORCE_RESET(); + __HAL_RCC_SPI1_RELEASE_RESET(); + } + #endif + + #if defined SPI2_BASE + if (spi_inst == SPI2) { + __HAL_RCC_SPI2_CLK_ENABLE(); + __HAL_RCC_SPI2_FORCE_RESET(); + __HAL_RCC_SPI2_RELEASE_RESET(); + } + #endif + + #if defined SPI3_BASE + if (spi_inst == SPI3) { + __HAL_RCC_SPI3_CLK_ENABLE(); + __HAL_RCC_SPI3_FORCE_RESET(); + __HAL_RCC_SPI3_RELEASE_RESET(); + } + #endif + + #if defined SPI4_BASE + if (spi_inst == SPI4) { + __HAL_RCC_SPI4_CLK_ENABLE(); + __HAL_RCC_SPI4_FORCE_RESET(); + __HAL_RCC_SPI4_RELEASE_RESET(); + } + #endif + + #if defined SPI5_BASE + if (spi_inst == SPI5) { + __HAL_RCC_SPI5_CLK_ENABLE(); + __HAL_RCC_SPI5_FORCE_RESET(); + __HAL_RCC_SPI5_RELEASE_RESET(); + } + #endif + + #if defined SPI6_BASE + if (spi_inst == SPI6) { + __HAL_RCC_SPI6_CLK_ENABLE(); + __HAL_RCC_SPI6_FORCE_RESET(); + __HAL_RCC_SPI6_RELEASE_RESET(); + } + #endif + + _spi.Instance = spi_inst; + _spi.Init.Direction = SPI_DIRECTION_1LINE; + _spi.Init.Mode = SPI_MODE_MASTER; + _spi.Init.DataSize = SPI_DATASIZE_8BIT; + _spi.Init.CLKPolarity = SPI_POLARITY_LOW; + _spi.Init.CLKPhase = SPI_PHASE_2EDGE; + _spi.Init.NSS = SPI_NSS_SOFT; + _spi.Init.FirstBit = SPI_FIRSTBIT_MSB; + _spi.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + _spi.Init.CRCPolynomial = 7; + _spi.Init.TIMode = SPI_TIMODE_DISABLE; + #if defined(SPI_NSS_PULSE_DISABLE) + _spi.Init.NSSPMode = SPI_NSS_PULSE_DISABLE; + #endif + + uint32_t spi_freq = spi_getClkFreqInst(spi_inst); + if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV2_MHZ)) { + _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2; + } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV4_MHZ)) { + _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4; + } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV8_MHZ)) { + _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8; + } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV16_MHZ)) { + _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16; + } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV32_MHZ)) { + _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32; + } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV64_MHZ)) { + _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64; + } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV128_MHZ)) { + _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128; + } else { + _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; + } + + if (HAL_SPI_Init(&_spi) != HAL_OK) { + // setup error + Serial.println("TLE5012B setup error"); + } +}; + +uint16_t TLE5012B::readRawAngle() { + uint8_t data[4]; + readBytes(TLE5012B_ANGLE_REG, data, 2); + return (((uint16_t)data[0] << 8) | data[1]) & 0x7FFF; +}; + + +float TLE5012B::getCurrentAngle() { + return ((float)readRawAngle())/TLE5012B_CPR * _2PI; +}; // angle in radians, return current value + + +void TLE5012B::readBytes(uint16_t reg, uint8_t *data, uint8_t len) { + digitalWrite(_nCS, LOW); + + reg |= TLE5012B_READ_REGISTER + (len>>1); + uint8_t txbuffer[2] = { (uint8_t)(reg >> 8), (uint8_t)(reg & 0x00FF) }; + HAL_SPI_Transmit(&_spi, txbuffer, 2, 100); // TODO check return value for error, timeout + //delayMicroseconds(1); + HAL_SPI_Receive(&_spi, data, len + 2, 100); + + digitalWrite(_nCS, HIGH); +}; + + +#endif diff --git a/src/encoders/tle5012b/STM32TLE5012B.h b/src/encoders/tle5012b/STM32TLE5012B.h new file mode 100644 index 0000000..6ae6c65 --- /dev/null +++ b/src/encoders/tle5012b/STM32TLE5012B.h @@ -0,0 +1,45 @@ +#ifndef __TLE5012B_H__ +#define __TLE5012B_H__ + +#include "Arduino.h" + +#if defined(_STM32_DEF_) + + +#define _2PI 6.28318530718f +#define TLE5012B_CPR 32768.0f + +#define TLE5012B_READ_REGISTER 0x8000 + +enum TLE5012B_Register : uint16_t { + TLE5012B_ANGLE_REG = 0x0020, + TLE5012B_SPEED_REG = 0x0030 +}; + + + +class TLE5012B { + public: + TLE5012B(int data, int sck, int nCS, uint32_t freq = 1000000); + ~TLE5012B(); + virtual void init(); + + uint16_t readRawAngle(); + float getCurrentAngle(); // angle in radians, return current value + private: + + void readBytes(uint16_t reg, uint8_t *data, uint8_t len); + + int _data; + int _sck; + int _nCS; + uint32_t _freq; + SPI_HandleTypeDef _spi; +}; + + + + +#endif + +#endif \ No newline at end of file