From 55f2de8f03e11aaac63b25e76ca8e9eb89edba03 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Sep 2023 20:11:41 +0200 Subject: [PATCH 01/19] Prepare README and version for next release --- README.md | 15 ++++----------- library.properties | 2 +- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index e5f571d..19d17dd 100644 --- a/README.md +++ b/README.md @@ -10,17 +10,10 @@ 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 - -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+) +v1.0.6 - Released July 2023, for Simple FOC 2.3.1 or later + +What's changed since 1.0.5? +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6+) ## What is included 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. From 76b356e9b6f1bc946db23b3738249f179ce86da7 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Sep 2023 20:18:22 +0200 Subject: [PATCH 02/19] updated README --- README.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/README.md b/README.md index 19d17dd..cb33e97 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,7 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un v1.0.6 - Released July 2023, for Simple FOC 2.3.1 or later What's changed since 1.0.5? +- Added AS5600 Sensor Driver - Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6+) ## What is included @@ -29,6 +30,7 @@ What is here? See the sections below. Each driver or function should come with i - [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. @@ -88,6 +90,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 From ed759444960f5b94af1936d0379ca9fbf399ab9c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 2 Oct 2023 21:15:02 +0200 Subject: [PATCH 03/19] CORDIC trig functions for STM32G4 --- src/utilities/stm32math/README.md | 41 +++++++++ .../stm32math/STM32G4CORDICTrigFunctions.cpp | 83 +++++++++++++++++++ .../stm32math/STM32G4CORDICTrigFunctions.h | 10 +++ 3 files changed, 134 insertions(+) create mode 100644 src/utilities/stm32math/README.md create mode 100644 src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp create mode 100644 src/utilities/stm32math/STM32G4CORDICTrigFunctions.h diff --git a/src/utilities/stm32math/README.md b/src/utilities/stm32math/README.md new file mode 100644 index 0000000..614231d --- /dev/null +++ b/src/utilities/stm32math/README.md @@ -0,0 +1,41 @@ + +# SimpleFOC math functions for STM32 CORDIC + +Accellerated trigonometry functions are provided for STM32 MCUs having a CORDIC peripheral. The CORDIC hardware can compute sine and cosine values in just a few processor cycles. + +## Usage + +To use it, set the build-flag `-DHAL_CORDIC_MODULE_ENABLED` for the CORDIC to be enabled in the framework. + +Then, include the following in your code: + +```c++ +#include +#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 From 260580f3d291322da21337da3a3911fe1db4736e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 2 Oct 2023 21:51:36 +0200 Subject: [PATCH 04/19] STSPIN32G4 driver --- src/drivers/stspin32g4/README.md | 42 ++++++ src/drivers/stspin32g4/STSPIN32G4.cpp | 176 ++++++++++++++++++++++++++ src/drivers/stspin32g4/STSPIN32G4.h | 148 ++++++++++++++++++++++ 3 files changed, 366 insertions(+) create mode 100644 src/drivers/stspin32g4/README.md create mode 100644 src/drivers/stspin32g4/STSPIN32G4.cpp create mode 100644 src/drivers/stspin32g4/STSPIN32G4.h 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..6320650 --- /dev/null +++ b/src/drivers/stspin32g4/STSPIN32G4.cpp @@ -0,0 +1,176 @@ + +#include "./STSPIN32G4.h" + +#ifdef _STM32_DEF_ + + +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); + pinMode(STSPIN32G4_PIN_READY, INPUT_PULLUP); + pinMode(STSPIN32G4_PIN_FAULT, INPUT_PULLUP); + + // wake up + digitalWrite(STSPIN32G4_PIN_WAKE, HIGH); + + // init I2C + _wire.begin(); + + // init PWM + return BLDCDriver6PWM::init(); + + // TODO init fault monitor +}; + + + +void STSPIN32G4::wake() { + digitalWrite(STSPIN32G4_PIN_WAKE, HIGH); +}; + + + +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..62b7500 --- /dev/null +++ b/src/drivers/stspin32g4/STSPIN32G4.h @@ -0,0 +1,148 @@ + +#pragma once + +#include "Arduino.h" + +#ifdef _STM32_DEF_ + +#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 0x19 +#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 From 7e4f89db5d5c0ac7129b30be73d6cd0f4472b69a Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 3 Oct 2023 13:40:31 +0200 Subject: [PATCH 05/19] restrict compile of STSPIN32G4 to generic G431VBTX --- src/drivers/stspin32g4/STSPIN32G4.cpp | 2 +- src/drivers/stspin32g4/STSPIN32G4.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/stspin32g4/STSPIN32G4.cpp b/src/drivers/stspin32g4/STSPIN32G4.cpp index 6320650..ac0aa7d 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.cpp +++ b/src/drivers/stspin32g4/STSPIN32G4.cpp @@ -1,7 +1,7 @@ #include "./STSPIN32G4.h" -#ifdef _STM32_DEF_ +#ifdef ARDUINO_GENERIC_G431VBTX STSPIN32G4::STSPIN32G4() : BLDCDriver6PWM(STSPIN32G4_PIN_INUH, STSPIN32G4_PIN_INUL, STSPIN32G4_PIN_INVH, diff --git a/src/drivers/stspin32g4/STSPIN32G4.h b/src/drivers/stspin32g4/STSPIN32G4.h index 62b7500..88503b7 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.h +++ b/src/drivers/stspin32g4/STSPIN32G4.h @@ -3,7 +3,7 @@ #include "Arduino.h" -#ifdef _STM32_DEF_ +#ifdef ARDUINO_GENERIC_G431VBTX #include "Wire.h" #include "drivers/BLDCDriver6PWM.h" From 0870788d80e6eea4d2d66d455852cdffaea80ae3 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 4 Oct 2023 15:46:41 +0200 Subject: [PATCH 06/19] working on init --- src/drivers/stspin32g4/STSPIN32G4.cpp | 24 +++++++++++++++++------- src/drivers/stspin32g4/STSPIN32G4.h | 4 ++-- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/drivers/stspin32g4/STSPIN32G4.cpp b/src/drivers/stspin32g4/STSPIN32G4.cpp index ac0aa7d..e8d1a48 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.cpp +++ b/src/drivers/stspin32g4/STSPIN32G4.cpp @@ -18,28 +18,38 @@ STSPIN32G4::~STSPIN32G4(){ -int STSPIN32G4::init(){ +int STSPIN32G4::initSTSPIN(){ // init pins pinMode(STSPIN32G4_PIN_WAKE, OUTPUT); + digitalWrite(STSPIN32G4_PIN_WAKE, LOW); pinMode(STSPIN32G4_PIN_READY, INPUT_PULLUP); pinMode(STSPIN32G4_PIN_FAULT, INPUT_PULLUP); - - // wake up - digitalWrite(STSPIN32G4_PIN_WAKE, HIGH); + pinMode(STSPIN32G4_PIN_INUH, OUTPUT); + pinMode(STSPIN32G4_PIN_INUL, OUTPUT); + pinMode(STSPIN32G4_PIN_INVH, OUTPUT); + pinMode(STSPIN32G4_PIN_INVL, OUTPUT); + pinMode(STSPIN32G4_PIN_INWH, OUTPUT); + pinMode(STSPIN32G4_PIN_INWL, OUTPUT); + digitalWrite(STSPIN32G4_PIN_INUH, LOW); + digitalWrite(STSPIN32G4_PIN_INUL, LOW); + digitalWrite(STSPIN32G4_PIN_INVH, LOW); + digitalWrite(STSPIN32G4_PIN_INVL, LOW); + digitalWrite(STSPIN32G4_PIN_INWH, LOW); + digitalWrite(STSPIN32G4_PIN_INWL, LOW); // init I2C _wire.begin(); - // init PWM - return BLDCDriver6PWM::init(); - // TODO init fault monitor + return 0; }; void STSPIN32G4::wake() { digitalWrite(STSPIN32G4_PIN_WAKE, HIGH); + delayMicroseconds(50); // 50ms high pulse to wake up + digitalWrite(STSPIN32G4_PIN_WAKE, LOW); }; diff --git a/src/drivers/stspin32g4/STSPIN32G4.h b/src/drivers/stspin32g4/STSPIN32G4.h index 88503b7..2a86439 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.h +++ b/src/drivers/stspin32g4/STSPIN32G4.h @@ -29,7 +29,7 @@ #define STSPIN32G4_REG_LOGIC 0x02 #define STSPIN32G4_REG_READY 0x07 #define STSPIN32G4_REG_NFAULT 0x08 -#define STSPIN32G4_REG_CLEAR 0x19 +#define STSPIN32G4_REG_CLEAR 0x09 #define STSPIN32G4_REG_STBY 0x0A #define STSPIN32G4_REG_LOCK 0x0B #define STSPIN32G4_REG_RESET 0x0C @@ -116,7 +116,7 @@ class STSPIN32G4 : public BLDCDriver6PWM { STSPIN32G4(); ~STSPIN32G4(); - int init() override; + int initSTSPIN(); void wake(); void sleep(); From ad9e8ffe8eb322df48b266dc60cc7aa8fc47b071 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 4 Oct 2023 19:10:59 +0200 Subject: [PATCH 07/19] STSPIN32G4 init is working --- src/drivers/stspin32g4/STSPIN32G4.cpp | 24 ++++++++++-------------- src/drivers/stspin32g4/STSPIN32G4.h | 2 +- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/src/drivers/stspin32g4/STSPIN32G4.cpp b/src/drivers/stspin32g4/STSPIN32G4.cpp index e8d1a48..2917791 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.cpp +++ b/src/drivers/stspin32g4/STSPIN32G4.cpp @@ -18,30 +18,26 @@ STSPIN32G4::~STSPIN32G4(){ -int STSPIN32G4::initSTSPIN(){ +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); - pinMode(STSPIN32G4_PIN_INUH, OUTPUT); - pinMode(STSPIN32G4_PIN_INUL, OUTPUT); - pinMode(STSPIN32G4_PIN_INVH, OUTPUT); - pinMode(STSPIN32G4_PIN_INVL, OUTPUT); - pinMode(STSPIN32G4_PIN_INWH, OUTPUT); - pinMode(STSPIN32G4_PIN_INWL, OUTPUT); - digitalWrite(STSPIN32G4_PIN_INUH, LOW); - digitalWrite(STSPIN32G4_PIN_INUL, LOW); - digitalWrite(STSPIN32G4_PIN_INVH, LOW); - digitalWrite(STSPIN32G4_PIN_INVL, LOW); - digitalWrite(STSPIN32G4_PIN_INWH, LOW); - digitalWrite(STSPIN32G4_PIN_INWL, LOW); + + 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 0; + + return isReady() ? 1 : 0; }; diff --git a/src/drivers/stspin32g4/STSPIN32G4.h b/src/drivers/stspin32g4/STSPIN32G4.h index 2a86439..3df8137 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.h +++ b/src/drivers/stspin32g4/STSPIN32G4.h @@ -116,7 +116,7 @@ class STSPIN32G4 : public BLDCDriver6PWM { STSPIN32G4(); ~STSPIN32G4(); - int initSTSPIN(); + int init() override; void wake(); void sleep(); From 8a4d287afccb3944ed7e5ef4822002106908e2cc Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 30 Oct 2023 23:39:41 +0100 Subject: [PATCH 08/19] don't use dev platform for esp32 tests --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 From c66b9db82a70678d2d7fddf2392ecbd36f9a7000 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 3 Nov 2023 00:02:35 +0100 Subject: [PATCH 09/19] fix bug with SPI mode 3 --- src/encoders/mt6835/MT6835.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/encoders/mt6835/MT6835.cpp b/src/encoders/mt6835/MT6835.cpp index aeaa200..413eb86 100644 --- a/src/encoders/mt6835/MT6835.cpp +++ b/src/encoders/mt6835/MT6835.cpp @@ -39,13 +39,13 @@ 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; return (data[2] << 13) | (data[3] << 5) | (data[4] >> 3); }; @@ -252,13 +252,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) { From b1ae311a693d2ba8e4aea0a563f7ff2bb7d04881 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 7 Nov 2023 22:15:11 +0100 Subject: [PATCH 10/19] add a stm32 LPTIM encoder --- src/encoders/stm32lpencoder/PinMap_LPTIM.c | 38 +++++++++ src/encoders/stm32lpencoder/README.md | 46 ++++++++++ .../stm32lpencoder/STM32LowPowerEncoder.cpp | 84 +++++++++++++++++++ .../stm32lpencoder/STM32LowPowerEncoder.h | 33 ++++++++ 4 files changed, 201 insertions(+) create mode 100644 src/encoders/stm32lpencoder/PinMap_LPTIM.c create mode 100644 src/encoders/stm32lpencoder/README.md create mode 100644 src/encoders/stm32lpencoder/STM32LowPowerEncoder.cpp create mode 100644 src/encoders/stm32lpencoder/STM32LowPowerEncoder.h 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 From 0255fccf4dbb061cf98e023812070415932de5c1 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 7 Nov 2023 22:16:06 +0100 Subject: [PATCH 11/19] add a check to ensure encoder mode is supported --- src/encoders/stm32hwencoder/STM32HWEncoder.cpp | 7 +++++++ 1 file changed, 7 insertions(+) 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; From 2316b78daf84f97feede7ddf0478784bbe43afeb Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 7 Nov 2023 22:18:06 +0100 Subject: [PATCH 12/19] Add CRC check to MT6835 --- src/encoders/mt6835/MT6835.cpp | 35 +++++++++++++++++++++++++++++++++- src/encoders/mt6835/MT6835.h | 5 +++++ 2 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/encoders/mt6835/MT6835.cpp b/src/encoders/mt6835/MT6835.cpp index 413eb86..f035f8d 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; }; @@ -47,6 +79,7 @@ uint32_t MT6835::readRawAngle21(){ digitalWrite(nCS, HIGH); spi->endTransaction(); laststatus = data[4]&0x07; + lastcrc = data[5]; return (data[2] << 13) | (data[3] << 5) | (data[4] >> 3); }; diff --git a/src/encoders/mt6835/MT6835.h b/src/encoders/mt6835/MT6835.h index 5751901..1ef6e98 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 @@ -202,14 +203,18 @@ class MT6835 { 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 From eb2fc66ca842534b16dba246c9e17bfad11e21d2 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 8 Nov 2023 00:16:08 +0100 Subject: [PATCH 13/19] unfinished STM32FlashSettingsStorage --- src/settings/README.md | 4 +- src/settings/stm32/README.md | 50 ++++++ .../stm32/STM32FlashSettingsStorage.cpp | 148 ++++++++++++++++++ .../stm32/STM32FlashSettingsStorage.h | 51 ++++++ 4 files changed, 252 insertions(+), 1 deletion(-) create mode 100644 src/settings/stm32/README.md create mode 100644 src/settings/stm32/STM32FlashSettingsStorage.cpp create mode 100644 src/settings/stm32/STM32FlashSettingsStorage.h diff --git a/src/settings/README.md b/src/settings/README.md index 9bd70fb..ff506ad 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. +- [STM32 Flash](stm32/) :warning: UNFINISHED \ +Store settings directly to STM32 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..4e0f4e3 --- /dev/null +++ b/src/settings/stm32/README.md @@ -0,0 +1,50 @@ + +# 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. + +## 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..f4e60ca --- /dev/null +++ b/src/settings/stm32/STM32FlashSettingsStorage.cpp @@ -0,0 +1,148 @@ + + +#include "./STM32FlashSettingsStorage.h" + +#if defined(_STM32_DEF_) + +#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"); + delay(50); + erasePage(_page); + delay(50); +}; + + +void STM32FlashSettingsStorage::erasePage(uint32_t page) { + FLASH_EraseInitTypeDef eraseInit; + eraseInit.TypeErase = FLASH_TYPEERASE_PAGES; + eraseInit.Page = page; + eraseInit.Banks = _bank; + eraseInit.NbPages = 1; + uint32_t err; + SimpleFOCDebug::print("SS: erase page "); + SimpleFOCDebug::println((int)page); + if (HAL_FLASHEx_Erase(&eraseInit, &err) != HAL_OK) { + SimpleFOCDebug::println("SS: flash erase err"); + HAL_FLASH_Lock(); + return; + } +}; + + +void STM32FlashSettingsStorage::flushBuffer() { + uint32_t page = PAGE_OF(_writeptr+7); + if (page != _page) { + erasePage(page); + _page = page; + } + if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, (uint32_t)_writeptr, _writeBuffer.l)!=HAL_OK) + SimpleFOCDebug::println("SS: Flash write err"); + _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 Date: Tue, 7 Nov 2023 20:02:35 -0500 Subject: [PATCH 14/19] self cal verification --- src/encoders/mt6835/MT6835.cpp | 15 +++++++++++++++ src/encoders/mt6835/MT6835.h | 2 ++ 2 files changed, 17 insertions(+) diff --git a/src/encoders/mt6835/MT6835.cpp b/src/encoders/mt6835/MT6835.cpp index aeaa200..4010d48 100644 --- a/src/encoders/mt6835/MT6835.cpp +++ b/src/encoders/mt6835/MT6835.cpp @@ -55,6 +55,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->startTransaction(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; diff --git a/src/encoders/mt6835/MT6835.h b/src/encoders/mt6835/MT6835.h index 5751901..8518034 100644 --- a/src/encoders/mt6835/MT6835.h +++ b/src/encoders/mt6835/MT6835.h @@ -199,6 +199,8 @@ class MT6835 { uint8_t getStatus(); + uint8_t getCalibrationStatus(); + bool setZeroFromCurrentPosition(); bool writeEEPROM(); // wait 6s after calling this method From 5daf24eec2b3cae0e81407009da630c6045eb3c1 Mon Sep 17 00:00:00 2001 From: matei jordache Date: Tue, 7 Nov 2023 20:11:33 -0500 Subject: [PATCH 15/19] oops typo --- src/encoders/mt6835/MT6835.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/encoders/mt6835/MT6835.cpp b/src/encoders/mt6835/MT6835.cpp index 4010d48..28f56bc 100644 --- a/src/encoders/mt6835/MT6835.cpp +++ b/src/encoders/mt6835/MT6835.cpp @@ -60,7 +60,7 @@ uint8_t MT6835::getCalibrationStatus(){ data[0] = MT6835_OP_READ << 4 | MT6835_REG_CAL_STATUS >> 8; data[1] = MT6835_REG_CAL_STATUS; - spi->startTransaction(settings); + spi->beginTransaction(settings); if(nCS >= 0) digitalWrite(nCS, LOW); spi->transfer(data, 3); From 4b15bfcc83eac4e3834fb0348e7b3ee7a1a53464 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 8 Nov 2023 12:44:42 +0100 Subject: [PATCH 16/19] fix bugs in STM32G4 settings storage --- src/settings/README.md | 4 ++-- src/settings/stm32/README.md | 8 ++++++++ .../stm32/STM32FlashSettingsStorage.cpp | 20 ++++++++++++------- .../stm32/STM32FlashSettingsStorage.h | 8 ++++---- 4 files changed, 27 insertions(+), 13 deletions(-) diff --git a/src/settings/README.md b/src/settings/README.md index ff506ad..545971f 100644 --- a/src/settings/README.md +++ b/src/settings/README.md @@ -19,8 +19,8 @@ Supported hardware: Some SAMD chips have a built-in NVRAM flash, which can be written from user code. - [CAT24 I2C Flash EEPROMs](i2c/) :warning: UNFINISHED \ Simple programmable memories with I2C interface. Memory space is quite limited, but enough for storing settings. -- [STM32 Flash](stm32/) :warning: UNFINISHED \ -Store settings directly to STM32 on-board flash +- [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 index 4e0f4e3..973e672 100644 --- a/src/settings/stm32/README.md +++ b/src/settings/stm32/README.md @@ -12,6 +12,14 @@ of space, maximum. If you need more space than this, you can provide the start a 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. diff --git a/src/settings/stm32/STM32FlashSettingsStorage.cpp b/src/settings/stm32/STM32FlashSettingsStorage.cpp index f4e60ca..27899e0 100644 --- a/src/settings/stm32/STM32FlashSettingsStorage.cpp +++ b/src/settings/stm32/STM32FlashSettingsStorage.cpp @@ -2,7 +2,7 @@ #include "./STM32FlashSettingsStorage.h" -#if defined(_STM32_DEF_) +#if defined(STM32G4xx) #include "communication/SimpleFOCDebug.h" @@ -38,9 +38,8 @@ void STM32FlashSettingsStorage::beforeSave(){ _page = PAGE_OF(_writeptr); if (HAL_FLASH_Unlock()!=HAL_OK) SimpleFOCDebug::println("SS: Flash unlock err"); - delay(50); + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPTVERR); erasePage(_page); - delay(50); }; @@ -48,13 +47,16 @@ void STM32FlashSettingsStorage::erasePage(uint32_t page) { FLASH_EraseInitTypeDef eraseInit; eraseInit.TypeErase = FLASH_TYPEERASE_PAGES; eraseInit.Page = page; - eraseInit.Banks = _bank; + 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) { - SimpleFOCDebug::println("SS: flash erase err"); + uint32_t ferr = HAL_FLASH_GetError(); + SimpleFOCDebug::print("SS: flash erase err nr "); + SimpleFOCDebug::println((int)ferr); HAL_FLASH_Lock(); return; } @@ -67,8 +69,12 @@ void STM32FlashSettingsStorage::flushBuffer() { erasePage(page); _page = page; } - if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, (uint32_t)_writeptr, _writeBuffer.l)!=HAL_OK) - SimpleFOCDebug::println("SS: Flash write err"); + __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; diff --git a/src/settings/stm32/STM32FlashSettingsStorage.h b/src/settings/stm32/STM32FlashSettingsStorage.h index 1a23205..e66e68e 100644 --- a/src/settings/stm32/STM32FlashSettingsStorage.h +++ b/src/settings/stm32/STM32FlashSettingsStorage.h @@ -3,7 +3,7 @@ #include "../SettingsStorage.h" -#if defined(_STM32_DEF_) +#if defined(STM32G4xx) #define PAGE_OF(x) (((uint32_t)x - FLASH_BASE) / FLASH_PAGE_SIZE) @@ -15,8 +15,8 @@ class STM32FlashSettingsStorage : public SettingsStorage { void init() override; - uint32_t _bank = FLASH_BANK_1; - + uint32_t _bank = FLASH_BANK_1; // TODO also support bank 2 + protected: uint8_t readByte(uint8_t* valueToSet) override; uint8_t readFloat(float* valueToSet) override; @@ -41,7 +41,7 @@ class STM32FlashSettingsStorage : public SettingsStorage { uint8_t* _writeptr; uint8_t _buffed = 0; uint32_t _page = 0; - union { + __attribute__((aligned(8))) union { uint8_t b[8]; uint64_t l; } _writeBuffer; // 64-bit buffer From ad05d2fde7a0d026591e43663828169280eacb1c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 8 Nov 2023 12:55:13 +0100 Subject: [PATCH 17/19] Update main README --- README.md | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index cb33e97..820c4a4 100644 --- a/README.md +++ b/README.md @@ -10,10 +10,13 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un ## New Release -v1.0.6 - Released July 2023, for Simple FOC 2.3.1 or later +v1.0.6 - Released July 2023, for Simple FOC 2.3.2 or later What's changed since 1.0.5? -- Added AS5600 Sensor Driver +- Added AS5600 sensor driver +- Added STM32FlashSettingsStorage driver, supporting STM32G4 MCUs +- Added STSPIN32G4 driver +- 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 @@ -24,6 +27,7 @@ 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 @@ -58,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 From 28110b8e23ab57a268f91f9d5d9a51fa4d1a218a Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 1 Dec 2023 23:34:29 +0100 Subject: [PATCH 18/19] update readme for 1.0.6 release --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 820c4a4..75c046a 100644 --- a/README.md +++ b/README.md @@ -13,9 +13,9 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un v1.0.6 - Released July 2023, for Simple FOC 2.3.2 or later What's changed since 1.0.5? -- Added AS5600 sensor driver -- Added STM32FlashSettingsStorage driver, supporting STM32G4 MCUs - 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+) From e3036a690f2449b1f4ada7fc9a69413c31928cbd Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 1 Dec 2023 23:38:01 +0100 Subject: [PATCH 19/19] add correct release month in readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 75c046a..69923ec 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un ## New Release -v1.0.6 - Released July 2023, for Simple FOC 2.3.2 or later +v1.0.6 - Released December 2023, for Simple FOC 2.3.2 or later What's changed since 1.0.5? - Added STSPIN32G4 driver