Implement STM32HWEncoder count setting functions and getter for handler

src/encoders/stm32hwencoder/README.md

@@ -6,7 +6,7 @@ Big thank you to @conroy-cheers for originally contributing this code in pull re
 
 ## Warning
 
-This code has not been tested much! Use at your own risk, your milage may vary.
+This code has not been tested much! Use at your own risk, your milage may vary.\
 I have tested on: STM32F401CC/TIM4-PB6,PB7
 
 ## Hardware setup
@@ -17,8 +17,7 @@ Not all of the timers support the encoder mode, so check the datasheet which tim
 
 An excellent option can be to use the STM32CubeIDE software's pin assignment view to quickly check which pins connect to which timer.
 
-Note that the index (I) pin is currently not used.
-
+Right now the index pin can be any pin supporting interrupts.
 
 ## Software setup
 
@@ -46,4 +45,22 @@ void setup() {
     encoder.init();
 }
 
+```
+
+Set the encoder angle:
+
+```c++
+encoder.setCurrentAngle(1.28);
+```
+
+Set the encoder count:
+
+```c++
+encoder.setEncoderCount(693);
+```
+
+Get the encoder timer handle:
+
+```c++
+encoder.getEncoderTimerHandle();
 ```

src/encoders/stm32hwencoder/STM32HWEncoder.cpp

@@ -15,15 +15,30 @@ STM32HWEncoder::STM32HWEncoder(unsigned int _ppr, int pinA, int pinB, int pinI)
     index_found = false;
 }
 
+// function returns encoder handle
+TIM_HandleTypeDef STM32HWEncoder::getEncoderTimerHandle() { return encoder_handle; }
+
 /*
   Shaft angle calculation
 */
 float STM32HWEncoder::getSensorAngle() { 
     return _2PI * encoder_handle.Instance->CNT / static_cast<float>(cpr);
 }
+/* 
+  Set the current angle using CNT register
+*/
+void STM32HWEncoder::setCurrentAngle(float set_angle) {
+    encoder_handle.Instance->CNT = set_angle * cpr / _2PI;
+}
+/*
+  Modify encoder count directly
+*/
+void STM32HWEncoder::setEncoderCount(uint32_t ecount) {
+    encoder_handle.Instance->CNT = ecount;
+}
 
 // getter for index pin
-int STM32HWEncoder::needsSearch() { return false && !index_found; }
+int STM32HWEncoder::needsSearch() { return hasIndex() && !index_found; }
 
 // private function used to determine if encoder has index
 int STM32HWEncoder::hasIndex() { return (_pinI!=NC); }
@@ -34,11 +49,13 @@ void STM32HWEncoder::init() {
     TIM_TypeDef *InstanceA = (TIM_TypeDef *)pinmap_peripheral(_pinA, PinMap_TIM);
     if (!IS_TIM_ENCODER_INTERFACE_INSTANCE(InstanceA)) {
         initialized = false;
+        SIMPLEFOC_DEBUG("STM32HWEncoder pin A doesn't support encoder interface");
         return;
     }
     TIM_TypeDef *InstanceB = (TIM_TypeDef *)pinmap_peripheral(_pinB, PinMap_TIM);
     if (InstanceA != InstanceB) {
         initialized = false;
+        SIMPLEFOC_DEBUG("STM32HWEncoder pin B is not in the same timer as A");
         return;
     }
     pinmap_pinout(_pinA, PinMap_TIM);
@@ -74,17 +91,38 @@ void STM32HWEncoder::init() {
 
     if (HAL_TIM_Encoder_Init(&encoder_handle, &encoder_config) != HAL_OK) {
         initialized = false;
+        SIMPLEFOC_DEBUG("Couldn't initialize timer in encoder mode!");
         return;
     }
 
-    // TODO on STM32G4 MCUs we can use the TIMx_ETR pin for the index, and configure how it is handled automatically by the hardware
-    // on non-G4 MCUs we need to use an external interrupt to handle the index signal
-    // attachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)), [this]() {
-    //     encoder_handle.Instance->CNT = 0; // reset counter
-    //     index_found = true;
-    //     // detach interrupt
-    //     detachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)));
-    // }, index_polarity);
+    // TODO: figure out way to check if Index pin is specifically ETR line
+    //if(IS_TIM_ETR_INSTANCE())
+
+    // Encoder index configuration
+    //TIMEx_EncoderIndexConfigTypeDef encoder_indexconfig = {0};
+    //encoder_indexconfig.Polarity = TIM_ENCODERINDEX_POLARITY_NONINVERTED;
+    //encoder_indexconfig.Prescaler = TIM_ENCODERINDEX_PRESCALER_DIV1;
+    //encoder_indexconfig.Filter = 0;
+    //encoder_indexconfig.FirstIndexEnable = ENABLE;
+    //encoder_indexconfig.Position = TIM_ENCODERINDEX_POSITION_00;
+    //encoder_indexconfig.Direction = TIM_ENCODERINDEX_DIRECTION_UP_DOWN; // Double check this is ok
+    //if (HAL_TIMEx_ConfigEncoderIndex(&encoder_handle, &encoder_indexconfig) != HAL_OK)
+    //{
+    //    SIMPLEFOC_DEBUG("Couldn't configure encoder index pin");
+    //    initialized = false;
+    //    return;
+    //}
+
+    // If index pin provided attach search interrupt
+    if(hasIndex())
+    {
+        attachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)), [this]() {
+            encoder_handle.Instance->CNT = 0; // reset counter
+            index_found = true;
+            // detach interrupt
+            detachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)));
+        }, index_polarity);
+    }
 
     if (HAL_TIM_Encoder_Start(&encoder_handle, TIM_CHANNEL_1) != HAL_OK) {
         initialized = false;

src/encoders/stm32hwencoder/STM32HWEncoder.h

@@ -21,6 +21,9 @@ class STM32HWEncoder : public Sensor {
     void init() override;
     int needsSearch() override;
     int hasIndex();  // !< function returning 1 if encoder has index pin and 0 if not.
+    void setCurrentAngle(float set_angle); // !< helper function for setting the angle by using the count register
+    void setEncoderCount(uint32_t ecount); // !< function for setting the count register
+    TIM_HandleTypeDef getEncoderTimerHandle(); // !< function for getting the encoder timer handle
 
     bool initialized = false;
     uint32_t cpr;  //!< encoder cpr number