From 0892c045440db403f73d343f5e3795e26ba71577 Mon Sep 17 00:00:00 2001 From: Gabriel Zerbib Date: Tue, 28 Jul 2026 11:33:37 +0200 Subject: [PATCH] Phoque: add comments and debug temperature LUT, allow any stm32g4, add Phoque2 current sense, add Phoque1 external opamp mode --- .../phoque/Phoque_CurrentSense.cpp | 38 +++- .../phoque/Phoque_CurrentSense.hpp | 4 +- .../phoque1/Phoque1_CurrentSense.cpp | 59 ++++- .../phoque1/Phoque1_CurrentSense.hpp | 8 +- .../phoque2/Phoque2_CurrentSense.cpp | 205 ++++++++++++++++++ .../phoque2/Phoque2_CurrentSense.hpp | 28 +++ .../phoque2a/Phoque2a_CurrentSense.cpp | 18 +- 7 files changed, 328 insertions(+), 32 deletions(-) create mode 100644 src/current_sense/phoque2/Phoque2_CurrentSense.cpp create mode 100644 src/current_sense/phoque2/Phoque2_CurrentSense.hpp diff --git a/src/current_sense/phoque/Phoque_CurrentSense.cpp b/src/current_sense/phoque/Phoque_CurrentSense.cpp index e04bb4b..242ff8a 100644 --- a/src/current_sense/phoque/Phoque_CurrentSense.cpp +++ b/src/current_sense/phoque/Phoque_CurrentSense.cpp @@ -1,4 +1,4 @@ -#if defined(ARDUINO_PHOQUE) +#if defined(STM32G4xx) #include "Phoque_CurrentSense.hpp" #include @@ -103,7 +103,7 @@ int Phoque_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) if (HAL_ADC_Init(hadc1) != HAL_OK) { - SIMPLEFOC_DEBUG(PHOQUE_CS_DEBUG "HAL_ADC_Init failed!"); + SimpleFOCDebug::println(PHOQUE_CS_DEBUG "HAL_ADC_Init failed!"); } /** Configure the ADC multi-mode @@ -111,7 +111,7 @@ int Phoque_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) multimode.Mode = ADC_MODE_INDEPENDENT; if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK) { - SIMPLEFOC_DEBUG(PHOQUE_CS_DEBUG "HAL_ADCEx_MultiModeConfigChannel 1 failed!"); + SimpleFOCDebug::println(PHOQUE_CS_DEBUG "HAL_ADCEx_MultiModeConfigChannel 1 failed!"); } return 0; @@ -137,7 +137,7 @@ int Phoque_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) if (HAL_ADC_Init(hadc2) != HAL_OK) { - SIMPLEFOC_DEBUG(PHOQUE_CS_DEBUG "HAL_ADC_Init failed!"); + SimpleFOCDebug::println(PHOQUE_CS_DEBUG "HAL_ADC_Init failed!"); } return 0; @@ -157,7 +157,7 @@ void Phoque_CurrentSense::DMA_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *h HAL_DMA_DeInit(hdma_adc); if (HAL_DMA_Init(hdma_adc) != HAL_OK) { - SIMPLEFOC_DEBUG(PHOQUE_CS_DEBUG "HAL_DMA_Init failed!"); + SimpleFOCDebug::println(PHOQUE_CS_DEBUG "HAL_DMA_Init failed!"); } __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc); } @@ -177,7 +177,7 @@ void* Phoque_CurrentSense::SyncLowSide(void* _driver_params, void* _cs_params) bool tim_interrupt = _initTimerInterruptDownsampling(cs_params, driver_params, adc_int_config); if(tim_interrupt) { // error in the timer interrupt initialization - SIMPLEFOC_DEBUG(PHOQUE_CS_DEBUG "timer has no repetition counter, ADC interrupt not supported for this Phoque"); + SimpleFOCDebug::println(PHOQUE_CS_DEBUG "timer has no repetition counter, ADC interrupt not supported for this Phoque"); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } uint32_t adc_ccr = ADC12_COMMON->CCR; @@ -274,11 +274,11 @@ void* Phoque_CurrentSense::ConfigureADC(const void* driver_params, const int pin if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc1_buffer, adc1_len) != HAL_OK) { - SIMPLEFOC_DEBUG(PHOQUE_CS_DEBUG "ADC1 DMA read init failed"); + SimpleFOCDebug::println(PHOQUE_CS_DEBUG "ADC1 DMA read init failed"); } if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adc2_buffer, adc2_len) != HAL_OK) { - SIMPLEFOC_DEBUG(PHOQUE_CS_DEBUG "ADC2 DMA read init failed"); + SimpleFOCDebug::println(PHOQUE_CS_DEBUG "ADC2 DMA read init failed"); } Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams { @@ -307,7 +307,7 @@ PhaseCurrent_s Phoque_CurrentSense::getPhaseCurrents(){ int Phoque_CurrentSense::init() { if (driver==nullptr) { - SIMPLEFOC_DEBUG(PHOQUE_CS_DEBUG "Driver not linked!"); + SimpleFOCDebug::println(PHOQUE_CS_DEBUG "Driver not linked!"); return 0; } if (initialized) @@ -393,16 +393,21 @@ int Phoque_CurrentSense::driverAlign(float align_voltage, bool modulation_center #if defined(NTC_B_CONSTANT) && defined(NTC_T0) #include +//size of the look up table, in powers of two (7 is 128 values, 6 is 64 values...) #ifndef TEMP_LUT_SIZE #define TEMP_LUT_SIZE 7 #endif #ifndef TEMP_STEP - #define TEMP_STEP 1 //get every n degrees + #define TEMP_STEP 1 //get every n degrees. Can only be greater than 1 since temperature return type is int #endif #ifndef TEMP_INDEX_ZERO #define TEMP_INDEX_ZERO 20 //start at 20C #endif +#ifndef NTC_DIVIDER_BALANCE +#pragma message("NTC divider balance not set, assuming 1") +#endif +//compute the ideal ADC reading for each temperature uint16_t temperature_to_adc_reading(float temp_C) { //R0 : resistance at T0 @@ -418,7 +423,7 @@ uint16_t temperature_to_adc_reading(float temp_C) #else float R_over_R1 = R_over_R0; #endif - float adc_reading = R_over_R1/(1+R_over_R1) *adc_resolution; + float adc_reading = R_over_R1/(1+R_over_R1) * adc_resolution; return adc_reading; } @@ -430,8 +435,11 @@ std::array fill_temperature_lut() { return a; } +//bake the ADC readings into a look up table at compile time const auto temperature_lut = fill_temperature_lut(); +//perform binary search to find the temperature corresponding to the adc value +//Does not perform interpolation, made to fast first and foremost int Phoque_CurrentSense::get_temperature(uint16_t adc_value) { uint8_t index = 0; @@ -450,6 +458,14 @@ int Phoque_CurrentSense::read_temperature() const { return get_temperature(readRaw(A_TEMPERATURE)); } + +void Phoque_CurrentSense::print_temperature_LUT() +{ + for (size_t i = 0; i < temperature_lut.size(); i++) + { + SimpleFOCDebug::printf("%ddegC: %d\r\n", i*TEMP_STEP+TEMP_INDEX_ZERO, temperature_lut[i]); + } +} #endif #ifdef DMA_USE_INTERRUPT diff --git a/src/current_sense/phoque/Phoque_CurrentSense.hpp b/src/current_sense/phoque/Phoque_CurrentSense.hpp index 494bccf..57e64b1 100644 --- a/src/current_sense/phoque/Phoque_CurrentSense.hpp +++ b/src/current_sense/phoque/Phoque_CurrentSense.hpp @@ -1,6 +1,6 @@ #pragma once -#if defined(ARDUINO_PHOQUE) +#if defined(STM32G4xx) #include "common/base_classes/CurrentSense.h" @@ -24,6 +24,8 @@ class Phoque_CurrentSense : public CurrentSense #if defined(NTC_B_CONSTANT) && defined(NTC_T0) static int get_temperature(uint16_t adc_value); int read_temperature() const; + //Print the expected ADC values for each temperature in the look up table (for debug purposes) + static void print_temperature_LUT(); #endif protected: diff --git a/src/current_sense/phoque1/Phoque1_CurrentSense.cpp b/src/current_sense/phoque1/Phoque1_CurrentSense.cpp index a75e696..3caba4d 100644 --- a/src/current_sense/phoque1/Phoque1_CurrentSense.cpp +++ b/src/current_sense/phoque1/Phoque1_CurrentSense.cpp @@ -1,5 +1,6 @@ #if defined(ARDUINO_PHOQUE1) + #include "Phoque1_CurrentSense.hpp" #include "communication/SimpleFOCDebug.h" #include "current_sense/hardware_specific/stm32/stm32_adc_utils.h" @@ -11,6 +12,7 @@ static OPAMP_HandleTypeDef hopamp3; Phoque1_CurrentSense::Phoque1_CurrentSense(float _shunt_resistor, float _gain, bool _read_bemf) :Phoque_CurrentSense(_shunt_resistor, _gain, _read_bemf) { + pga_gain = _gain; pinA = A_CURRU_H; pinB = A_CURRV_H; pinC = A_CURRW_H; @@ -35,15 +37,38 @@ void Phoque1_CurrentSense::OPAMP_Init() GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Pin = GPIO_PIN_1|GPIO_PIN_3 | GPIO_PIN_5|GPIO_PIN_7; //Opamp 1 | Opamp 2 + #ifdef OPAMP_USE_EXTERNAL_CHANNEL + GPIO_InitStruct.Pin |= GPIO_PIN_2 | GPIO_PIN_6; + #endif HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_2; // Opamp 3 + #ifdef OPAMP_USE_EXTERNAL_CHANNEL + GPIO_InitStruct.Pin |= GPIO_PIN_1; + #endif 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])); + uint32_t gain_setting = OPAMP_PGA_GAIN_32_OR_MINUS_31; + switch (pga_gain) + { + case 32: + gain_setting = OPAMP_PGA_GAIN_32_OR_MINUS_31; + break; + case 16: + gain_setting = OPAMP_PGA_GAIN_16_OR_MINUS_15; + break; + case 8: + gain_setting = OPAMP_PGA_GAIN_8_OR_MINUS_7; + break; + + default: + break; + } + for (size_t i = 0; i < sizeof(opamp_handles)/sizeof(opamp_handles[0]); i++) { auto hopamp = opamp_handles[i]; @@ -52,15 +77,19 @@ void Phoque1_CurrentSense::OPAMP_Init() .PowerMode = OPAMP_POWERMODE_HIGHSPEED, .Mode = OPAMP_PGA_MODE, .NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0, + #ifdef OPAMP_USE_EXTERNAL_CHANNEL + .InternalOutput = DISABLE, + #else .InternalOutput = ENABLE, + #endif .TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE, .PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0_BIAS, - .PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15, + .PgaGain = gain_setting, .UserTrimming = OPAMP_TRIMMING_FACTORY, }; if (HAL_OPAMP_Init(hopamp) != HAL_OK) { - SIMPLEFOC_DEBUG("HAL_OPAMP_Init failed!"); + SimpleFOCDebug::println("HAL_OPAMP_Init failed!"); } HAL_OPAMP_Start(hopamp); } @@ -96,7 +125,11 @@ int Phoque1_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) /** Configure Regular Channel (Opamp 1 / phase W current) */ - sConfig.Channel = ADC_CHANNEL_13; // OP1_OUT is ADC1_IN13 for internal channel + #ifdef OPAMP_USE_EXTERNAL_CHANNEL + sConfig.Channel = ADC_CHANNEL_3; + #else + sConfig.Channel = _OPAMP_internal_channel_to_ADC(1, ADC1); // OP1_OUT is ADC1_IN13 for internal channel + #endif sConfig.Rank = ADC_REGULAR_RANK_1; sConfig.SamplingTime = SAMPLETIME_BULB; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) @@ -116,9 +149,9 @@ int Phoque1_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) SimpleFOCDebug::printf(ADC_ConfigFail, 2); } - /* Configure Regular Channel (PC4 / BEMFV / Phase V) + /* Configure Regular Channel (PA2 / BEMFW / Phase W) */ - sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFV), ADC1); + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFW), ADC1); sConfig.Rank = ADC_REGULAR_RANK_3; sConfig.SamplingTime = SAMPLETIME_IMPORTANT; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) @@ -164,7 +197,11 @@ int Phoque1_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) /** Configure Regular Channel (Opamp 2 / phase U current) */ - sConfig.Channel = ADC_CHANNEL_16; // OP2_OUT is ADC2_IN16 for internal channel + #ifdef OPAMP_USE_EXTERNAL_CHANNEL + sConfig.Channel = ADC_CHANNEL_3; + #else + sConfig.Channel = _OPAMP_internal_channel_to_ADC(2, ADC2); // OP2_OUT is ADC2_IN16 for internal channel + #endif sConfig.Rank = ADC_REGULAR_RANK_1; sConfig.SamplingTime = SAMPLETIME_BULB; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) @@ -173,7 +210,11 @@ int Phoque1_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) } /** Configure Regular Channel (Opamp 3 / phase V current) */ - sConfig.Channel = ADC_CHANNEL_18; // OP3_OUT is ADC2_IN18 for internal channel + #ifdef OPAMP_USE_EXTERNAL_CHANNEL + sConfig.Channel = ADC_CHANNEL_12; + #else + sConfig.Channel = _OPAMP_internal_channel_to_ADC(3, ADC2); // OP3_OUT is ADC2_IN18 for internal channel + #endif sConfig.Rank = ADC_REGULAR_RANK_2; sConfig.SamplingTime = SAMPLETIME_IMPORTANT; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) @@ -183,9 +224,9 @@ int Phoque1_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) if(read_bemf) { - /** Configure Regular Channel (PA2 / BEMFW / Phase W) + /** Configure Regular Channel (PC4 / BEMFV / Phase V) */ - sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFW), ADC2); + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFV), ADC2); sConfig.Rank = ADC_REGULAR_RANK_3; sConfig.SamplingTime = SAMPLETIME_IMPORTANT; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) diff --git a/src/current_sense/phoque1/Phoque1_CurrentSense.hpp b/src/current_sense/phoque1/Phoque1_CurrentSense.hpp index 1ba1b6d..807e42b 100644 --- a/src/current_sense/phoque1/Phoque1_CurrentSense.hpp +++ b/src/current_sense/phoque1/Phoque1_CurrentSense.hpp @@ -3,9 +3,13 @@ #if defined(ARDUINO_PHOQUE1) #include "current_sense/phoque/Phoque_CurrentSense.hpp" +#include class Phoque1_CurrentSense : public Phoque_CurrentSense { +private: + uint8_t pga_gain = 32; + public: Phoque1_CurrentSense(float shunt_resistor, float gain, bool read_bemf=false); Phoque1_CurrentSense(float mVpA, bool read_bemf=false); @@ -13,9 +17,9 @@ class Phoque1_CurrentSense : public Phoque_CurrentSense virtual uint16_t readRaw(const int pin) const override; - static constexpr float compute_equivalent_shunt(float rshunt = 3e-3f, float rline = 1.5e3f, float rup = 2.2e3f, float rdown = 22e3f) + static constexpr float compute_equivalent_shunt(float rshunt = 3e-3f, float rline = 1.5e3f, float rup = 100e3f, float rdown = INFINITY) //On bg431-esc1, this is 3m, 1.5k, 22k, 2.2k { - float req1 = rup*rdown/(rup+rdown); + float req1 = isinff(rdown) ? rup : rup*rdown/(rup+rdown); return rshunt*req1/(rshunt+rline+req1); } diff --git a/src/current_sense/phoque2/Phoque2_CurrentSense.cpp b/src/current_sense/phoque2/Phoque2_CurrentSense.cpp new file mode 100644 index 0000000..6a137f2 --- /dev/null +++ b/src/current_sense/phoque2/Phoque2_CurrentSense.cpp @@ -0,0 +1,205 @@ +#if defined(ARDUINO_PHOQUE2) + +#include "Phoque2_CurrentSense.hpp" +#include "communication/SimpleFOCDebug.h" +#include "current_sense/hardware_specific/stm32/stm32_adc_utils.h" + + + +Phoque2_CurrentSense::Phoque2_CurrentSense(float _shunt_resistor, float _gain, bool _read_bemf) + :Phoque_CurrentSense(_shunt_resistor, _gain, _read_bemf) +{ + pinA = A_CURRU; + pinB = A_CURRV; + pinC = A_CURRW; +} + +Phoque2_CurrentSense::Phoque2_CurrentSense(float mVpA, bool _read_bemf) + :Phoque_CurrentSense(mVpA, _read_bemf) +{ + pinA = A_CURRU; + pinB = A_CURRV; + pinC = A_CURRW; +} + +Phoque2_CurrentSense::~Phoque2_CurrentSense() +{ +} + +#define SAMPLETIME_IMPORTANT ADC_SAMPLETIME_6CYCLES_5 +#define SAMPLETIME_PERIPHERAL ADC_SAMPLETIME_47CYCLES_5 + +int Phoque2_CurrentSense::get_adc1_important_duration() +{ + return (read_bemf ? 2:0) * get_conversion_duration(6) + 6; +} + +int Phoque2_CurrentSense::get_adc2_important_duration() +{ + return (read_bemf ? 2:1) * get_conversion_duration(6) + 6; +} + + +int Phoque2_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 (PA2 / phase U current) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_CURRU), ADC1); //ADC_CHANNEL_3; + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = SAMPLETIME_IMPORTANT; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); + } + + if (read_bemf) + { + /* Configure Regular Channel (PA1 / BEMFV / Phase V) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFV), ADC1); //ADC_CHANNEL_2; + sConfig.Rank = ADC_REGULAR_RANK_2; + sConfig.SamplingTime = SAMPLETIME_IMPORTANT; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); + } + + /* Configure Regular Channel (PA3 / BEMFW / Phase W) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFW), ADC1); //ADC_CHANNEL_4; + sConfig.Rank = ADC_REGULAR_RANK_3; + sConfig.SamplingTime = SAMPLETIME_IMPORTANT; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); + } + } + + //****************************************************************** + // Aux analog readings + /* Configure Regular Channel (PB12, mosfet temperature) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_TEMPERATURE), ADC1); //ADC_CHANNEL_11; + sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_4 : ADC_REGULAR_RANK_2; + sConfig.SamplingTime = SAMPLETIME_PERIPHERAL; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); + } + + /** Configure Regular Channel (PB0 / Bus voltage monitor) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_VBUS), ADC1); //ADC_CHANNEL_15; + sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_5 : ADC_REGULAR_RANK_3; + sConfig.SamplingTime = SAMPLETIME_PERIPHERAL; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); + } + return hadc1->Init.NbrOfConversion; +} + +int Phoque2_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 (PA7 / phase V current) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_CURRV), ADC2); //ADC_CHANNEL_4; + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = SAMPLETIME_IMPORTANT; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); + } + /** Configure Regular Channel (PC4 / phase W current) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_CURRW), ADC2); //ADC_CHANNEL_5; + sConfig.Rank = ADC_REGULAR_RANK_2; + sConfig.SamplingTime = SAMPLETIME_IMPORTANT; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); + } + + if(read_bemf) + { + /** Configure Regular Channel (PA1 / BEMFU / Phase U) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFU), ADC2); //ADC_CHANNEL_1; + sConfig.Rank = ADC_REGULAR_RANK_3; + sConfig.SamplingTime = SAMPLETIME_IMPORTANT; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); + } + } + + /** Configure Regular Channel (PB2 / Potentiometer) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_POTENTIOMETER), ADC2); //ADC_CHANNEL_12; + sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_4 : ADC_REGULAR_RANK_3; + sConfig.SamplingTime = SAMPLETIME_PERIPHERAL; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); + } + return hadc2->Init.NbrOfConversion; +} + +inline uint16_t Phoque2_CurrentSense::readRaw(const int pin) const +{ + switch (pin) + { + case A_CURRU: + case -1: + return adc1_buffer[0]; + case A_CURRV: + case -2: + return adc2_buffer[0]; + case A_CURRW: + case -3: + return adc2_buffer[1]; + + case A_BEMFU: + return adc2_buffer[2]; + case A_BEMFV: + return adc1_buffer[1]; + case A_BEMFW: + return adc1_buffer[2]; + + case A_POTENTIOMETER: + return adc2_buffer[2+read_bemf]; + case A_TEMPERATURE: + return adc1_buffer[1+read_bemf*2]; + case A_VBUS: + return adc1_buffer[2+read_bemf*2]; + default: + return 0; + } +} + +inline void Phoque2_CurrentSense::clear_currents() +{ + adc1_buffer[0] = UINT16_MAX; + adc2_buffer[0] = UINT16_MAX; + adc2_buffer[1] = UINT16_MAX; +} + +#endif \ No newline at end of file diff --git a/src/current_sense/phoque2/Phoque2_CurrentSense.hpp b/src/current_sense/phoque2/Phoque2_CurrentSense.hpp new file mode 100644 index 0000000..eb0eee4 --- /dev/null +++ b/src/current_sense/phoque2/Phoque2_CurrentSense.hpp @@ -0,0 +1,28 @@ +#pragma once + +#if defined(ARDUINO_PHOQUE2) + +#include + +class Phoque2_CurrentSense : public Phoque_CurrentSense +{ + //Current sense for the Phoque2 board + //for best performance, center modulation should be off +public: + Phoque2_CurrentSense(float shunt_resistor, float gain, bool read_bemf=false); + Phoque2_CurrentSense(float mVpA, bool read_bemf=false); + virtual ~Phoque2_CurrentSense(); + + virtual uint16_t readRaw(const int pin) const override; + +private: + + virtual int get_adc1_important_duration() override; + virtual int get_adc2_important_duration() override; + virtual int ADC1_Init(ADC_HandleTypeDef* hadc1) override; + virtual int ADC2_Init(ADC_HandleTypeDef* hadc2) override; + + void clear_currents() final; +}; + +#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 6d608a7..afc3281 100644 --- a/src/current_sense/phoque2a/Phoque2a_CurrentSense.cpp +++ b/src/current_sense/phoque2a/Phoque2a_CurrentSense.cpp @@ -58,7 +58,7 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.SamplingTime = SAMPLETIME_IMPORTANT; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); } if (read_bemf) @@ -70,7 +70,7 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.SamplingTime = SAMPLETIME_IMPORTANT; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); } /* Configure Regular Channel (PB1 / BEMFW / Phase W) @@ -80,7 +80,7 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.SamplingTime = SAMPLETIME_IMPORTANT; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); } } @@ -93,7 +93,7 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.SamplingTime = SAMPLETIME_PERIPHERAL; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); } /** Configure Regular Channel (PA3 / Bus voltage monitor) @@ -103,7 +103,7 @@ int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.SamplingTime = SAMPLETIME_PERIPHERAL; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); } return hadc1->Init.NbrOfConversion; } @@ -126,7 +126,7 @@ int Phoque2a_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.SamplingTime = SAMPLETIME_IMPORTANT; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { - SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); } /** Configure Regular Channel (PB2 / phase W current) */ @@ -135,7 +135,7 @@ int Phoque2a_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.SamplingTime = SAMPLETIME_IMPORTANT; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { - SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); } if(read_bemf) @@ -147,7 +147,7 @@ int Phoque2a_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.SamplingTime = SAMPLETIME_IMPORTANT; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { - SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); } } @@ -158,7 +158,7 @@ int Phoque2a_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.SamplingTime = SAMPLETIME_PERIPHERAL; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { - SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + SimpleFOCDebug::println("HAL_ADC_ConfigChannel failed!"); } return hadc2->Init.NbrOfConversion; }