Skip to content

Commit c8002cc

Browse files
runger1101001runger1101001
authored
Merge pull request simplefoc#459 from simplefoc/feat_default_dir_only_no_sensor
use default dir only if sensor not connected
2 parents cad1bc5 + f182385 commit c8002cc

File tree

9 files changed

+651
-21
lines changed

9 files changed

+651
-21
lines changed

‎src/BLDCMotor.cpp‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -93,10 +93,13 @@ int BLDCMotor::init() {
9393
P_angle.limit = velocity_limit;
9494

9595
// if using open loop control, set a CW as the default direction if not already set
96-
if ((controller==MotionControlType::angle_openloop
97-
||controller==MotionControlType::velocity_openloop)
98-
&& (sensor_direction == Direction::UNKNOWN)) {
99-
sensor_direction = Direction::CW;
96+
// only if no sensor is used
97+
if(!sensor){
98+
if ((controller==MotionControlType::angle_openloop
99+
||controller==MotionControlType::velocity_openloop)
100+
&& (sensor_direction == Direction::UNKNOWN)) {
101+
sensor_direction = Direction::CW;
102+
}
100103
}
101104

102105
_delay(500);

‎src/HybridStepperMotor.cpp‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _induc
3030
void HybridStepperMotor::linkDriver(BLDCDriver *_driver)
3131
{
3232
driver = _driver;
33+
SIMPLEFOC_DEBUG("MOT: BLDCDriver linked, using pin C as the mid-phase");
3334
}
3435

3536
// init hardware pins
@@ -64,10 +65,13 @@ int HybridStepperMotor::init()
6465
P_angle.limit = velocity_limit;
6566

6667
// if using open loop control, set a CW as the default direction if not already set
67-
if ((controller==MotionControlType::angle_openloop
68-
||controller==MotionControlType::velocity_openloop)
69-
&& (sensor_direction == Direction::UNKNOWN)) {
70-
sensor_direction = Direction::CW;
68+
// only if no sensor is used
69+
if(!sensor){
70+
if ((controller==MotionControlType::angle_openloop
71+
||controller==MotionControlType::velocity_openloop)
72+
&& (sensor_direction == Direction::UNKNOWN)) {
73+
sensor_direction = Direction::CW;
74+
}
7175
}
7276

7377
_delay(500);

‎src/StepperMotor.cpp‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -57,10 +57,13 @@ int StepperMotor::init() {
5757
P_angle.limit = velocity_limit;
5858

5959
// if using open loop control, set a CW as the default direction if not already set
60-
if ((controller==MotionControlType::angle_openloop
61-
||controller==MotionControlType::velocity_openloop)
62-
&& (sensor_direction == Direction::UNKNOWN)) {
63-
sensor_direction = Direction::CW;
60+
// only if no sensor is used
61+
if(!sensor){
62+
if ((controller==MotionControlType::angle_openloop
63+
||controller==MotionControlType::velocity_openloop)
64+
&& (sensor_direction == Direction::UNKNOWN)) {
65+
sensor_direction = Direction::CW;
66+
}
6467
}
6568

6669
_delay(500);

‎src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp‎

Lines changed: 217 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,217 @@
1+
#include "stm32h7_hal.h"
2+
3+
#if defined(STM32H7xx)
4+
5+
//#define SIMPLEFOC_STM32_DEBUG
6+
7+
#include "../../../../communication/SimpleFOCDebug.h"
8+
#define _TRGO_NOT_AVAILABLE 12345
9+
10+
ADC_HandleTypeDef hadc;
11+
12+
/**
13+
* Function initializing the ADC and the injected channels for the low-side current sensing
14+
*
15+
* @param cs_params - current sense parameters
16+
* @param driver_params - driver parameters
17+
*
18+
* @return int - 0 if success
19+
*/
20+
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
21+
{
22+
ADC_InjectionConfTypeDef sConfigInjected;
23+
24+
// check if all pins belong to the same ADC
25+
ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr;
26+
ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr;
27+
ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr;
28+
if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) ||
29+
((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) ||
30+
((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3))
31+
){
32+
#ifdef SIMPLEFOC_STM32_DEBUG
33+
SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!");
34+
#endif
35+
return -1;
36+
}
37+
38+
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
39+
*/
40+
hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
41+
42+
if(hadc.Instance == ADC1) __HAL_RCC_ADC12_CLK_ENABLE();
43+
#ifdef ADC2 // if defined ADC2
44+
else if(hadc.Instance == ADC2) __HAL_RCC_ADC12_CLK_ENABLE();
45+
#endif
46+
#ifdef ADC3 // if defined ADC3
47+
else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE();
48+
#endif
49+
else{
50+
#ifdef SIMPLEFOC_STM32_DEBUG
51+
SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
52+
#endif
53+
return -1; // error not a valid ADC instance
54+
}
55+
56+
#ifdef SIMPLEFOC_STM32_DEBUG
57+
SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1);
58+
#endif
59+
60+
hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
61+
hadc.Init.Resolution = ADC_RESOLUTION_12B;
62+
hadc.Init.ScanConvMode = ENABLE;
63+
hadc.Init.ContinuousConvMode = DISABLE;
64+
hadc.Init.DiscontinuousConvMode = DISABLE;
65+
hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
66+
hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now
67+
#if defined(ADC_VER_V5_V90)
68+
// only for ADC3
69+
if(hadc.Instance == ADC3){
70+
hadc.Init.DataAlign = ADC3_DATAALIGN_RIGHT;
71+
}
72+
// more info here
73+
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L170C13-L170C27
74+
hadc.Init.DMAContinuousRequests = DISABLE;
75+
// not sure about this one!!! maybe use: ADC_SAMPLING_MODE_NORMAL
76+
hadc.Init.SamplingMode = ADC_SAMPLING_MODE_BULB;
77+
#endif
78+
hadc.Init.NbrOfConversion = 1;
79+
hadc.Init.NbrOfDiscConversion = 0;
80+
hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
81+
82+
hadc.Init.LowPowerAutoWait = DISABLE;
83+
84+
85+
if ( HAL_ADC_Init(&hadc) != HAL_OK){
86+
#ifdef SIMPLEFOC_STM32_DEBUG
87+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!");
88+
#endif
89+
return -1;
90+
}
91+
92+
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
93+
*/
94+
sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
95+
// if ADC1 or ADC2
96+
if(hadc.Instance == ADC1 || hadc.Instance == ADC2){
97+
// more info here: https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L658
98+
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLE_5;
99+
}else {
100+
// adc3
101+
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L673
102+
sConfigInjected.InjectedSamplingTime = ADC3_SAMPLETIME_2CYCLES_5;
103+
}
104+
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISINGFALLING;
105+
sConfigInjected.AutoInjectedConv = DISABLE;
106+
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
107+
sConfigInjected.InjectedOffset = 0;
108+
sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED;
109+
sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE;
110+
sConfigInjected.QueueInjectedContext = DISABLE;
111+
sConfigInjected.InjecOversamplingMode = DISABLE;
112+
113+
// automating TRGO flag finding - hardware specific
114+
uint8_t tim_num = 0;
115+
for (size_t i=0; i<6; i++) {
116+
TIM_HandleTypeDef *timer_to_check = driver_params->timers_handle[tim_num++];
117+
TIM_TypeDef *instance_to_check = timer_to_check->Instance;
118+
119+
uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check);
120+
if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
121+
122+
// check if TRGO used already - if yes use the next timer
123+
if((timer_to_check->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync
124+
(timer_to_check->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync
125+
{
126+
continue;
127+
}
128+
129+
// if the code comes here, it has found the timer available
130+
// timer does have trgo flag for injected channels
131+
sConfigInjected.ExternalTrigInjecConv = trigger_flag;
132+
133+
// this will be the timer with which the ADC will sync
134+
cs_params->timer_handle = timer_to_check;
135+
// done
136+
break;
137+
}
138+
if( cs_params->timer_handle == NP ){
139+
// not possible to use these timers for low-side current sense
140+
#ifdef SIMPLEFOC_STM32_DEBUG
141+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
142+
#endif
143+
return -1;
144+
}else{
145+
#ifdef SIMPLEFOC_STM32_DEBUG
146+
SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance));
147+
#endif
148+
}
149+
150+
uint32_t ranks[4] = {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4};
151+
for(int i=0; i<3; i++){
152+
// skip if not set
153+
if (!_isset(cs_params->pins[i])) continue;
154+
sConfigInjected.InjectedRank = ranks[i];
155+
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]));
156+
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
157+
#ifdef SIMPLEFOC_STM32_DEBUG
158+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) );
159+
#endif
160+
return -1;
161+
}
162+
}
163+
164+
165+
delay(1000);
166+
#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
167+
// enable interrupt
168+
HAL_NVIC_SetPriority(ADC_IRQn, 0, 0);
169+
HAL_NVIC_EnableIRQ(ADC_IRQn);
170+
#endif
171+
172+
cs_params->adc_handle = &hadc;
173+
return 0;
174+
}
175+
176+
177+
/**
178+
* Function to initialize the ADC GPIO pins
179+
*
180+
* @param cs_params current sense parameters
181+
* @param pinA pin number for phase A
182+
* @param pinB pin number for phase B
183+
* @param pinC pin number for phase C
184+
* @return int 0 if success, -1 if error
185+
*/
186+
int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
187+
{
188+
int pins[3] = {pinA, pinB, pinC};
189+
const char* port_names[3] = {"A", "B", "C"};
190+
for(int i=0; i<3; i++){
191+
if(_isset(pins[i])){
192+
// check if pin is an analog pin
193+
if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){
194+
#ifdef SIMPLEFOC_STM32_DEBUG
195+
SimpleFOCDebug::print("STM32-CS: ERR: Pin ");
196+
SimpleFOCDebug::print(port_names[i]);
197+
SimpleFOCDebug::println(" does not belong to any ADC!");
198+
#endif
199+
return -1;
200+
}
201+
pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC);
202+
cs_params->pins[i] = pins[i];
203+
}
204+
}
205+
return 0;
206+
}
207+
208+
#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
209+
extern "C" {
210+
void ADC_IRQHandler(void)
211+
{
212+
HAL_ADC_IRQHandler(&hadc);
213+
}
214+
}
215+
#endif
216+
217+
#endif

‎src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
#pragma once
2+
3+
#include "Arduino.h"
4+
5+
#if defined(STM32H7xx)
6+
#include "stm32h7xx_hal.h"
7+
#include "../stm32_mcu.h"
8+
#include "stm32h7_utils.h"
9+
10+
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params);
11+
int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
12+
13+
#endif

0 commit comments

Comments
 (0)