From 0746c42eaafa8cffaf1fd6ddc4a9042b0fd8da4d Mon Sep 17 00:00:00 2001 From: Gabriel Zerbib Date: Mon, 25 May 2026 09:55:44 +0200 Subject: [PATCH 1/3] Add Phoque1 current sense, lighten Phoque2 current sense code --- .../phoque1/Phoque1_CurrentSense.cpp | 219 ++++++++++++++++++ .../phoque1/Phoque1_CurrentSense.hpp | 26 +++ .../phoque2a/Phoque2a_CurrentSense.cpp | 33 +-- 3 files changed, 251 insertions(+), 27 deletions(-) create mode 100644 src/current_sense/phoque1/Phoque1_CurrentSense.cpp create mode 100644 src/current_sense/phoque1/Phoque1_CurrentSense.hpp diff --git a/src/current_sense/phoque1/Phoque1_CurrentSense.cpp b/src/current_sense/phoque1/Phoque1_CurrentSense.cpp new file mode 100644 index 0000000..2943af9 --- /dev/null +++ b/src/current_sense/phoque1/Phoque1_CurrentSense.cpp @@ -0,0 +1,219 @@ +#if defined(ARDUINO_PHOQUE1) + +#include "Phoque1_CurrentSense.hpp" +#include "communication/SimpleFOCDebug.h" +#include "current_sense/hardware_specific/stm32/stm32_adc_utils.h" + +static OPAMP_HandleTypeDef hopamp1; +static OPAMP_HandleTypeDef hopamp2; +static OPAMP_HandleTypeDef hopamp3; + +Phoque1_CurrentSense::Phoque1_CurrentSense(float _shunt_resistor, float _gain, bool _read_bemf) + :Phoque_CurrentSense(_shunt_resistor, _gain, _read_bemf) +{ + pinA = A_CURRU_H; + pinB = A_CURRV_H; + pinC = A_CURRW_H; +} + +Phoque1_CurrentSense::Phoque1_CurrentSense(float mVpA, bool _read_bemf) + :Phoque_CurrentSense(mVpA, _read_bemf) +{ + pinA = A_CURRU_H; + pinB = A_CURRV_H; + pinC = A_CURRW_H; +} + +Phoque1_CurrentSense::~Phoque1_CurrentSense() +{ +} + +void Phoque1_CurrentSense::Opamp_Init() +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + GPIO_InitStruct.Pin = GPIO_PIN_1|GPIO_PIN_3 | GPIO_PIN_5|GPIO_PIN_7; //Opamp 1 | Opamp 2 + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_2; // Opamp 3 + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + OPAMP_HandleTypeDef *opamp_handles[] = {&hopamp1, &hopamp2, &hopamp3}; + OPAMP_TypeDef *opamp_instances[] = {OPAMP1, OPAMP2, OPAMP3}; + static_assert(sizeof(opamp_handles)/sizeof(opamp_handles[0]) == sizeof(opamp_instances)/sizeof(opamp_instances[0])); + + for (size_t i = 0; i < sizeof(opamp_handles)/sizeof(opamp_handles[0]); i++) + { + auto hopamp = opamp_handles[i]; + hopamp->Instance = opamp_instances[i]; + hopamp->Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED; + hopamp->Init.Mode = OPAMP_PGA_MODE; + hopamp->Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0; + hopamp->Init.InternalOutput = ENABLE; + hopamp->Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; + hopamp->Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0_BIAS; + hopamp->Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15; + hopamp->Init.UserTrimming = OPAMP_TRIMMING_FACTORY; + if (HAL_OPAMP_Init(hopamp) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_OPAMP_Init failed!"); + } + HAL_OPAMP_Start(hopamp); + } +} + +int Phoque1_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) +{ + ADC_ChannelConfTypeDef sConfig = {0}; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + + hadc1->Init.NbrOfConversion += 3 + read_bemf * 2; + Phoque_CurrentSense::ADC1_Init(hadc1); + + /** Configure Regular Channel (Opamp 1 / phase W current) + */ + sConfig.Channel = ADC_CHANNEL_13; // OP1_OUT is ADC1_IN13 for internal channel + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + if(read_bemf) + { + /* Configure Regular Channel (PA0 / BEMFU / Phase U) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFU), ADC1); + sConfig.Rank = ADC_REGULAR_RANK_2; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + /* Configure Regular Channel (PC4 / BEMFV / Phase V) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFV), ADC1); + sConfig.Rank = ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + } + + //****************************************************************** + // Aux analog readings + /* Configure Regular Channel (PC1, supply voltage) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_VBUS), ADC1); + sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_4 : ADC_REGULAR_RANK_2; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + /** Configure Regular Channel (PC0, Potentiometer) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_POTENTIOMETER), ADC1); + sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_5 : ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + return hadc1->Init.NbrOfConversion; +} + +int Phoque1_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) +{ + ADC_ChannelConfTypeDef sConfig = {0}; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + + hadc2->Init.NbrOfConversion += 3 + read_bemf; + Phoque_CurrentSense::ADC2_Init(hadc2); + + /** Configure Regular Channel (Opamp 2 / phase U current) + */ + sConfig.Channel = ADC_CHANNEL_16; // OP2_OUT is ADC2_IN16 for internal channel + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + /** Configure Regular Channel (Opamp 3 / phase V current) + */ + sConfig.Channel = ADC_CHANNEL_18; // OP3_OUT is ADC2_IN18 for internal channel + sConfig.Rank = ADC_REGULAR_RANK_2; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + if(read_bemf) + { + /** Configure Regular Channel (PA2 / BEMFW / Phase W) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFW), ADC2); + sConfig.Rank = ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + } + + /** Configure Regular Channel (PF1 / Mosfet temperature) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_TEMPERATURE), ADC2); + sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_4 : ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + return hadc2->Init.NbrOfConversion; +} + +uint16_t Phoque1_CurrentSense::readRaw(const int pin) +{ + switch (pin) + { + case A_CURRU_H: + case -1: + return adc2_buffer[0]; + case A_CURRV_H: + case -2: + return adc2_buffer[1]; + case A_CURRW_H: + case -3: + return adc1_buffer[0]; + + case A_BEMFU: + return adc1_buffer[1]; + case A_BEMFV: + return adc1_buffer[2]; + case A_BEMFW: + return adc2_buffer[2]; + + case A_POTENTIOMETER: + return adc1_buffer[2+read_bemf*2]; + case A_TEMPERATURE: + return adc2_buffer[2+read_bemf]; + case A_VBUS: + return adc1_buffer[1+read_bemf*2]; + default: + return 0; + } +} + +#endif \ No newline at end of file diff --git a/src/current_sense/phoque1/Phoque1_CurrentSense.hpp b/src/current_sense/phoque1/Phoque1_CurrentSense.hpp new file mode 100644 index 0000000..ddf2890 --- /dev/null +++ b/src/current_sense/phoque1/Phoque1_CurrentSense.hpp @@ -0,0 +1,26 @@ +#pragma once + +#if defined(ARDUINO_PHOQUE1) + +#include "current_sense/phoque/Phoque_CurrentSense.hpp" + +class Phoque1_CurrentSense : public Phoque_CurrentSense +{ +private: + volatile uint16_t *adc1_buffer = nullptr; + volatile uint16_t *adc2_buffer = nullptr; + bool read_bemf; +public: + Phoque1_CurrentSense(float shunt_resistor, float gain, bool read_bemf=false); + Phoque1_CurrentSense(float mVpA, bool read_bemf=false); + ~Phoque1_CurrentSense(); + + uint16_t readRaw(const int pin); + +private: + void Opamp_Init(); + int ADC1_Init(ADC_HandleTypeDef* hadc1); + int ADC2_Init(ADC_HandleTypeDef* hadc2); +}; + +#endif \ No newline at end of file diff --git a/src/current_sense/phoque2a/Phoque2a_CurrentSense.cpp b/src/current_sense/phoque2a/Phoque2a_CurrentSense.cpp index b7fbe80..2a8e957 100644 --- a/src/current_sense/phoque2a/Phoque2a_CurrentSense.cpp +++ b/src/current_sense/phoque2a/Phoque2a_CurrentSense.cpp @@ -29,6 +29,9 @@ Phoque2a_CurrentSense::~Phoque2a_CurrentSense() int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) { ADC_ChannelConfTypeDef sConfig = {0}; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; hadc1->Init.NbrOfConversion += 3 + read_bemf * 2; @@ -39,9 +42,6 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Channel = _getADCChannel(analogInputToPinName(A_CURRU), ADC1); //ADC_CHANNEL_2; sConfig.Rank = ADC_REGULAR_RANK_1; sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); @@ -54,9 +54,6 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFV), ADC1); //ADC_CHANNEL_15; sConfig.Rank = ADC_REGULAR_RANK_2; sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); @@ -67,9 +64,6 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFW), ADC1); //ADC_CHANNEL_12; sConfig.Rank = ADC_REGULAR_RANK_3; sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); @@ -83,9 +77,6 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Channel = _getADCChannel(analogInputToPinName(A_TEMPERATURE), ADC1); //ADC_CHANNEL_11; sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_4 : ADC_REGULAR_RANK_2; sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); @@ -96,9 +87,6 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Channel = _getADCChannel(analogInputToPinName(A_VBUS), ADC1); //ADC_CHANNEL_4; sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_5 : ADC_REGULAR_RANK_3; sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); @@ -109,6 +97,9 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) int Phoque2a_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) { ADC_ChannelConfTypeDef sConfig = {0}; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; hadc2->Init.NbrOfConversion += 3 + read_bemf; @@ -119,9 +110,6 @@ int Phoque2a_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.Channel = _getADCChannel(analogInputToPinName(A_CURRV), ADC2); //ADC_CHANNEL_5; sConfig.Rank = ADC_REGULAR_RANK_1; sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); @@ -131,9 +119,6 @@ int Phoque2a_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.Channel = _getADCChannel(analogInputToPinName(A_CURRW), ADC2); //ADC_CHANNEL_12; sConfig.Rank = ADC_REGULAR_RANK_2; sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); @@ -146,9 +131,6 @@ int Phoque2a_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFU), ADC2); //ADC_CHANNEL_1; sConfig.Rank = ADC_REGULAR_RANK_3; sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); @@ -160,9 +142,6 @@ int Phoque2a_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.Channel = _getADCChannel(analogInputToPinName(A_POTENTIOMETER), ADC2); //ADC_CHANNEL_17; sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_4 : ADC_REGULAR_RANK_3; sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); From ebfc050d900c9e38cf29d9a9b68daaef509b4538 Mon Sep 17 00:00:00 2001 From: Gabriel Zerbib Date: Wed, 27 May 2026 12:50:22 +0200 Subject: [PATCH 2/3] Phoque: fix shadowed variables and align --- src/current_sense/phoque/Phoque_CurrentSense.cpp | 2 +- src/current_sense/phoque1/Phoque1_CurrentSense.hpp | 4 ---- src/current_sense/phoque2a/Phoque2a_CurrentSense.hpp | 4 ---- src/drivers/stspin32g4/STSPIN32G4.h | 1 + 4 files changed, 2 insertions(+), 9 deletions(-) diff --git a/src/current_sense/phoque/Phoque_CurrentSense.cpp b/src/current_sense/phoque/Phoque_CurrentSense.cpp index 91d4fe0..b2e5bc4 100644 --- a/src/current_sense/phoque/Phoque_CurrentSense.cpp +++ b/src/current_sense/phoque/Phoque_CurrentSense.cpp @@ -306,7 +306,7 @@ int Phoque_CurrentSense::driverAlign(float align_voltage, bool modulation_center { _UNUSED(align_voltage); _UNUSED(modulation_centered); - return 0; + return 1; } extern "C" { diff --git a/src/current_sense/phoque1/Phoque1_CurrentSense.hpp b/src/current_sense/phoque1/Phoque1_CurrentSense.hpp index ddf2890..01c963a 100644 --- a/src/current_sense/phoque1/Phoque1_CurrentSense.hpp +++ b/src/current_sense/phoque1/Phoque1_CurrentSense.hpp @@ -6,10 +6,6 @@ class Phoque1_CurrentSense : public Phoque_CurrentSense { -private: - volatile uint16_t *adc1_buffer = nullptr; - volatile uint16_t *adc2_buffer = nullptr; - bool read_bemf; public: Phoque1_CurrentSense(float shunt_resistor, float gain, bool read_bemf=false); Phoque1_CurrentSense(float mVpA, bool read_bemf=false); diff --git a/src/current_sense/phoque2a/Phoque2a_CurrentSense.hpp b/src/current_sense/phoque2a/Phoque2a_CurrentSense.hpp index 99d538e..8878c3c 100644 --- a/src/current_sense/phoque2a/Phoque2a_CurrentSense.hpp +++ b/src/current_sense/phoque2a/Phoque2a_CurrentSense.hpp @@ -8,10 +8,6 @@ class Phoque2a_CurrentSense : public Phoque_CurrentSense { //Current sense for the Phoque2 board //for best performance, center modulation should be off -private: - volatile uint16_t *adc1_buffer = nullptr; - volatile uint16_t *adc2_buffer = nullptr; - bool read_bemf; public: Phoque2a_CurrentSense(float shunt_resistor, float gain, bool read_bemf=false); Phoque2a_CurrentSense(float mVpA, bool read_bemf=false); diff --git a/src/drivers/stspin32g4/STSPIN32G4.h b/src/drivers/stspin32g4/STSPIN32G4.h index ed12acf..3ca0395 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.h +++ b/src/drivers/stspin32g4/STSPIN32G4.h @@ -5,6 +5,7 @@ #ifdef ARDUINO_GENERIC_G431VBTX +#include "stm32g4xx_hal_i2c.h" #include "Wire.h" #include "drivers/BLDCDriver6PWM.h" From 67ec7c7c3286fd8fecfc404794d2834c1288b754 Mon Sep 17 00:00:00 2001 From: Gabriel Zerbib Date: Sat, 13 Jun 2026 19:38:27 +0200 Subject: [PATCH 3/3] Phoque current sense: use DMA2, disable interrupts on DMA --- .../phoque/Phoque_CurrentSense.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/current_sense/phoque/Phoque_CurrentSense.cpp b/src/current_sense/phoque/Phoque_CurrentSense.cpp index b2e5bc4..1f45851 100644 --- a/src/current_sense/phoque/Phoque_CurrentSense.cpp +++ b/src/current_sense/phoque/Phoque_CurrentSense.cpp @@ -54,13 +54,15 @@ void Phoque_CurrentSense::DMA_Init() __HAL_RCC_DMA1_CLK_ENABLE(); __HAL_RCC_DMA2_CLK_ENABLE(); + #ifdef DMA_USE_INTERRUPT /* DMA interrupt init */ /* DMA1_Channel1_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0); HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn); /* DMA1_Channel2_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn); + HAL_NVIC_SetPriority(DMA2_Channel1_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(DMA2_Channel1_IRQn); + #endif // Enable external clock for ADC RCC_PeriphCLKInitTypeDef PeriphClkInit; @@ -171,7 +173,6 @@ void* Phoque_CurrentSense::SyncLowSide(void* _driver_params, void* _cs_params) SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt not supported for this Phoque"); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } - // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); @@ -202,7 +203,7 @@ void* Phoque_CurrentSense::ConfigureADC(const void* driver_params, const int pin HAL_ADCEx_Calibration_Start(&hadc2,ADC_SINGLE_ENDED); DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1); - DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2); + DMA1_Init(&hadc2, &hdma_adc2, DMA2_Channel1, DMA_REQUEST_ADC2); if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc1_buffer, adc1_len) != HAL_OK) { @@ -276,6 +277,7 @@ int Phoque_CurrentSense::init() void Phoque_CurrentSense::calibrateOffsets(){ const int calibration_rounds = 2000; + // find adc offset = zero current voltage uint32_t accA = 0, accB = 0, accC = 0; // read the adc voltage 1000 times ( arbitrary number ) @@ -287,7 +289,11 @@ void Phoque_CurrentSense::calibrateOffsets(){ //no new data: this iteration didn't happen i--; //wait for interrupt (dma irq should do it) + #ifdef DMA_USE_INTERRUPT __WFI(); + #else + delayMicroseconds(100); + #endif continue; } //invalidate current data @@ -296,10 +302,12 @@ void Phoque_CurrentSense::calibrateOffsets(){ accB += currv; accC += currw; } + // calculate the mean offsets offset_ia = accA * ((Stm32CurrentSenseParams*)params)->adc_voltage_conv / calibration_rounds; offset_ib = accB * ((Stm32CurrentSenseParams*)params)->adc_voltage_conv / calibration_rounds; offset_ic = accC * ((Stm32CurrentSenseParams*)params)->adc_voltage_conv / calibration_rounds; + SimpleFOCDebug::printf("PHOQUE-CS: Calibrated centers at %f, %f, %f\r\n", offset_ia, offset_ib, offset_ic); } int Phoque_CurrentSense::driverAlign(float align_voltage, bool modulation_centered) @@ -309,14 +317,16 @@ int Phoque_CurrentSense::driverAlign(float align_voltage, bool modulation_center return 1; } +#ifdef DMA_USE_INTERRUPT extern "C" { void DMA1_Channel1_IRQHandler(void) { HAL_DMA_IRQHandler(&hdma_adc1); } -void DMA1_Channel2_IRQHandler(void) { +void DMA2_Channel1_IRQHandler(void) { HAL_DMA_IRQHandler(&hdma_adc2); } } +#endif #endif \ No newline at end of file