diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index bd74689..c2aae10 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -46,11 +46,11 @@ jobs: # 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 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json required-libraries: Simple FOC sketches-exclude: calibrated smoothing - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 - platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json required-libraries: Simple FOC sketches-exclude: calibrated smoothing - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 diff --git a/README.md b/README.md index e5f571d..69923ec 100644 --- a/README.md +++ b/README.md @@ -10,17 +10,14 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un ## New Release -v1.0.5 - Released July 2023, for Simple FOC 2.3.1 +v1.0.6 - Released December 2023, for Simple FOC 2.3.2 or later -What's changed since 1.0.4? -- Added smoothing sensor by [@dekutree64](https://github.com/dekutree64) -- Added TMD6200 SPI driver by [@YaseenTwati](https://github.com/YaseenTwati) -- Added HybridStepperMotor by [@VIPQualityPost](https://github.com/VIPQualityPost) -- New Settings abstraction to load and save SimpleFOC settings and calibration -- New Settings driver: SAMDNVMSettingsStorage -- SimpleFOCRegisters abstraction, mapping SimpleFOC parameters to virtual "Registers" -- Updated I2CCommander to use the new registers abstraction -- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.5+) +What's changed since 1.0.5? +- Added STSPIN32G4 driver +- Added STM32G4 CORDIC code, for greatly accellerated trig functions on supported MCUs +- Added STM32FlashSettingsStorage driver, supporting STM32G4 MCUs +- Improvements in the MT6835 sensor driver +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6+) ## What is included @@ -30,12 +27,14 @@ What is here? See the sections below. Each driver or function should come with i - [TMC6200 driver](src/drivers/tmc6200/) - SPI driver for Trinamics TMC6200 motor driver IC. - [DRV8316 driver](src/drivers/drv8316/) - SPI driver for TI's DRV8316 motor driver IC. + - [STSPIN32G4 driver](src/drivers/stspin32g4/) - I2C and BLDCDriver for the STSPIN32G4 integrated gate driver MCU. ### Encoders - [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. - [AS5047U SPI driver](src/encoders/as5047u/) - SPI driver for the AMS AS5047U absolute position magnetic rotary encoder ICs. + - [AS5600 I2C driver](src/encoders/as5600/) - I2C driver for the AMS AS5600 and AS5600L 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. @@ -63,11 +62,19 @@ Load and store SimpleFOC motor settings, based on register abstraction. - [SAMD NVM storage driver](src/settings/samd/) - Store settings to the NVM flash memory in your SAMD MCU - [CAT24 I2C EEPROM storage driver](src/settings/i2c/) - Store settings to CAT24 I2C EEPROMs + - [STM32 flash storage driver](src/settings/stm32/) - Store settings directly to STM32 on-board flash, currently supporting STM32G4 MCUs. + +### Motor classes + +Drive different kinds of motors, or use alternate algorithms to SimpleFOC's default BLDCMotor and StepperMotor classes. + + - [HybridStepperMotor](motors/HybridStepperMotor/) - Drive stepper motors with 3 phases. + ### Utilities - [STM32 PWM Input driver](src/utilities/stm32pwm/) - PWM Input driver for STM32 MCUs. Accurately measure PWM inputs with zero MCU overhead. - + - [STM32 CORDIC trig driver](src/utilities/stm32math/) - CORDIC driver to accellerate sine and cosine calculations in SimpleFOC, on STM32 MCUs which have a CORDIC unit. ## How to use @@ -95,6 +102,16 @@ Find out more information about the Arduino SimpleFOC project on the [docs websi ## Release History +What's changed since 1.0.4? +- Added smoothing sensor by [@dekutree64](https://github.com/dekutree64) +- Added TMD6200 SPI driver by [@YaseenTwati](https://github.com/YaseenTwati) +- Added HybridStepperMotor by [@VIPQualityPost](https://github.com/VIPQualityPost) +- New Settings abstraction to load and save SimpleFOC settings and calibration +- New Settings driver: SAMDNVMSettingsStorage +- SimpleFOCRegisters abstraction, mapping SimpleFOC parameters to virtual "Registers" +- Updated I2CCommander to use the new registers abstraction +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.5+) + What's changed since 1.0.3? - New Comms/Input: STM32SpeedDirCommander - New Utility: STM32PWMInput diff --git a/library.properties b/library.properties index d3565e4..8b4fcb8 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SimpleFOCDrivers -version=1.0.5 +version=1.0.6 author=Simplefoc maintainer=Simplefoc sentence=A library of supporting drivers for SimpleFOC. Motor drivers chips, encoder chips, current sensing and supporting code. diff --git a/src/drivers/stspin32g4/README.md b/src/drivers/stspin32g4/README.md new file mode 100644 index 0000000..6b296d7 --- /dev/null +++ b/src/drivers/stspin32g4/README.md @@ -0,0 +1,42 @@ + +# SimpleFOC STSPIN32G4 Driver + +This driver initializes the PWM stage of the STSPIN32G4, and provides access to its configuration via I2C. + +:warning: in development! + +## Setup + +Since there are currently no standard boards for Arduino based on the STSPIN32G4 you will need a custom board definition, associated linker script and project setup to compile for your board. These topics are out of scope for this driver, but you can find a working example for the [FunQi STSPIN32G4 board](TODO link) [here](TODO link); + +Once you can compile for your board, and flash it with a "blinky" test sketch, then you're ready to try SimpleFOC and more complex code. + +## Usage + +Basic usage, as you can see it is very simple. Since the pins are all pre-defined due to internal connections, setup +is easier than with the standard drivers. Here is an example for open loop mode: + +```c++ +#include +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "drivers/stspin32g4/STSPIN32G4.h" + + +STSPIN32G4 driver = STSPIN32G4(); +BLDCMotor motor = BLDCMotor(7); + +void setup() { + driver.voltage_power_supply = 12.0f; + driver.init(); + motor.voltage_limit = driver.voltage_limit / 2.0f; + motor.controller = MotionControlType::velocity_openloop; + motor.linkDriver(&driver); + motor.init(); +} + +void loop(){ + motor.move(5.0f); // 5 rad/s open loop + delayMicroseconds(100); // STM32G4 is very fast, add a delay in open loop if we do nothing else +} +``` \ No newline at end of file diff --git a/src/drivers/stspin32g4/STSPIN32G4.cpp b/src/drivers/stspin32g4/STSPIN32G4.cpp new file mode 100644 index 0000000..2917791 --- /dev/null +++ b/src/drivers/stspin32g4/STSPIN32G4.cpp @@ -0,0 +1,182 @@ + +#include "./STSPIN32G4.h" + +#ifdef ARDUINO_GENERIC_G431VBTX + + +STSPIN32G4::STSPIN32G4() : BLDCDriver6PWM(STSPIN32G4_PIN_INUH, STSPIN32G4_PIN_INUL, STSPIN32G4_PIN_INVH, + STSPIN32G4_PIN_INVL, STSPIN32G4_PIN_INWH, STSPIN32G4_PIN_INWL), + _wire(STSPIN32G4_PIN_SDA, STSPIN32G4_PIN_SCL) { + +}; + + + +STSPIN32G4::~STSPIN32G4(){ + _wire.end(); +}; + + + +int STSPIN32G4::init(){ + // init pins + pinMode(STSPIN32G4_PIN_WAKE, OUTPUT); + digitalWrite(STSPIN32G4_PIN_WAKE, LOW); + pinMode(STSPIN32G4_PIN_READY, INPUT_PULLUP); + pinMode(STSPIN32G4_PIN_FAULT, INPUT_PULLUP); + + int result = this->BLDCDriver6PWM::init(); + if(result == 0) return result; + setPwm(0,0,0); // set the phases to off + + // init I2C + _wire.begin(); + + delayMicroseconds(50); // give driver a moment to wake up + clearFaults(); // clear the faults + + // TODO init fault monitor + + return isReady() ? 1 : 0; +}; + + + +void STSPIN32G4::wake() { + digitalWrite(STSPIN32G4_PIN_WAKE, HIGH); + delayMicroseconds(50); // 50ms high pulse to wake up + digitalWrite(STSPIN32G4_PIN_WAKE, LOW); +}; + + + +void STSPIN32G4::sleep() { + digitalWrite(STSPIN32G4_PIN_WAKE, LOW); + writeRegister(STSPIN32G4_REG_STBY, 0x01); +}; + + + +bool STSPIN32G4::isReady(){ + return digitalRead(STSPIN32G4_PIN_READY)==HIGH; +}; + + + +bool STSPIN32G4::isFault(){ + return digitalRead(STSPIN32G4_PIN_FAULT)==LOW; +}; + + + +STSPIN32G4Status STSPIN32G4::status(){ + STSPIN32G4Status result; + result.reg = readRegister(STSPIN32G4_REG_STATUS); + return result; +}; + + + +void STSPIN32G4::lock(){ + writeRegister(STSPIN32G4_REG_LOCK, 0x00); +}; + + + +void STSPIN32G4::unlock(){ + writeRegister(STSPIN32G4_REG_LOCK, 0xF0); +}; + + + +STSPIN32G4NFault STSPIN32G4::getNFaultRegister(){ + STSPIN32G4NFault result; + result.reg = readRegister(STSPIN32G4_REG_NFAULT); + return result; +}; + + + +STSPIN32G4Ready STSPIN32G4::getReadyRegister(){ + STSPIN32G4Ready result; + result.reg = readRegister(STSPIN32G4_REG_READY); + return result; +}; + + + +STSPIN32G4Logic STSPIN32G4::getLogicRegister(){ + STSPIN32G4Logic result; + result.reg = readRegister(STSPIN32G4_REG_LOGIC); + return result; +}; + + + +STSPIN32G4PowMng STSPIN32G4::getPowMngRegister(){ + STSPIN32G4PowMng result; + result.reg = readRegister(STSPIN32G4_REG_POWMNG); + return result; +}; + + + +void STSPIN32G4::setNFaultRegister(STSPIN32G4NFault value){ + writeRegister(STSPIN32G4_REG_NFAULT, value.reg); +}; + + + +void STSPIN32G4::setReadyRegister(STSPIN32G4Ready value){ + writeRegister(STSPIN32G4_REG_READY, value.reg); +}; + + + +void STSPIN32G4::setLogicRegister(STSPIN32G4Logic value){ + writeRegister(STSPIN32G4_REG_LOGIC, value.reg); +}; + + + +void STSPIN32G4::setPowMngRegister(STSPIN32G4PowMng value){ + writeRegister(STSPIN32G4_REG_POWMNG, value.reg); +}; + + + +void STSPIN32G4::resetRegisters(){ + // write 0xFF to reset register + writeRegister(STSPIN32G4_REG_RESET, 0xFF); +}; + + + +void STSPIN32G4::clearFaults(){ + // write 0xFF to clear faults + writeRegister(STSPIN32G4_REG_CLEAR, 0xFF); +}; + + + +uint8_t STSPIN32G4::readRegister(uint8_t reg){ + uint8_t result = 0; + _wire.beginTransmission(STSPIN32G4_I2C_ADDR); + _wire.write(reg); + _wire.endTransmission(false); + _wire.requestFrom(STSPIN32G4_I2C_ADDR, 1); + result = _wire.read(); + return result; +}; + + + +void STSPIN32G4::writeRegister(uint8_t reg, uint8_t val){ + _wire.beginTransmission(STSPIN32G4_I2C_ADDR); + _wire.write(reg); + _wire.write(val); + _wire.endTransmission(); +}; + + +#endif \ No newline at end of file diff --git a/src/drivers/stspin32g4/STSPIN32G4.h b/src/drivers/stspin32g4/STSPIN32G4.h new file mode 100644 index 0000000..3df8137 --- /dev/null +++ b/src/drivers/stspin32g4/STSPIN32G4.h @@ -0,0 +1,148 @@ + +#pragma once + +#include "Arduino.h" + +#ifdef ARDUINO_GENERIC_G431VBTX + +#include "Wire.h" +#include "drivers/BLDCDriver6PWM.h" + + +#define STSPIN32G4_PIN_INUL PE8 +#define STSPIN32G4_PIN_INUH PE9 +#define STSPIN32G4_PIN_INVL PE10 +#define STSPIN32G4_PIN_INVH PE11 +#define STSPIN32G4_PIN_INWL PE12 +#define STSPIN32G4_PIN_INWH PE13 + +#define STSPIN32G4_PIN_WAKE PE7 +#define STSPIN32G4_PIN_READY PE14 +#define STSPIN32G4_PIN_FAULT PE15 + +#define STSPIN32G4_PIN_SDA PC9 +#define STSPIN32G4_PIN_SCL PC8 + +#define STSPIN32G4_I2C_ADDR 0b1000111 + +#define STSPIN32G4_REG_POWMNG 0x01 +#define STSPIN32G4_REG_LOGIC 0x02 +#define STSPIN32G4_REG_READY 0x07 +#define STSPIN32G4_REG_NFAULT 0x08 +#define STSPIN32G4_REG_CLEAR 0x09 +#define STSPIN32G4_REG_STBY 0x0A +#define STSPIN32G4_REG_LOCK 0x0B +#define STSPIN32G4_REG_RESET 0x0C +#define STSPIN32G4_REG_STATUS 0x80 + + + + + +union STSPIN32G4Status { + struct { + uint8_t vcc_uvlo:1; + uint8_t thsd:1; + uint8_t vds_p:1; + uint8_t reset:1; + uint8_t unused:3; + uint8_t lock:1; + }; + uint8_t reg; +}; + + + +union STSPIN32G4NFault { + struct { + uint8_t vcc_uvlo_flt:1; + uint8_t thsd_flt:1; + uint8_t vds_p_flt:1; + uint8_t unused:5; + }; + uint8_t reg; +}; + + +union STSPIN32G4Ready { + struct { + uint8_t vcc_uvlo_rdy:1; + uint8_t thsd_rdy:1; + uint8_t unused:1; + uint8_t stby_rdy:1; + uint8_t unused2:4; + }; + uint8_t reg; +}; + + + +union STSPIN32G4Logic { + struct { + uint8_t ilock:1; + uint8_t dtmin:1; + uint8_t vds_p_deg:2; + uint8_t unused:4; + }; + uint8_t reg; +}; + + + +union STSPIN32G4PowMng { + struct { + uint8_t vcc_val:2; + uint8_t unused:2; + uint8_t stby_reg_en:1; + uint8_t vcc_dis:1; + uint8_t reg3v3_dis:1; + uint8_t unused2:1; + }; + uint8_t reg; +}; + + + + + + + + + + + +class STSPIN32G4 : public BLDCDriver6PWM { +public: + STSPIN32G4(); + ~STSPIN32G4(); + + int init() override; + + void wake(); + void sleep(); + bool isReady(); + bool isFault(); + + STSPIN32G4Status status(); + void lock(); + void unlock(); + STSPIN32G4NFault getNFaultRegister(); + STSPIN32G4Ready getReadyRegister(); + STSPIN32G4Logic getLogicRegister(); + STSPIN32G4PowMng getPowMngRegister(); + void setNFaultRegister(STSPIN32G4NFault value); + void setReadyRegister(STSPIN32G4Ready value); + void setLogicRegister(STSPIN32G4Logic value); + void setPowMngRegister(STSPIN32G4PowMng value); + void resetRegisters(); + void clearFaults(); + +protected: + uint8_t readRegister(uint8_t reg); + void writeRegister(uint8_t reg, uint8_t val); + + TwoWire _wire; +}; + + +#endif \ No newline at end of file diff --git a/src/encoders/mt6835/MT6835.cpp b/src/encoders/mt6835/MT6835.cpp index aeaa200..ff0ac97 100644 --- a/src/encoders/mt6835/MT6835.cpp +++ b/src/encoders/mt6835/MT6835.cpp @@ -26,7 +26,39 @@ void MT6835::init(SPIClass* _spi) { float MT6835::getCurrentAngle(){ - return readRawAngle21() / (float)MT6835_CPR * _2PI; + uint32_t rawangle = readRawAngle21(); + if (checkcrc) { + if (lastcrc != calcCrc(rawangle, laststatus)) { + laststatus |= MT6835_CRC_ERROR; + return -1; // return -1 to signal CRC error - the current angle has to be non-negative otherwise + } + } + return rawangle / (float)MT6835_CPR * _2PI; +}; + + +// calculate crc8 of 21 angle bits and 3 status bits +// polynomial: x^8 + x^2 + x + 1 = 0x07, (0xE0 reflected) init is 0x00, no final xor +// TOOD table-based version +uint8_t MT6835::calcCrc(uint32_t angle, uint8_t status) { + uint8_t crc = 0x00; + + uint8_t input = angle>>13; + crc ^= input; + for (int k = 8; k > 0; k--) + crc = (crc & (0x01<<7))?(crc<<1)^0x07:crc<<1; + + input = (angle>>5) & 0xFF; + crc ^= input; + for (int k = 8; k > 0; k--) + crc = (crc & (0x01<<7))?(crc<<1)^0x07:crc<<1; + + input = ((angle<<3) & 0xFF) | (status & 0x07); + crc ^= input; + for (int k = 8; k > 0; k--) + crc = (crc & (0x01<<7))?(crc<<1)^0x07:crc<<1; + + return crc; }; @@ -39,14 +71,15 @@ uint32_t MT6835::readRawAngle21(){ data[3] = 0; data[4] = 0; data[5] = 0; + spi->beginTransaction(settings); if (nCS >= 0) digitalWrite(nCS, LOW); - spi->beginTransaction(settings); spi->transfer(data, 6); - spi->endTransaction(); if (nCS >= 0) digitalWrite(nCS, HIGH); + spi->endTransaction(); laststatus = data[4]&0x07; + lastcrc = data[5]; return (data[2] << 13) | (data[3] << 5) | (data[4] >> 3); }; @@ -55,6 +88,21 @@ uint8_t MT6835::getStatus(){ return laststatus; }; +uint8_t MT6835::getCalibrationStatus(){ + uint8_t data[3] = {0}; + data[0] = MT6835_OP_READ << 4 | MT6835_REG_CAL_STATUS >> 8; + data[1] = MT6835_REG_CAL_STATUS; + + spi->beginTransaction(settings); + if(nCS >= 0) + digitalWrite(nCS, LOW); + spi->transfer(data, 3); + if(nCS >= 0) + digitalWrite(nCS, HIGH); + spi->endTransaction(); + + return data[2] >> 6; +} bool MT6835::setZeroFromCurrentPosition(){ MT6835Command cmd; @@ -252,13 +300,13 @@ uint32_t swap_bytes(uint32_t net) void MT6835::transfer24(MT6835Command* outValue) { uint32_t buff = swap_bytes(outValue->val); + spi->beginTransaction(settings); if (nCS >= 0) digitalWrite(nCS, LOW); - spi->beginTransaction(settings); spi->transfer(&buff, 3); - spi->endTransaction(); if (nCS >= 0) digitalWrite(nCS, HIGH); + spi->endTransaction(); outValue->val = swap_bytes(buff); }; uint8_t MT6835::readRegister(uint16_t reg) { diff --git a/src/encoders/mt6835/MT6835.h b/src/encoders/mt6835/MT6835.h index 5751901..8732704 100644 --- a/src/encoders/mt6835/MT6835.h +++ b/src/encoders/mt6835/MT6835.h @@ -22,6 +22,7 @@ #define MT6835_STATUS_OVERSPEED 0x01 #define MT6835_STATUS_WEAKFIELD 0x02 #define MT6835_STATUS_UNDERVOLT 0x04 +#define MT6835_CRC_ERROR 0x08 #define MT6835_WRITE_ACK 0x55 @@ -199,17 +200,23 @@ class MT6835 { uint8_t getStatus(); + uint8_t getCalibrationStatus(); + bool setZeroFromCurrentPosition(); bool writeEEPROM(); // wait 6s after calling this method + bool checkcrc = false; + private: SPIClass* spi; SPISettings settings; int nCS = -1; uint8_t laststatus = 0; + uint8_t lastcrc = 0; void transfer24(MT6835Command* outValue); uint8_t readRegister(uint16_t reg); bool writeRegister(uint16_t reg, uint8_t value); + uint8_t calcCrc(uint32_t angle, uint8_t status); }; \ No newline at end of file diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp index 0525e3f..456cf75 100644 --- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp +++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp @@ -29,6 +29,10 @@ int STM32HWEncoder::hasIndex() { return 0; } void STM32HWEncoder::init() { // GPIO configuration TIM_TypeDef *InstanceA = (TIM_TypeDef *)pinmap_peripheral(_pinA, PinMap_TIM); + if (!IS_TIM_ENCODER_INTERFACE_INSTANCE(InstanceA)) { + initialized = false; + return; + } TIM_TypeDef *InstanceB = (TIM_TypeDef *)pinmap_peripheral(_pinB, PinMap_TIM); if (InstanceA != InstanceB) { initialized = false; @@ -37,6 +41,9 @@ void STM32HWEncoder::init() { pinmap_pinout(_pinA, PinMap_TIM); pinmap_pinout(_pinB, PinMap_TIM); + // TODO check width: + //IS_TIM_32B_COUNTER_INSTANCE(InstanceA); + // set up timer for encoder encoder_handle.Init.Period = cpr - 1; encoder_handle.Init.Prescaler = 0; diff --git a/src/encoders/stm32lpencoder/PinMap_LPTIM.c b/src/encoders/stm32lpencoder/PinMap_LPTIM.c new file mode 100644 index 0000000..53dcd1b --- /dev/null +++ b/src/encoders/stm32lpencoder/PinMap_LPTIM.c @@ -0,0 +1,38 @@ +#include "Arduino.h" + +#ifdef HAL_LPTIM_MODULE_ENABLED + + +// TODO which models does this apply to? the entire series, or the line? +#if defined(STM32G431KB) || defined(STM32G431CB) +WEAK const PinMap PinMap_LPTIM[] = { + {PB_5, LPTIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF11_LPTIM1, 0, 0)}, // LPTIM1_CH1 + {PB_7, LPTIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF11_LPTIM1, 1, 0)}, // LPTIM1_CH2 + {NC, NP, 0} +}; +WEAK const PinMap PinMap_LPTIMETR[] = { + {PB_6, LPTIM1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF11_LPTIM1)}, // LPTIM1_ETR + {NC, NP, 0} +}; +#endif + + +#if defined(STM32G473QE) || defined(STM32G431VB) +WEAK const PinMap PinMap_LPTIM[] = { + {PB_5, LPTIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF11_LPTIM1, 0, 0)}, // LPTIM1_CH1 + {PB_7, LPTIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF11_LPTIM1, 1, 0)}, // LPTIM1_CH2 + {PC_0, LPTIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF1_LPTIM1, 0, 0)}, // LPTIM1_CH1 + {PC_2, LPTIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF1_LPTIM1, 1, 0)}, // LPTIM1_CH2 + {NC, NP, 0} +}; +WEAK const PinMap PinMap_LPTIMETR[] = { + {PB_6, LPTIM1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF11_LPTIM1)}, // LPTIM1_ETR + {PC_3, LPTIM1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF1_LPTIM1)}, // LPTIM1_ETR + {NC, NP, 0} +}; +#endif + + + + +#endif // HAL_LPTIM_MODULE_ENABLED \ No newline at end of file diff --git a/src/encoders/stm32lpencoder/README.md b/src/encoders/stm32lpencoder/README.md new file mode 100644 index 0000000..801ec74 --- /dev/null +++ b/src/encoders/stm32lpencoder/README.md @@ -0,0 +1,46 @@ + +# STM32 Low Power Encoder driver + +This is a driver for the encoder mode of STM32 MCU LPTIM timers. + +:warning: not yet tested + +The LPTIM timers are low power timers available on some STM32 MCUs. Some LPTIM timers have an encoder mode (typically LPTIM1 and LPTIM2). Using this encoder class has zero MCU overhead as the hardware handles all the encoder counting. There is also no need for interrupt handlers, interrupts are not used. However, you must be sure to call `encoder.update()` frequently (at least 2x per revolution) to correctly keep track of the full rotations. + +In theory the LPTIM timers can keep counting when the MCU is in sleep mode, but in practice tracking the full rotations would be difficult to achieve. + +## Compatibility + +Unfortunately, Arduino framework (stm32duino) provides no support for low power timers, and there is no "PinMap" to identify which pins are LPTIM outputs. + +If you want to use this driver, your MCU has to be part of the custom `PinMap_LPTIM.c` file used by this driver. Currently, we've added the following MCUs: + +- STM32G431KB +- STM32G431CB +- STM32G473QE +- STM32G431VB + +Adding MCUs is not hard if you look at the format [in the file](./PinMap_LPTIM.c), and pull requests for other MCUs would be gladly accepted. + +## Usage + +Like all our encoders, you must specify the PPR (pulses per revolution). This number should come from your encoder's datasheet. Note: internally, the encoder will use a 4x higher CPR (counts per revolution) than the PPR value you provide. + +You may need to enable the LPTIM in stm32duino: + +``` +-DHAL_LPTIM_MODULE_ENABLED +``` + +Its easy to use: + +```c++ +STM32LowPowerEncoder encoder = STM32LowPowerEncoder(4096, PB5, PB7); // 4096 is the PPR + +vpid setup() { + ... + encoder.init(); + motor.linkSensor(&encoder); + ... +} +``` \ No newline at end of file diff --git a/src/encoders/stm32lpencoder/STM32LowPowerEncoder.cpp b/src/encoders/stm32lpencoder/STM32LowPowerEncoder.cpp new file mode 100644 index 0000000..6d1d040 --- /dev/null +++ b/src/encoders/stm32lpencoder/STM32LowPowerEncoder.cpp @@ -0,0 +1,84 @@ + + +#include "./STM32LowPowerEncoder.h" + +#if defined(_STM32_DEF_) && defined(HAL_LPTIM_MODULE_ENABLED) + + +extern const PinMap PinMap_LPTIM[]; +extern const PinMap PinMap_LPTIMETR[]; + + +STM32LowPowerEncoder::STM32LowPowerEncoder(unsigned int ppr, int pinA, int pinB, int pinI){ + cpr = ppr * 4; // 4x for quadrature + _pinA = digitalPinToPinName(pinA); + _pinB = digitalPinToPinName(pinB); + _pinI = digitalPinToPinName(pinI); +}; + +void STM32LowPowerEncoder::init(){ + _encoder_handle.Instance = (LPTIM_TypeDef*)pinmap_peripheral(_pinA, PinMap_LPTIM); + if (!IS_LPTIM_ENCODER_INSTANCE(_encoder_handle.Instance)) { + initialized = false; + return; + } + LPTIM_TypeDef* inst = (LPTIM_TypeDef*)pinmap_peripheral(_pinA, PinMap_LPTIM); + if (inst!=_encoder_handle.Instance) { // check the other pins are on the same timer + initialized = false; + return; + } + if (_pinI!=NC) { + inst = (LPTIM_TypeDef*)pinmap_peripheral(_pinI, PinMap_LPTIMETR); + if (inst!=_encoder_handle.Instance) { + initialized = false; + return; + } + } + _encoder_handle.Init.Clock.Source = LPTIM_CLOCKSOURCE_APBCLOCK_LPOSC; + _encoder_handle.Init.Clock.Prescaler = LPTIM_PRESCALER_DIV1; + _encoder_handle.Init.UltraLowPowerClock.Polarity = LPTIM_CLOCKPOLARITY_RISING_FALLING; + _encoder_handle.Init.UltraLowPowerClock.SampleTime = LPTIM_CLOCKSAMPLETIME_DIRECTTRANSITION; + if (_pinI==NC) + _encoder_handle.Init.Trigger.Source = LPTIM_TRIGSOURCE_SOFTWARE; + else { + _encoder_handle.Init.Trigger.Source = LPTIM_TRIGSOURCE_0; + _encoder_handle.Init.Trigger.ActiveEdge = LPTIM_ACTIVEEDGE_RISING; + _encoder_handle.Init.Trigger.SampleTime = LPTIM_TRIGSAMPLETIME_DIRECTTRANSITION; + } + _encoder_handle.Init.OutputPolarity = LPTIM_OUTPUTPOLARITY_HIGH; + _encoder_handle.Init.UpdateMode = LPTIM_UPDATE_IMMEDIATE; + _encoder_handle.Init.CounterSource = LPTIM_COUNTERSOURCE_EXTERNAL; + _encoder_handle.Init.Input1Source = LPTIM_INPUT1SOURCE_GPIO; + _encoder_handle.Init.Input2Source = LPTIM_INPUT2SOURCE_GPIO; + if (HAL_LPTIM_Init(&_encoder_handle) != HAL_OK) + initialized = false; + else { + if (HAL_LPTIM_Encoder_Start(&_encoder_handle, cpr) != HAL_OK) + initialized = false; + else { + pinmap_pinout(_pinA, PinMap_LPTIM); + pinmap_pinout(_pinB, PinMap_LPTIM); + if (_pinI != NC) + pinmap_pinout(_pinI, PinMap_LPTIMETR); + initialized = true; + } + } +}; + + +int STM32LowPowerEncoder::needsSearch(){ + return false; +}; + + +int STM32LowPowerEncoder::hasIndex(){ + return 0; // TODO: implement index +}; + + + +float STM32LowPowerEncoder::getSensorAngle(){ + return _2PI * (_encoder_handle.Instance->CNT&0xFFFFUL) / (float)cpr; +}; + +#endif \ No newline at end of file diff --git a/src/encoders/stm32lpencoder/STM32LowPowerEncoder.h b/src/encoders/stm32lpencoder/STM32LowPowerEncoder.h new file mode 100644 index 0000000..8411a78 --- /dev/null +++ b/src/encoders/stm32lpencoder/STM32LowPowerEncoder.h @@ -0,0 +1,33 @@ + +#include "Arduino.h" + + +#if defined(_STM32_DEF_) && defined(HAL_LPTIM_MODULE_ENABLED) + +#include "common/base_classes/Sensor.h" +#include "common/foc_utils.h" + +class STM32LowPowerEncoder : public Sensor { + public: + /** + Encoder class constructor + @param ppr impulses per rotation (cpr=ppr*4) + */ + explicit STM32LowPowerEncoder(unsigned int ppr, int pinA, int pinB, int pinI=-1); + + void init() override; + int needsSearch() override; + int hasIndex(); // !< function returning 1 if encoder has index pin and 0 if not. + + bool initialized = false; + uint32_t cpr; //!< encoder cpr number + PinName _pinA, _pinB, _pinI; + + protected: + float getSensorAngle() override; + + LPTIM_HandleTypeDef _encoder_handle; + +}; + +#endif \ No newline at end of file diff --git a/src/settings/README.md b/src/settings/README.md index 9bd70fb..545971f 100644 --- a/src/settings/README.md +++ b/src/settings/README.md @@ -17,8 +17,10 @@ Supported hardware: - [SAMD NVM](samd/) \ Some SAMD chips have a built-in NVRAM flash, which can be written from user code. -- [CAT24 I2C Flash EEPROMs](i2c/) \ :warning: UNFINISHED +- [CAT24 I2C Flash EEPROMs](i2c/) :warning: UNFINISHED \ Simple programmable memories with I2C interface. Memory space is quite limited, but enough for storing settings. +- [STM32G4 Flash](stm32/) \ +Store settings directly to STM32G4 MCU's on-board flash. See Roadmap, below, for systems we may support in the future. diff --git a/src/settings/stm32/README.md b/src/settings/stm32/README.md new file mode 100644 index 0000000..973e672 --- /dev/null +++ b/src/settings/stm32/README.md @@ -0,0 +1,58 @@ + +# STM32 Flash Settings Storage + +Store your settings directly to STM32 MCU's on-board flash memory. + +To store the settings, the flash memory has to be erased in whole pages. + +The page size is typically 1kB or 2kB. You can check your page size in the datasheet for your MCU. + +The default settings storage location is the last page of the flash memory, meaning you have one page worth +of space, maximum. If you need more space than this, you can provide the start address for the settings storage. + +Needless to say, you can only store settings to flash if that memory is not also needed by your program. So you have to have some reserve space in the flash of the MCU you're using. If memory is too tight, use a STM32 model with more flash. + +## Compatibility + +Currently only **STM32G4** MCUs are supported. + +G4s are nice MCUs for using the flash because the page size is 2kB and you can erase the flash page by page. For this reason the default settings use the last page of flash as the storage location, leaving all the rest of the flash for your code and data. Pages are 2kB in size, which should be more than enough for most setups. + +Note: systems with dual bank flash have not been tested, and will probably not work. + +## Board setup + +When storing settings to the Flash of your STM32 MCU, you have to consider what happens when you reprogram the MCU. Unless you have taken steps in the board setup files to reserve the pages of flash used by the settings, they will generally be erased and/or overwritten when programming the MCU. + +To solve this, you can configure your board setup to reserve memory for settings, and not use that memory for program storage. Configuring STM32 board definitions for Arduino is out of scope for this readme, please find another reference. + +## Usage + +Using it is easy. Watch out if you have more than one page worth of settings to store, in which case you will need to configure the address to a different value from the default one. + +```c++ +// uses default address, which is 1 page below the maximum flash address, e.g. the last page of the flash +STM32FlashSettingsStorage settings = STM32FlashSettingsStorage(); + +// or use an address of your choice, in this case 10kB below the end of flash: +#define SETTINGS_ADDRESS (FLASH_BASE + FLASH_SIZE - (1024*10)) +STM32FlashSettingsStorage settings = STM32FlashSettingsStorage(SETTINGS_ADDRESS); + +// use as normal in the setup method: +void setup() { + SimpleFOCDebug::enable(); + + // link driver etc + // set default settings on motor, etc + + settings.addMotor(&motor); + settings.init(); + SettingsStatus loadStatus = settings.loadSettings(); + driver.init(); + motor.init(); + motor.initFOC(); + if (motor.motor_status == FOCMotorStatus::motor_ready && loadStatus != SFOC_SETTINGS_SUCCESS) { + settings.saveSettings(); + } +} +``` \ No newline at end of file diff --git a/src/settings/stm32/STM32FlashSettingsStorage.cpp b/src/settings/stm32/STM32FlashSettingsStorage.cpp new file mode 100644 index 0000000..27899e0 --- /dev/null +++ b/src/settings/stm32/STM32FlashSettingsStorage.cpp @@ -0,0 +1,154 @@ + + +#include "./STM32FlashSettingsStorage.h" + +#if defined(STM32G4xx) + +#include "communication/SimpleFOCDebug.h" + + +STM32FlashSettingsStorage::STM32FlashSettingsStorage(uint32_t address) { + _address = address; +}; + +STM32FlashSettingsStorage::~STM32FlashSettingsStorage(){}; + + +void STM32FlashSettingsStorage::init(){ + if (!IS_FLASH_PROGRAM_ADDRESS(_address)) + SimpleFOCDebug::println("SS: Invalid address"); + SettingsStorage::init(); + reset(); +}; + + +void STM32FlashSettingsStorage::reset() { + _currptr = (uint8_t*)_address; +}; + + +void STM32FlashSettingsStorage::beforeLoad(){ + reset(); +}; + +void STM32FlashSettingsStorage::beforeSave(){ + _writeptr = (uint8_t*)_address; + _buffed = 0; + _writeBuffer.l = 0; + _page = PAGE_OF(_writeptr); + if (HAL_FLASH_Unlock()!=HAL_OK) + SimpleFOCDebug::println("SS: Flash unlock err"); + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPTVERR); + erasePage(_page); +}; + + +void STM32FlashSettingsStorage::erasePage(uint32_t page) { + FLASH_EraseInitTypeDef eraseInit; + eraseInit.TypeErase = FLASH_TYPEERASE_PAGES; + eraseInit.Page = page; + eraseInit.Banks = 0;//_bank; + eraseInit.NbPages = 1; + uint32_t err; + SimpleFOCDebug::print("SS: erase page "); + SimpleFOCDebug::println((int)page); + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS); + if (HAL_FLASHEx_Erase(&eraseInit, &err) != HAL_OK) { + uint32_t ferr = HAL_FLASH_GetError(); + SimpleFOCDebug::print("SS: flash erase err nr "); + SimpleFOCDebug::println((int)ferr); + HAL_FLASH_Lock(); + return; + } +}; + + +void STM32FlashSettingsStorage::flushBuffer() { + uint32_t page = PAGE_OF(_writeptr+7); + if (page != _page) { + erasePage(page); + _page = page; + } + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS); + if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, (uint32_t)_writeptr, _writeBuffer.l)!=HAL_OK) { + uint32_t ferr = HAL_FLASH_GetError(); + SimpleFOCDebug::println("SS: flash write err nr "); + SimpleFOCDebug::println((int)ferr); + } + _writeptr += 8; + _buffed = 0; + _writeBuffer.l = 0; +}; + + +void STM32FlashSettingsStorage::afterSave(){ + if (_buffed > 0) + flushBuffer(); + HAL_FLASH_Lock(); +}; + + + +int STM32FlashSettingsStorage::writeBytes(void* value, int numBytes) { + uint8_t* bytes = (uint8_t*)value; + for (int i=0;i +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "utilities/stm32math/STM32G4CORDICTrigFunctions.h" + +void setup() { + ... + + SimpleFOC_CORDIC_Config(); // initialize the CORDIC + + ... +} + +``` + +That's it. The faster trig functions will be used automatically by the SimpleFOC code. + +You can use them yourself in your own code too: + +```c++ + +float angle = 0.5f; // in radians +float s = _sin(angle); +float c = _cos(angle); + +// get both sine and cosine in one operation +_sincos(angle, &s, &c); + +``` \ No newline at end of file diff --git a/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp b/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp new file mode 100644 index 0000000..2095ee8 --- /dev/null +++ b/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp @@ -0,0 +1,83 @@ + +#include "./STM32G4CORDICTrigFunctions.h" + +#ifdef HAL_CORDIC_MODULE_ENABLED + +#include "Arduino.h" +//#include "stm32g4xx_hal_cordic.h" +#include "stm32g4xx_ll_cordic.h" +#include "stm32g4xx_ll_rcc.h" +#include "stm32g4xx_ll_bus.h" +#include "common/foc_utils.h" +#include "arm_math.h" + +CORDIC_HandleTypeDef thisCordic; + +bool SimpleFOC_CORDIC_Config(void){ + //__HAL_RCC_CORDIC_CLK_ENABLE(); + // CORDIC_ConfigTypeDef sConfig; + // thisCordic.Instance = CORDIC; + // if (HAL_CORDIC_Init(&thisCordic) != HAL_OK) { + // Error_Handler(); + // return false; + // } + + // sConfig.Function = CORDIC_FUNCTION_COSINE; + // sConfig.Precision = CORDIC_PRECISION_6CYCLES; + // sConfig.Scale = CORDIC_SCALE_0; + // sConfig.NbWrite = CORDIC_NBWRITE_1; + // sConfig.NbRead = CORDIC_NBREAD_2; + // sConfig.InSize = CORDIC_INSIZE_32BITS; + // sConfig.OutSize = CORDIC_OUTSIZE_32BITS; + // if (HAL_CORDIC_Configure(&thisCordic, &sConfig) != HAL_OK) { + // /* Channel Configuration Error */ + // Error_Handler(); + // return false; + // } + + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_CORDIC); + LL_CORDIC_Config(CORDIC, LL_CORDIC_FUNCTION_COSINE, /* cosine function */ + LL_CORDIC_PRECISION_6CYCLES, /* max precision for q1.31 cosine */ + LL_CORDIC_SCALE_0, /* no scale */ + LL_CORDIC_NBWRITE_1, /* One input data: angle. Second input data (modulus) is 1 after cordic reset */ + LL_CORDIC_NBREAD_2, /* Two output data: cosine, then sine */ + LL_CORDIC_INSIZE_32BITS, /* q1.31 format for input data */ + LL_CORDIC_OUTSIZE_32BITS); /* q1.31 format for output data */ + return true; +}; + + + +float _sin(float a) { + a = fmod(a, _2PI); + if (a>_PI) a -= _2PI; + if (a<-_PI) a += _2PI; + CORDIC->WDATA = (q31_t)((a / _PI) * 0x80000000); + q31_t out_cos = (int32_t)CORDIC->RDATA; // read cosine result + q31_t out_sin = (int32_t)CORDIC->RDATA; // read sine result + return (float)out_sin / (float)0x80000000; +} + +float _cos(float a) { + a = fmod(a, _2PI); + if (a>_PI) a -= _2PI; + if (a<-_PI) a += _2PI; + CORDIC->WDATA = (q31_t)((a / _PI) * 0x80000000); + q31_t out_cos = (int32_t)CORDIC->RDATA; // read cosine result + q31_t out_sin = (int32_t)CORDIC->RDATA; // read sine result + return (float)out_cos / (float)0x80000000; +} + +void _sincos(float a, float* s, float* c) { + a = fmod(a, _2PI); + if (a>_PI) a -= _2PI; + if (a<-_PI) a += _2PI; + CORDIC->WDATA = (q31_t)((a / _PI) * 0x80000000); + q31_t out_cos = (int32_t)CORDIC->RDATA; // read cosine result + q31_t out_sin = (int32_t)CORDIC->RDATA; // read sine result + *c = (float)out_cos / (float)0x80000000; + *s = (float)out_sin / (float)0x80000000; +} + + +#endif diff --git a/src/utilities/stm32math/STM32G4CORDICTrigFunctions.h b/src/utilities/stm32math/STM32G4CORDICTrigFunctions.h new file mode 100644 index 0000000..9277ac5 --- /dev/null +++ b/src/utilities/stm32math/STM32G4CORDICTrigFunctions.h @@ -0,0 +1,10 @@ + +#pragma once + +#ifdef HAL_CORDIC_MODULE_ENABLED + + +bool SimpleFOC_CORDIC_Config(void); + + +#endif