Improve Phoque-series current sense code

src/current_sense/phoque/Phoque_CurrentSense.cpp

@@ -1,4 +1,4 @@
-#if defined(ARDUINO_PHOQUE)
+#if defined(STM32G4xx)
 
 #include "Phoque_CurrentSense.hpp"
 #include <communication/SimpleFOCDebug.h>
@@ -103,15 +103,15 @@ 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 
 	*/
 	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 <array>
 
+//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<T, N> fill_temperature_lut() {
     return a;
 }
 
+//bake the ADC readings into a look up table at compile time
 const auto temperature_lut = fill_temperature_lut<uint16_t, 1<<TEMP_LUT_SIZE>();
 
+//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

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:

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)

src/current_sense/phoque1/Phoque1_CurrentSense.hpp

@@ -3,19 +3,23 @@
 #if defined(ARDUINO_PHOQUE1)
 
 #include "current_sense/phoque/Phoque_CurrentSense.hpp"
+#include <stm32g4xx.h>
 
 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);
 	~Phoque1_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);
 	}