Merge branch 'dev' into Candas1-autotuning

Author: askuric

src/common/base_classes/FOCMotor.h

@@ -46,8 +46,9 @@ enum MotionControlType : uint8_t {
   torque            = 0x00,     //!< Torque control
   velocity          = 0x01,     //!< Velocity motion control
   angle             = 0x02,     //!< Position/angle motion control
-  velocity_openloop = 0x03,     //!< Open loop velocity control
-  angle_openloop    = 0x04      //!< Open loop position/angle control
+  velocity_openloop = 0x03,
+  angle_openloop    = 0x04,
+  angle_nocascade   = 0x05      //!< Position/angle motion control without velocity cascade
 };
 
 /**

src/communication/Commander.cpp

@@ -443,6 +443,9 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
             case MotionControlType::angle_openloop:
               println(F("angle open"));
               break;
+            case MotionControlType::angle_nocascade:
+              println(F("angle nocascade"));
+              break;
           }
             break;
         }
@@ -552,6 +555,7 @@ void Commander::target(FOCMotor* motor,  char* user_cmd, char* separator){
   float pos, vel, torque;
   char* next_value;
   switch(motor->controller){
+    case MotionControlType::angle_nocascade:
     case MotionControlType::torque: // setting torque target
       torque = atof(strtok (user_cmd, separator));
       motor->target = torque;

src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp

@@ -186,7 +186,8 @@ void* IRAM_ATTR _driverSyncLowSide(void* driver_params, void* cs_params){
   // if comparator creation failed, fall back to on_full callback
   if (cs->pretrig_comparator){
     // Calculate pwm duty cycle ticks for pre-trigger channel
-    uint32_t pwm_duty_cycle  = p->mcpwm_period * (0.75 - ((float)p->pwm_frequency*SIMPLEFOC_CS_PRETRIGGER_US)/1e6/2.0);
+    // TODO: verify the timing it seems to be correct between 15 and 20kHz (but needs better testing)
+    uint32_t pwm_duty_cycle  = p->mcpwm_period * (0.75 - ((float)p->pwm_frequency*SIMPLEFOC_CS_PRETRIGGER_US)/1e6/2.0); 
     // set up the comparator duty cycle
     CHECK_CS_ERR(mcpwm_comparator_set_compare_value((mcpwm_cmpr_handle_t)cs->pretrig_comparator, pwm_duty_cycle),
                 "Failed to set pretrigger compare value");

src/current_sense/hardware_specific/stm32/stm32_adc_hal.h

@@ -0,0 +1,23 @@
+#ifndef STM32_ADC_INCLUDE_HAL
+
+#include "Arduino.h"
+
+#if defined(_STM32_DEF_) 
+
+#include "stm32_mcu.h"
+
+// for searching the best ADCs, we need to know the number of ADCs 
+// it might be better to use some HAL variable for example ADC_COUNT
+// here I've assumed the maximum number of ADCs is 5
+#define ADC_COUNT 5
+
+// pointer to the ADC handles used in the project
+ADC_HandleTypeDef* _get_adc_handles();
+
+int _adc_init_regular(ADC_TypeDef* adc_instance);
+int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params);
+int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
+
+#endif
+
+#endif

src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp

@@ -1,10 +1,19 @@
 #include "stm32_adc_utils.h"
 #include "stm32_mcu.h"
+#include "stm32_adc_hal.h"
 
 #if defined(_STM32_DEF_) 
 
+#ifdef STM32F1xx
+#include "stm32f1xx_ll_adc.h"
+#endif
+#ifdef STM32F7xx
+#include "stm32f7xx_ll_adc.h"
+#endif
 
 
+extern ADC_HandleTypeDef hadc[];
+
 int _adcToIndex(ADC_TypeDef *AdcHandle){
   if(AdcHandle == ADC1) return 0;
 #ifdef ADC2 // if ADC2 exists
@@ -110,14 +119,39 @@ int _findIndexOfLastPinMapADCEntry(int pin) {
   return _findIndexOfLastEntry(pinName);
 }
 
+// find the best ADC for the given pin
+// returns the ADC_TypeDef pointer or nullptr if not found
+// It returns already configured ADC if possible
+// otherwise it returns the first available unconfigured ADC
+ADC_TypeDef* _findBestADCForRegularPin(int pin, ADC_HandleTypeDef adc_handles[]) {
+  PinName pinName = digitalPinToPinName(pin);
+  int index = _findIndexOfFirstPinMapADCEntry(pin);
+  int last_index = _findIndexOfLastPinMapADCEntry(pin);
+  if (index == -1) {
+    return nullptr;
+  }
+  for (int j = index; j <= last_index; j++) {
+    if (PinMap_ADC[j].pin == NC) {
+      break;
+    }
+    int adcIndex = _adcToIndex((ADC_TypeDef*)PinMap_ADC[j].peripheral);
+    if (adc_handles[adcIndex].Instance != NP) {
+      // if ADC is already configured, return it
+      return (ADC_TypeDef*)PinMap_ADC[j].peripheral;
+    }
+  }
+  // return the first available ADC
+  return (ADC_TypeDef*)PinMap_ADC[index].peripheral;
+}
+
 // find the best ADC combination for the given pins
 // returns the index of the best ADC 
 // each pin can be connected to multiple ADCs
 // the function will try to find a single ADC that can be used for all pins
 // if not possible it will return nullptr
-ADC_TypeDef* _findBestADCForPins(int numPins, int pins[]) {
+ADC_TypeDef* _findBestADCForInjectedPins(int numPins, int pins[], ADC_HandleTypeDef adc_handles[]) {
 
-  // assuning that there is less than 8 ADCs
+  // assuning that there is at most 5 ADCs
   uint8_t pins_at_adc[ADC_COUNT] = {0};
 
   // check how many pins are there and are not set
@@ -152,11 +186,23 @@ ADC_TypeDef* _findBestADCForPins(int numPins, int pins[]) {
     SimpleFOCDebug::print(i+1);
     SimpleFOCDebug::print(" pins: ");
     SimpleFOCDebug::println(pins_at_adc[i]);
+    if (adc_handles[i].Instance != NP) {
+      // check if ADC injeted is already in use
+      if(!LL_ADC_INJ_IsTriggerSourceSWStart(adc_handles[i].Instance)) {
+       SimpleFOCDebug::print("STM32-CS: ADC");
+        SimpleFOCDebug::print(i+1);
+        SimpleFOCDebug::println(" already in use for injected channels!");
+      }
+    }
   }
 #endif
 
   // now take the first ADC that has all pins connected
   for (int i = 0; i < ADC_COUNT; i++) {
+    if (adc_handles[i].Instance != NP) {
+      if (!LL_ADC_INJ_IsTriggerSourceSWStart(adc_handles[i].Instance))
+        continue; // ADC already in use for injected
+    }
     if (pins_at_adc[i] == no_pins) {
       return _indexToADC(i);
     }
@@ -363,7 +409,6 @@ uint32_t _getADCChannel(PinName pin, ADC_TypeDef *AdcHandle )
   for (int i = first_ind; i <= last_ind; i++) {
     if (PinMap_ADC[i].peripheral == AdcHandle) {
       channel =_getADCChannelFromPinMap(PinMap_ADC[i].pin);
-      SIMPLEFOC_DEBUG("STM32-CS: ADC channel: ", (int)STM_PIN_CHANNEL(pinmap_function(PinMap_ADC[i].pin, PinMap_ADC)));
       break;
     }
   }
@@ -494,4 +539,134 @@ float _readADCInjectedChannelVoltage(int pin, void* cs_params, Stm32AdcInterrupt
   #endif
 }
 
+
+
+int last_pin[ADC_COUNT] = {-1,-1,-1,-1,-1};
+uint32_t last_channel[ADC_COUNT] = {0,0,0,0,0};
+
+/**
+ * Read a regular ADC channel while injected channels are running for current sensing.
+ * 
+ * This function performs a one-shot regular conversion on the same ADC that is being
+ * used for injected current sensing. Injected conversions have hardware priority and
+ * will pre-empt regular conversions, so this function may experience some latency.
+ * 
+ * The function will retry a few times if the ADC returns HAL_BUSY, making it suitable
+ * for reading auxiliary sensors (temperature, voltage, potentiometers, etc.) while
+ * motor control is active.
+ * 
+ * @param pin - Arduino pin number to read (must be on the same ADC as current sensing)
+ * @return float - Voltage reading in volts, or -1.0f on error
+ */
+float _readRegularADCVoltage(const int pin){
+
+  ADC_HandleTypeDef* hadc = _get_adc_handles();
+
+  int adc_index = NOT_SET;
+  for(int i = 0; i < ADC_COUNT; i++){
+    if(last_pin[i] == pin){
+      adc_index = i;
+      break;
+    }
+  }
+  // avoid re-configuring the channel if reading the same pin as last time
+  if(!_isset(adc_index)){
+    ADC_TypeDef* adc_instance = _findBestADCForRegularPin(pin, hadc);
+    if(adc_instance == NP){
+  #ifdef SIMPLEFOC_STM32_DEBUG
+      SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
+  #endif
+      return -1.0f;
+    } 
+    adc_index = _adcToIndex(adc_instance);
+
+    ADC_HandleTypeDef adc_handle = hadc[adc_index];
+    if (adc_handle.Instance == NP) {
+  #ifdef SIMPLEFOC_STM32_DEBUG
+      SIMPLEFOC_DEBUG("STM32-CS: WARN: ADC not configured, need to configure it: ADC", adc_index+1);
+  #endif
+      if(_adc_init_regular(adc_instance) != 0){
+  #ifdef SIMPLEFOC_STM32_DEBUG
+        SIMPLEFOC_DEBUG("STM32-CS: ERR: Failed to initialize ADC for pin ", pin);
+  #endif
+        return -1.0f;
+      }
+    }
+
+    
+  last_pin[adc_index] = pin;
+  // Configure the regular channel for this pin
+  PinName pinName = analogInputToPinName(pin);
+  uint32_t channel = _getADCChannel(pinName, adc_instance);
+  
+  last_channel[adc_index] = channel;
+  }
+  
+
+  ADC_ChannelConfTypeDef sConfig = {0};
+  sConfig.Channel = last_channel[adc_index];
+  // the shortes possible sampling time 
+  // this seems to be a constant in HAL - the shortest time enum is equal to 0
+  // G4 - 2.5 cycles
+  // F1, H7 - 1.5 cycles
+  // L4 - 2.5 cycles
+  // F4, F7 - 3 cycles
+  sConfig.SamplingTime = 0; 
+
+#ifdef ADC_REGULAR_RANK_1
+  sConfig.Rank = ADC_REGULAR_RANK_1;
+#else
+  sConfig.Rank = 1;
+#endif
+#ifdef ADC_SINGLE_ENDED
+  sConfig.SingleDiff = ADC_SINGLE_ENDED;
+#endif
+#ifdef ADC_OFFSET_NONE
+  sConfig.OffsetNumber = ADC_OFFSET_NONE;
+#endif
+#ifndef STM32F1xx
+  sConfig.Offset = 0;
+#endif 
+
+  if (HAL_ADC_ConfigChannel(&hadc[adc_index], &sConfig) != HAL_OK) {
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: ERR: Failed to configure regular channel");
+#endif
+    return -1.0f;
+  }
+
+  // Try to start conversion, with retries for HAL_BUSY
+  // (ADC may be busy with injected conversion)
+  HAL_StatusTypeDef status;
+  int retries = 5;
+  
+  do {
+    status = HAL_ADC_Start(&hadc[adc_index]);
+    if (status == HAL_BUSY) {
+      // Wait a bit for injected conversion to complete
+      delayMicroseconds(1);
+      retries--;
+    }
+  } while (status == HAL_BUSY && retries > 0);
+  
+  if (status != HAL_OK) {
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: ERR: ADC busy or failed to start");
+#endif
+    return -1.0f;
+  }
+  
+  // Wait for conversion to complete
+  // Timeout of 1ms should be more than enough
+  if (HAL_ADC_PollForConversion(&hadc[adc_index], 1) == HAL_OK) {
+    uint32_t raw = HAL_ADC_GetValue(&hadc[adc_index]);
+    return raw * 3.3f / 4096.0f; // assuming 12-bit ADC and 3.3V reference
+  }
+  
+#ifdef SIMPLEFOC_STM32_DEBUG
+  SIMPLEFOC_DEBUG("STM32-CS: ERR: Regular conversion timeout");
+#endif
+  return -1.0f;
+}
+
 #endif

src/current_sense/hardware_specific/stm32/stm32_adc_utils.h

@@ -7,19 +7,12 @@
 
 #define _TRGO_NOT_AVAILABLE 12345
 
-// for searching the best ADCs, we need to know the number of ADCs 
-// it might be better to use some HAL variable for example ADC_COUNT
-// here I've assumed the maximum number of ADCs is 5
-#define ADC_COUNT 5
-
 
 #include "../../../common/foc_utils.h"
 #include "../../../communication/SimpleFOCDebug.h"
 #include "../../../drivers/hardware_specific/stm32/stm32_mcu.h"
 #include "stm32_mcu.h"
 
-
-
 /* Exported Functions */
 /**
   * @brief  Return ADC HAL channel linked to a PinName
@@ -43,8 +36,8 @@ int _adcToIndex(ADC_TypeDef *AdcHandle);
 // functions helping to find the best ADC channel
 int _findIndexOfFirstPinMapADCEntry(int pin);
 int _findIndexOfLastPinMapADCEntry(int pin);
-ADC_TypeDef* _findBestADCForPins(int num_pins, int pins[]);
-
+ADC_TypeDef* _findBestADCForInjectedPins(int num_pins, int pins[], ADC_HandleTypeDef adc_handles[]);
+ADC_TypeDef* _findBestADCForRegularPin(int pin, ADC_HandleTypeDef adc_handles[]);
 
 // Structure to hold ADC interrupt configuration per ADC instance
 struct Stm32AdcInterruptConfig {
@@ -61,5 +54,7 @@ uint8_t _handleInjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle, Stm32AdcIn
 // returns the voltage 
 // if the pin is not found in the current sense parameters, returns 0
 float _readADCInjectedChannelVoltage(int pin, void* cs_params, Stm32AdcInterruptConfig& adc_interrupt_config, uint32_t adc_val[4]);
+
+
 #endif
 #endif

src/current_sense/hardware_specific/stm32/stm32_mcu.cpp

@@ -8,6 +8,8 @@
 #define _ADC_VOLTAGE 3.3f
 #define _ADC_RESOLUTION 1024.0f
 
+extern ADC_HandleTypeDef hadc[];
+
 // function reading an ADC value and returning the read voltage
 void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
   _UNUSED(driver_params);
@@ -30,5 +32,4 @@ __attribute__((weak))  float _readADCVoltageInline(const int pinA, const void* c
   return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
 }
 
-
 #endif

src/current_sense/hardware_specific/stm32/stm32_mcu.h

@@ -20,5 +20,26 @@ typedef struct Stm32CurrentSenseParams {
 } Stm32CurrentSenseParams;
 
 
+/**
+ * Read a regular ADC channel while injected channels are running for current sensing.
+ * Injected conversions have hardware priority and will pre-empt regular conversions.
+ * 
+ * This funciton performs a one-shot regular conversion on either on the same ADC as the one 
+ * used for injected current sensing, or on another ADC if the pin belongs to a different ADC.
+ * The funciton will initialize the ADC for regular conversion if not already initialized. 
+ * 
+ * NOTE:
+ * The low-side current sensing code already initializes the ADC for injected conversions and regullar conversions
+ * so if the same ADC is used for regular reading, no re-configuration is needed.
+ * 
+ * NOTE:
+ * This function will be relatively slow >10us in comparision to the injected reading. 
+ * But it is much better than analogRead though.
+ * 
+ * @param pin - the Arduino pin to be read (must be an ADC pin on the same ADC)
+ * @return float - the voltage read from the pin, or -1.0f on error
+ */
+float _readRegularADCVoltage(const int pin);
+
 #endif
 #endif