Skip to content

Add Phoque1 current sense, lighten Phoque2 current sense code #91

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Moddingear wants to merge 1 commit into
base: dev
Choose a base branch
from
+251 −27
Open

Add Phoque1 current sense, lighten Phoque2 current sense code #91

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
219 changes: 219 additions & 0 deletions src/current_sense/phoque1/Phoque1_CurrentSense.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
26 changes: 26 additions & 0 deletions src/current_sense/phoque1/Phoque1_CurrentSense.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
33 changes: 6 additions & 27 deletions src/current_sense/phoque2a/Phoque2a_CurrentSense.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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!");
Expand All @@ -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!");
Expand All @@ -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!");
Expand All @@ -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!");
Expand All @@ -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!");
Expand All @@ -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;

Expand All @@ -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!");
Expand All @@ -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!");
Expand All @@ -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!");
Expand All @@ -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!");
Expand Down
Loading