bldc/mcpwm_foc.c

2933 lines
78 KiB
C
Raw Normal View History

2015-12-08 12:01:23 -08:00
/*
Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se
2015-12-08 12:01:23 -08:00
2016-11-04 07:18:34 -07:00
This file is part of the VESC firmware.
The VESC firmware is free software: you can redistribute it and/or modify
2015-12-08 12:01:23 -08:00
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
2016-11-04 07:18:34 -07:00
The VESC firmware is distributed in the hope that it will be useful,
2015-12-08 12:01:23 -08:00
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
2016-11-04 07:18:34 -07:00
*/
2015-12-08 12:01:23 -08:00
2017-09-21 12:20:11 -07:00
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
2015-12-08 12:01:23 -08:00
#include "mcpwm_foc.h"
#include "mc_interface.h"
#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "digital_filter.h"
#include "utils.h"
#include "ledpwm.h"
#include "terminal.h"
#include "encoder.h"
#include "commands.h"
#include "timeout.h"
#include "timer.h"
2015-12-08 12:01:23 -08:00
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "virtual_motor.h"
2015-12-08 12:01:23 -08:00
// Private types
typedef struct {
float id_target;
float iq_target;
float max_duty;
float duty_now;
float phase;
float i_alpha;
float i_beta;
float i_abs;
float i_abs_filter;
2015-12-08 12:01:23 -08:00
float i_bus;
float v_bus;
float v_alpha;
float v_beta;
float mod_d;
float mod_q;
float id;
float iq;
float id_filter;
float iq_filter;
2015-12-08 12:01:23 -08:00
float vd;
float vq;
2016-11-04 07:18:34 -07:00
float vd_int;
float vq_int;
uint32_t svm_sector;
2015-12-08 12:01:23 -08:00
} motor_state_t;
typedef struct {
int sample_num;
float avg_current_tot;
float avg_voltage_tot;
bool measure_inductance_now;
float measure_inductance_duty;
} mc_sample_t;
// Private variables
static volatile mc_configuration *m_conf;
static volatile mc_state m_state;
static volatile mc_control_mode m_control_mode;
static volatile motor_state_t m_motor_state;
static volatile int m_curr0_sum;
static volatile int m_curr1_sum;
static volatile int m_curr_samples;
static volatile int m_curr0_offset;
static volatile int m_curr1_offset;
static volatile int m_curr_unbalance;
2015-12-08 12:01:23 -08:00
static volatile bool m_phase_override;
static volatile float m_phase_now_override;
static volatile float m_duty_cycle_set;
static volatile float m_id_set;
static volatile float m_iq_set;
2016-11-04 07:18:34 -07:00
static volatile float m_openloop_speed;
2019-02-18 10:30:19 -08:00
static volatile float m_openloop_phase;
2015-12-08 12:01:23 -08:00
static volatile bool m_dccal_done;
static volatile bool m_output_on;
static volatile float m_pos_pid_set;
2015-12-08 12:01:23 -08:00
static volatile float m_speed_pid_set_rpm;
static volatile float m_phase_now_observer;
static volatile float m_phase_now_observer_override;
static volatile bool m_phase_observer_override;
static volatile float m_phase_now_encoder;
static volatile float m_phase_now_encoder_no_index;
static volatile float m_observer_x1;
static volatile float m_observer_x2;
static volatile float m_pll_phase;
static volatile float m_pll_speed;
static volatile mc_sample_t m_samples;
static volatile int m_tachometer;
static volatile int m_tachometer_abs;
2019-02-18 10:30:19 -08:00
static volatile float m_last_adc_isr_duration;
static volatile float m_pos_pid_now;
2019-02-18 10:30:19 -08:00
static volatile bool m_init_done = false;
static volatile float m_gamma_now;
static volatile bool m_using_encoder;
2015-12-08 12:01:23 -08:00
#ifdef HW_HAS_3_SHUNTS
static volatile int m_curr2_sum;
static volatile int m_curr2_offset;
#endif
2015-12-08 12:01:23 -08:00
// Private functions
static void do_dc_cal(void);
void observer_update(float v_alpha, float v_beta, float i_alpha, float i_beta,
float dt, volatile float *x1, volatile float *x2, volatile float *phase);
static void pll_run(float phase, float dt, volatile float *phase_var,
volatile float *speed_var);
static void control_current(volatile motor_state_t *state_m, float dt);
static void svm(float alpha, float beta, uint32_t PWMHalfPeriod,
uint32_t* tAout, uint32_t* tBout, uint32_t* tCout, uint32_t *svm_sector);
static void run_pid_control_pos(float angle_now, float angle_set, float dt);
2015-12-08 12:01:23 -08:00
static void run_pid_control_speed(float dt);
static void stop_pwm_hw(void);
static void start_pwm_hw(void);
static int read_hall(void);
static float correct_encoder(float obs_angle, float enc_angle, float speed);
static float correct_hall(float angle, float speed, float dt);
2015-12-08 12:01:23 -08:00
// Threads
static THD_WORKING_AREA(timer_thread_wa, 2048);
static THD_FUNCTION(timer_thread, arg);
static volatile bool timer_thd_stop;
// Macros
#ifdef HW_HAS_3_SHUNTS
#define TIMER_UPDATE_DUTY(duty1, duty2, duty3) \
TIM1->CR1 |= TIM_CR1_UDIS; \
TIM1->CCR1 = duty1; \
TIM1->CCR2 = duty2; \
TIM1->CCR3 = duty3; \
TIM1->CR1 &= ~TIM_CR1_UDIS;
#else
2015-12-08 12:01:23 -08:00
#define TIMER_UPDATE_DUTY(duty1, duty2, duty3) \
TIM1->CR1 |= TIM_CR1_UDIS; \
TIM1->CCR1 = duty1; \
TIM1->CCR2 = duty3; \
TIM1->CCR3 = duty2; \
TIM1->CR1 &= ~TIM_CR1_UDIS;
#endif
2015-12-08 12:01:23 -08:00
2016-11-04 07:18:34 -07:00
#define TIMER_UPDATE_SAMP(samp) \
TIM8->CCR1 = samp;
2015-12-08 12:01:23 -08:00
2016-11-04 07:18:34 -07:00
#define TIMER_UPDATE_SAMP_TOP(samp, top) \
2015-12-08 12:01:23 -08:00
TIM1->CR1 |= TIM_CR1_UDIS; \
TIM8->CR1 |= TIM_CR1_UDIS; \
TIM1->ARR = top; \
2016-11-04 07:18:34 -07:00
TIM8->CCR1 = samp; \
2015-12-08 12:01:23 -08:00
TIM1->CR1 &= ~TIM_CR1_UDIS; \
TIM8->CR1 &= ~TIM_CR1_UDIS;
#ifdef HW_HAS_3_SHUNTS
2016-11-04 07:18:34 -07:00
#define TIMER_UPDATE_DUTY_SAMP(duty1, duty2, duty3, samp) \
TIM1->CR1 |= TIM_CR1_UDIS; \
TIM8->CR1 |= TIM_CR1_UDIS; \
TIM1->CCR1 = duty1; \
TIM1->CCR2 = duty2; \
TIM1->CCR3 = duty3; \
2016-11-04 07:18:34 -07:00
TIM8->CCR1 = samp; \
TIM1->CR1 &= ~TIM_CR1_UDIS; \
TIM8->CR1 &= ~TIM_CR1_UDIS;
#else
2016-11-04 07:18:34 -07:00
#define TIMER_UPDATE_DUTY_SAMP(duty1, duty2, duty3, samp) \
2015-12-08 12:01:23 -08:00
TIM1->CR1 |= TIM_CR1_UDIS; \
TIM8->CR1 |= TIM_CR1_UDIS; \
TIM1->CCR1 = duty1; \
TIM1->CCR2 = duty3; \
TIM1->CCR3 = duty2; \
2016-11-04 07:18:34 -07:00
TIM8->CCR1 = samp; \
2015-12-08 12:01:23 -08:00
TIM1->CR1 &= ~TIM_CR1_UDIS; \
TIM8->CR1 &= ~TIM_CR1_UDIS;
#endif
2015-12-08 12:01:23 -08:00
void mcpwm_foc_init(volatile mc_configuration *configuration) {
utils_sys_lock_cnt();
2016-11-04 07:18:34 -07:00
m_init_done = false;
2015-12-08 12:01:23 -08:00
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_BDTRInitTypeDef TIM_BDTRInitStructure;
m_conf = configuration;
// Initialize variables
m_state = MC_STATE_OFF;
m_control_mode = CONTROL_MODE_NONE;
m_curr0_sum = 0;
m_curr1_sum = 0;
m_curr_unbalance = 0.0;
2015-12-08 12:01:23 -08:00
m_curr_samples = 0;
m_dccal_done = false;
m_phase_override = false;
m_phase_now_override = 0.0;
m_duty_cycle_set = 0.0;
m_id_set = 0.0;
m_iq_set = 0.0;
2016-11-04 07:18:34 -07:00
m_openloop_speed = 0.0;
2019-02-18 10:30:19 -08:00
m_openloop_phase = 0.0;
2015-12-08 12:01:23 -08:00
m_output_on = false;
m_pos_pid_set = 0.0;
2015-12-08 12:01:23 -08:00
m_speed_pid_set_rpm = 0.0;
m_phase_now_observer = 0.0;
m_phase_now_observer_override = 0.0;
m_phase_observer_override = false;
m_phase_now_encoder = 0.0;
m_phase_now_encoder_no_index = 0.0;
m_observer_x1 = 0.0;
m_observer_x2 = 0.0;
m_pll_phase = 0.0;
m_pll_speed = 0.0;
m_tachometer = 0;
m_tachometer_abs = 0;
2019-02-18 10:30:19 -08:00
m_last_adc_isr_duration = 0;
m_pos_pid_now = 0.0;
m_gamma_now = 0.0;
m_using_encoder = false;
2015-12-08 12:01:23 -08:00
memset((void*)&m_motor_state, 0, sizeof(motor_state_t));
memset((void*)&m_samples, 0, sizeof(mc_sample_t));
virtual_motor_init();
#ifdef HW_HAS_3_SHUNTS
m_curr2_sum = 0;
#endif
2015-12-08 12:01:23 -08:00
TIM_DeInit(TIM1);
TIM_DeInit(TIM8);
TIM1->CNT = 0;
TIM8->CNT = 0;
// TIM1 clock enable
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
// Time Base configuration
TIM_TimeBaseStructure.TIM_Prescaler = 0;
2016-11-04 07:18:34 -07:00
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1;
2015-12-08 12:01:23 -08:00
TIM_TimeBaseStructure.TIM_Period = SYSTEM_CORE_CLOCK / (int)m_conf->foc_f_sw;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
2016-11-04 07:18:34 -07:00
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
2015-12-08 12:01:23 -08:00
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
// Channel 1, 2 and 3 Configuration in PWM mode
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
2015-12-08 12:01:23 -08:00
TIM_OCInitStructure.TIM_Pulse = TIM1->ARR / 2;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set;
TIM_OC1Init(TIM1, &TIM_OCInitStructure);
TIM_OC2Init(TIM1, &TIM_OCInitStructure);
TIM_OC3Init(TIM1, &TIM_OCInitStructure);
TIM_OC4Init(TIM1, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);
TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable);
TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);
2015-12-08 12:01:23 -08:00
// Automatic Output enable, Break, dead time and lock configuration
TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
2015-12-08 12:01:23 -08:00
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
TIM_BDTRInitStructure.TIM_DeadTime = conf_general_calculate_deadtime(HW_DEAD_TIME_NSEC, SYSTEM_CORE_CLOCK);
2015-12-08 12:01:23 -08:00
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
TIM_BDTRConfig(TIM1, &TIM_BDTRInitStructure);
TIM_CCPreloadControl(TIM1, ENABLE);
2015-12-08 12:01:23 -08:00
TIM_ARRPreloadConfig(TIM1, ENABLE);
/*
* ADC!
*/
ADC_CommonInitTypeDef ADC_CommonInitStructure;
DMA_InitTypeDef DMA_InitStructure;
ADC_InitTypeDef ADC_InitStructure;
// Clock
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | RCC_APB2Periph_ADC3, ENABLE);
2016-11-04 07:18:34 -07:00
dmaStreamAllocate(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)),
3,
(stm32_dmaisr_t)mcpwm_foc_adc_int_handler,
(void *)0);
2015-12-08 12:01:23 -08:00
// DMA for the ADC
DMA_InitStructure.DMA_Channel = DMA_Channel_0;
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&ADC_Value;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC->CDR;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
DMA_InitStructure.DMA_BufferSize = HW_ADC_CHANNELS;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(DMA2_Stream4, &DMA_InitStructure);
// DMA2_Stream0 enable
DMA_Cmd(DMA2_Stream4, ENABLE);
2016-11-04 07:18:34 -07:00
// Enable transfer complete interrupt
DMA_ITConfig(DMA2_Stream4, DMA_IT_TC, ENABLE);
2015-12-08 12:01:23 -08:00
// ADC Common Init
// Note that the ADC is running at 42MHz, which is higher than the
// specified 36MHz in the data sheet, but it works.
ADC_CommonInitStructure.ADC_Mode = ADC_TripleMode_RegSimult;
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2;
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1;
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
ADC_CommonInit(&ADC_CommonInitStructure);
// Channel-specific settings
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
ADC_InitStructure.ADC_ScanConvMode = ENABLE;
ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Falling;
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T8_CC1;
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
ADC_InitStructure.ADC_NbrOfConversion = HW_ADC_NBR_CONV;
ADC_Init(ADC1, &ADC_InitStructure);
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
ADC_InitStructure.ADC_ExternalTrigConv = 0;
ADC_Init(ADC2, &ADC_InitStructure);
ADC_Init(ADC3, &ADC_InitStructure);
// Enable Vrefint channel
ADC_TempSensorVrefintCmd(ENABLE);
// Enable DMA request after last transfer (Multi-ADC mode)
ADC_MultiModeDMARequestAfterLastTransferCmd(ENABLE);
hw_setup_adc_channels();
// Enable ADC1
ADC_Cmd(ADC1, ENABLE);
// Enable ADC2
ADC_Cmd(ADC2, ENABLE);
// Enable ADC3
ADC_Cmd(ADC3, ENABLE);
// ------------- Timer8 for ADC sampling ------------- //
// Time Base configuration
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
TIM_TimeBaseStructure.TIM_Prescaler = 0;
2016-11-04 07:18:34 -07:00
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
2015-12-08 12:01:23 -08:00
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
2016-11-04 07:18:34 -07:00
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
2015-12-08 12:01:23 -08:00
TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 500;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set;
TIM_OC1Init(TIM8, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
2015-12-08 12:01:23 -08:00
TIM_OC2Init(TIM8, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);
TIM_OC3Init(TIM8, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable);
2015-12-08 12:01:23 -08:00
TIM_ARRPreloadConfig(TIM8, ENABLE);
TIM_CCPreloadControl(TIM8, ENABLE);
2015-12-08 12:01:23 -08:00
// PWM outputs have to be enabled in order to trigger ADC on CCx
TIM_CtrlPWMOutputs(TIM8, ENABLE);
// TIM1 Master and TIM8 slave
TIM_SelectOutputTrigger(TIM1, TIM_TRGOSource_Update);
TIM_SelectMasterSlaveMode(TIM1, TIM_MasterSlaveMode_Enable);
TIM_SelectInputTrigger(TIM8, TIM_TS_ITR0);
TIM_SelectSlaveMode(TIM8, TIM_SlaveMode_Reset);
// Enable TIM1 and TIM8
TIM_Cmd(TIM1, ENABLE);
TIM_Cmd(TIM8, ENABLE);
// Main Output Enable
TIM_CtrlPWMOutputs(TIM1, ENABLE);
// ADC sampling locations
stop_pwm_hw();
// Sample intervals. For now they are fixed with voltage samples in the center of V7
// and current samples in the center of V0
2016-11-04 07:18:34 -07:00
TIMER_UPDATE_SAMP(MCPWM_FOC_CURRENT_SAMP_OFFSET);
2015-12-08 12:01:23 -08:00
// Enable CC1 interrupt, which will be fired in V0 and V7
TIM_ITConfig(TIM8, TIM_IT_CC1, ENABLE);
nvicEnableVector(TIM8_CC_IRQn, 6);
2015-12-08 12:01:23 -08:00
utils_sys_unlock_cnt();
2019-02-18 10:30:19 -08:00
CURRENT_FILTER_ON();
2015-12-08 12:01:23 -08:00
// Calibrate current offset
ENABLE_GATE();
DCCAL_OFF();
do_dc_cal();
// Start threads
timer_thd_stop = false;
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
// Check if the system has resumed from IWDG reset
2019-02-19 09:55:18 -08:00
if (timeout_had_IWDG_reset()) {
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET);
2019-02-19 09:55:18 -08:00
}
2016-11-04 07:18:34 -07:00
m_init_done = true;
2015-12-08 12:01:23 -08:00
}
void mcpwm_foc_deinit(void) {
2019-02-18 10:30:19 -08:00
if (!m_init_done) {
return;
}
2016-11-04 07:18:34 -07:00
m_init_done = false;
2015-12-08 12:01:23 -08:00
timer_thd_stop = true;
while (timer_thd_stop) {
chThdSleepMilliseconds(1);
}
TIM_DeInit(TIM1);
TIM_DeInit(TIM8);
ADC_DeInit();
DMA_DeInit(DMA2_Stream4);
nvicDisableVector(ADC_IRQn);
dmaStreamRelease(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)));
}
2016-11-04 07:18:34 -07:00
bool mcpwm_foc_init_done(void) {
return m_init_done;
}
2015-12-08 12:01:23 -08:00
void mcpwm_foc_set_configuration(volatile mc_configuration *configuration) {
m_conf = configuration;
m_control_mode = CONTROL_MODE_NONE;
m_state = MC_STATE_OFF;
stop_pwm_hw();
uint32_t top = SYSTEM_CORE_CLOCK / (int)m_conf->foc_f_sw;
2016-11-04 07:18:34 -07:00
TIMER_UPDATE_SAMP_TOP(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
2015-12-08 12:01:23 -08:00
}
mc_state mcpwm_foc_get_state(void) {
return m_state;
}
bool mcpwm_foc_is_dccal_done(void) {
return m_dccal_done;
}
/**
* Switch off all FETs.
2015-12-08 12:01:23 -08:00
*/
void mcpwm_foc_stop_pwm(void) {
mcpwm_foc_set_current(0.0);
}
/**
* Use duty cycle control. Absolute values less than MCPWM_MIN_DUTY_CYCLE will
* stop the motor.
*
* @param dutyCycle
* The duty cycle to use.
*/
void mcpwm_foc_set_duty(float dutyCycle) {
m_control_mode = CONTROL_MODE_DUTY;
m_duty_cycle_set = dutyCycle;
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
/**
* Use duty cycle control. Absolute values less than MCPWM_MIN_DUTY_CYCLE will
* stop the motor.
*
* WARNING: This function does not use ramping. A too large step with a large motor
* can destroy hardware.
*
* @param dutyCycle
* The duty cycle to use.
*/
void mcpwm_foc_set_duty_noramp(float dutyCycle) {
// TODO: Actually do this without ramping
mcpwm_foc_set_duty(dutyCycle);
}
/**
* Use PID rpm control. Note that this value has to be multiplied by half of
* the number of motor poles.
*
* @param rpm
* The electrical RPM goal value to use.
*/
void mcpwm_foc_set_pid_speed(float rpm) {
m_control_mode = CONTROL_MODE_SPEED;
m_speed_pid_set_rpm = rpm;
if (m_state != MC_STATE_RUNNING) {
2015-12-08 12:01:23 -08:00
m_state = MC_STATE_RUNNING;
}
}
/**
* Use PID position control. Note that this only works when encoder support
* is enabled.
*
* @param pos
* The desired position of the motor in degrees.
*/
void mcpwm_foc_set_pid_pos(float pos) {
m_control_mode = CONTROL_MODE_POS;
m_pos_pid_set = pos;
2015-12-08 12:01:23 -08:00
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
/**
* Use current control and specify a goal current to use. The sign determines
* the direction of the torque. Absolute values less than
* conf->cc_min_current will release the motor.
*
* @param current
* The current to use.
*/
void mcpwm_foc_set_current(float current) {
if (fabsf(current) < m_conf->cc_min_current) {
m_control_mode = CONTROL_MODE_NONE;
m_state = MC_STATE_OFF;
stop_pwm_hw();
return;
}
m_control_mode = CONTROL_MODE_CURRENT;
m_iq_set = current;
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
/**
* Brake the motor with a desired current. Absolute values less than
* conf->cc_min_current will release the motor.
*
* @param current
* The current to use. Positive and negative values give the same effect.
*/
void mcpwm_foc_set_brake_current(float current) {
if (fabsf(current) < m_conf->cc_min_current) {
m_control_mode = CONTROL_MODE_NONE;
m_state = MC_STATE_OFF;
stop_pwm_hw();
return;
}
m_control_mode = CONTROL_MODE_CURRENT_BRAKE;
m_iq_set = current;
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
2016-11-04 07:18:34 -07:00
/**
* Apply a fixed static current vector in open loop to emulate an electric
* handbrake.
*
* @param current
* The brake current to use.
*/
void mcpwm_foc_set_handbrake(float current) {
if (fabsf(current) < m_conf->cc_min_current) {
m_control_mode = CONTROL_MODE_NONE;
m_state = MC_STATE_OFF;
stop_pwm_hw();
return;
}
m_control_mode = CONTROL_MODE_HANDBRAKE;
m_iq_set = current;
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
/**
* Produce an openloop rotating current.
*
* @param current
* The current to use.
*
* @param rpm
* The RPM to use.
*/
void mcpwm_foc_set_openloop(float current, float rpm) {
if (fabsf(current) < m_conf->cc_min_current) {
m_control_mode = CONTROL_MODE_NONE;
m_state = MC_STATE_OFF;
stop_pwm_hw();
return;
}
2019-02-18 10:30:19 -08:00
utils_truncate_number(&current, -m_conf->l_current_max * m_conf->l_current_max_scale,
m_conf->l_current_max * m_conf->l_current_max_scale);
2016-11-04 07:18:34 -07:00
m_control_mode = CONTROL_MODE_OPENLOOP;
m_iq_set = current;
m_openloop_speed = rpm * ((2.0 * M_PI) / 60.0);
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
2019-02-18 10:30:19 -08:00
/**
* Produce an openloop current at a fixed phase.
*
* @param current
* The current to use.
*
* @param phase
* The phase to use in degrees, range [0.0 360.0]
*/
void mcpwm_foc_set_openloop_phase(float current, float phase) {
if (fabsf(current) < m_conf->cc_min_current) {
m_control_mode = CONTROL_MODE_NONE;
m_state = MC_STATE_OFF;
stop_pwm_hw();
return;
}
utils_truncate_number(&current, -m_conf->l_current_max * m_conf->l_current_max_scale,
m_conf->l_current_max * m_conf->l_current_max_scale);
m_control_mode = CONTROL_MODE_OPENLOOP_PHASE;
m_iq_set = current;
m_openloop_phase = phase * M_PI / 180.0;
utils_norm_angle_rad((float*)&m_openloop_phase);
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
/**
* Set current offsets values,
* this is used by the virtual motor to set the previously saved offsets back,
* when it is disconnected
*/
void mcpwm_foc_set_current_offsets(volatile int curr0_offset,
volatile int curr1_offset,
volatile int curr2_offset){
m_curr0_offset = curr0_offset;
m_curr1_offset = curr1_offset;
#ifdef HW_HAS_3_SHUNTS
m_curr2_offset = curr2_offset;
#else
(void)curr2_offset;
#endif
}
/**
* Produce an openloop rotating voltage.
*
* @param dutyCycle
* The duty cycle to use.
*
* @param rpm
* The RPM to use.
*/
void mcpwm_foc_set_openloop_duty(float dutyCycle, float rpm) {
m_control_mode = CONTROL_MODE_OPENLOOP_DUTY;
m_duty_cycle_set = dutyCycle;
m_openloop_speed = rpm * ((2.0 * M_PI) / 60.0);
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
/**
* Produce an openloop voltage at a fixed phase.
*
* @param dutyCycle
* The duty cycle to use.
*
* @param phase
* The phase to use in degrees, range [0.0 360.0]
*/
void mcpwm_foc_set_openloop_duty_phase(float dutyCycle, float phase) {
m_control_mode = CONTROL_MODE_OPENLOOP_DUTY_PHASE;
m_duty_cycle_set = dutyCycle;
m_openloop_phase = phase * M_PI / 180.0;
utils_norm_angle_rad((float*)&m_openloop_phase);
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
2015-12-08 12:01:23 -08:00
float mcpwm_foc_get_duty_cycle_set(void) {
return m_duty_cycle_set;
}
float mcpwm_foc_get_duty_cycle_now(void) {
return m_motor_state.duty_now;
}
float mcpwm_foc_get_pid_pos_set(void) {
return m_pos_pid_set;
}
float mcpwm_foc_get_pid_pos_now(void) {
return m_pos_pid_now;
}
2015-12-08 12:01:23 -08:00
/**
* Get the current switching frequency.
*
* @return
* The switching frequency in Hz.
*/
float mcpwm_foc_get_switching_frequency_now(void) {
return m_conf->foc_f_sw;
}
2016-11-04 07:18:34 -07:00
/**
* Get the current sampling frequency.
*
* @return
* The sampling frequency in Hz.
*/
float mcpwm_foc_get_sampling_frequency_now(void) {
#ifdef HW_HAS_PHASE_SHUNTS
if (m_conf->foc_sample_v0_v7) {
return m_conf->foc_f_sw;
} else {
return m_conf->foc_f_sw / 2.0;
}
2016-11-04 07:18:34 -07:00
#else
return m_conf->foc_f_sw / 2.0;
#endif
}
/**
* Returns Ts used for virtual motor sync
*/
float mcpwm_foc_get_ts(void){
#ifdef HW_HAS_PHASE_SHUNTS
if (m_conf->foc_sample_v0_v7) {
return (1.0 / m_conf->foc_f_sw) ;
} else {
return (1.0 / (m_conf->foc_f_sw / 2.0));
}
#else
return (1.0 / m_conf->foc_f_sw) ;
#endif
}
bool mcpwm_foc_is_using_encoder(void) {
return m_using_encoder;
}
2015-12-08 12:01:23 -08:00
/**
* Calculate the current RPM of the motor. This is a signed value and the sign
* depends on the direction the motor is rotating in. Note that this value has
* to be divided by half the number of motor poles.
*
* @return
* The RPM value.
*/
float mcpwm_foc_get_rpm(void) {
return m_pll_speed / ((2.0 * M_PI) / 60.0);
}
/**
* Get the motor current. The sign of this value will
* represent whether the motor is drawing (positive) or generating
* (negative) current. This is the q-axis current which produces torque.
*
* @return
* The motor current.
*/
float mcpwm_foc_get_tot_current(void) {
return SIGN(m_motor_state.vq) * m_motor_state.iq;
2015-12-08 12:01:23 -08:00
}
/**
* Get the filtered motor current. The sign of this value will
2015-12-08 12:01:23 -08:00
* represent whether the motor is drawing (positive) or generating
* (negative) current. This is the q-axis current which produces torque.
*
* @return
* The filtered motor current.
*/
float mcpwm_foc_get_tot_current_filtered(void) {
return SIGN(m_motor_state.vq) * m_motor_state.iq_filter;
2015-12-08 12:01:23 -08:00
}
/**
* Get the magnitude of the motor current, which includes both the
* D and Q axis.
*
* @return
* The magnitude of the motor current.
*/
float mcpwm_foc_get_abs_motor_current(void) {
return m_motor_state.i_abs;
}
/**
* Get the magnitude of the motor current unbalance
*
* @return
* The magnitude of the phase currents unbalance.
*/
float mcpwm_foc_get_abs_motor_current_unbalance(void) {
return (float)(m_curr_unbalance) * FAC_CURRENT;
}
/**
* Get the magnitude of the motor voltage.
*
* @return
* The magnitude of the motor voltage.
*/
float mcpwm_foc_get_abs_motor_voltage(void) {
const float vd_tmp = m_motor_state.vd;
const float vq_tmp = m_motor_state.vq;
return sqrtf(SQ(vd_tmp) + SQ(vq_tmp));
}
/**
* Get the filtered magnitude of the motor current, which includes both the
* D and Q axis.
*
* @return
* The magnitude of the motor current.
*/
float mcpwm_foc_get_abs_motor_current_filtered(void) {
return m_motor_state.i_abs_filter;
2015-12-08 12:01:23 -08:00
}
/**
* Get the motor current. The sign of this value represents the direction
* in which the motor generates torque.
*
* @return
* The motor current.
*/
float mcpwm_foc_get_tot_current_directional(void) {
return m_motor_state.iq;
2015-12-08 12:01:23 -08:00
}
/**
* Get the filtered motor current. The sign of this value represents the
* direction in which the motor generates torque.
*
* @return
* The filtered motor current.
*/
float mcpwm_foc_get_tot_current_directional_filtered(void) {
return m_motor_state.iq_filter;
2015-12-08 12:01:23 -08:00
}
2016-11-04 07:18:34 -07:00
/**
* Get the direct axis motor current.
*
* @return
* The D axis current.
*/
float mcpwm_foc_get_id(void) {
return m_motor_state.id;
}
/**
* Get the quadrature axis motor current.
*
* @return
* The Q axis current.
*/
float mcpwm_foc_get_iq(void) {
return m_motor_state.iq;
}
2015-12-08 12:01:23 -08:00
/**
* Get the input current to the motor controller.
*
* @return
* The input current.
*/
float mcpwm_foc_get_tot_current_in(void) {
return m_motor_state.i_bus;
2015-12-08 12:01:23 -08:00
}
/**
* Get the filtered input current to the motor controller.
2015-12-08 12:01:23 -08:00
*
* @return
* The filtered input current.
*/
float mcpwm_foc_get_tot_current_in_filtered(void) {
return m_motor_state.i_bus; // TODO: Calculate filtered current?
2015-12-08 12:01:23 -08:00
}
/**
* Read the number of steps the motor has rotated. This number is signed and
* will return a negative number when the motor is rotating backwards.
*
* @param reset
* If true, the tachometer counter will be reset after this call.
*
* @return
* The tachometer value in motor steps. The number of motor revolutions will
* be this number divided by (3 * MOTOR_POLE_NUMBER).
*/
int mcpwm_foc_get_tachometer_value(bool reset) {
int val = m_tachometer;
if (reset) {
m_tachometer = 0;
}
return val;
}
/**
* Read the absolute number of steps the motor has rotated.
*
* @param reset
* If true, the tachometer counter will be reset after this call.
*
* @return
* The tachometer value in motor steps. The number of motor revolutions will
* be this number divided by (3 * MOTOR_POLE_NUMBER).
*/
int mcpwm_foc_get_tachometer_abs_value(bool reset) {
int val = m_tachometer_abs;
if (reset) {
m_tachometer_abs = 0;
}
return val;
}
/**
* Read the motor phase.
*
* @return
* The phase angle in degrees.
*/
float mcpwm_foc_get_phase(void) {
float angle = m_motor_state.phase * (180.0 / M_PI);
utils_norm_angle(&angle);
return angle;
}
/**
* Read the phase that the observer has calculated.
*
* @return
* The phase angle in degrees.
*/
float mcpwm_foc_get_phase_observer(void) {
float angle = m_phase_now_observer * (180.0 / M_PI);
utils_norm_angle(&angle);
return angle;
}
/**
* Read the phase from based on the encoder.
*
* @return
* The phase angle in degrees.
*/
float mcpwm_foc_get_phase_encoder(void) {
float angle = m_phase_now_encoder * (180.0 / M_PI);
utils_norm_angle(&angle);
return angle;
}
float mcpwm_foc_get_vd(void) {
return m_motor_state.vd;
}
float mcpwm_foc_get_vq(void) {
return m_motor_state.vq;
}
/**
* Get current offsets,
* this is used by the virtual motor to save the current offsets,
* when it is connected
*/
void mcpwm_foc_get_current_offsets(volatile int *curr0_offset, volatile int *curr1_offset, volatile int *curr2_offset) {
*curr0_offset = m_curr0_offset;
*curr1_offset = m_curr1_offset;
#ifdef HW_HAS_3_SHUNTS
*curr2_offset = m_curr2_offset;
#else
*curr2_offset = 0;
#endif
}
2015-12-08 12:01:23 -08:00
/**
* Measure encoder offset and direction.
*
* @param current
* The locking open loop current for the motor.
*
* @param offset
* The detected offset.
*
* @param ratio
* The ratio between electrical and mechanical revolutions
*
* @param direction
* The detected direction.
*/
void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ratio, bool *inverted) {
mc_interface_lock();
2015-12-08 12:01:23 -08:00
m_phase_override = true;
m_id_set = current;
m_iq_set = 0.0;
m_control_mode = CONTROL_MODE_CURRENT;
m_state = MC_STATE_RUNNING;
// Disable timeout
systime_t tout = timeout_get_timeout_msec();
float tout_c = timeout_get_brake_current();
2016-11-04 07:18:34 -07:00
timeout_reset();
timeout_configure(600000, 0.0);
2015-12-08 12:01:23 -08:00
// Save configuration
float offset_old = m_conf->foc_encoder_offset;
float inverted_old = m_conf->foc_encoder_inverted;
float ratio_old = m_conf->foc_encoder_ratio;
m_conf->foc_encoder_offset = 0.0;
m_conf->foc_encoder_inverted = false;
m_conf->foc_encoder_ratio = 1.0;
// Find index
int cnt = 0;
while(!encoder_index_found()) {
for (float i = 0.0;i < 2.0 * M_PI;i += (2.0 * M_PI) / 500.0) {
m_phase_now_override = i;
chThdSleepMilliseconds(1);
}
cnt++;
if (cnt > 30) {
// Give up
break;
}
}
if (print) {
commands_printf("Index found");
}
2015-12-08 12:01:23 -08:00
// Rotate
for (float i = 0.0;i < 2.0 * M_PI;i += (2.0 * M_PI) / 500.0) {
m_phase_now_override = i;
chThdSleepMilliseconds(1);
}
if (print) {
commands_printf("Rotated for sync");
}
2015-12-08 12:01:23 -08:00
// Inverted and ratio
chThdSleepMilliseconds(1000);
const int it_rat = 20;
2015-12-08 12:01:23 -08:00
float s_sum = 0.0;
float c_sum = 0.0;
float first = m_phase_now_encoder;
for (int i = 0; i < it_rat; i++) {
2015-12-08 12:01:23 -08:00
float phase_old = m_phase_now_encoder;
float phase_ovr_tmp = m_phase_now_override;
for (float j = phase_ovr_tmp; j < phase_ovr_tmp + (2.0 / 3.0) * M_PI;
j += (2.0 * M_PI) / 500.0) {
m_phase_now_override = j;
chThdSleepMilliseconds(1);
}
utils_norm_angle_rad((float*)&m_phase_now_override);
chThdSleepMilliseconds(300);
float diff = utils_angle_difference_rad(m_phase_now_encoder, phase_old);
float s, c;
sincosf(diff, &s, &c);
s_sum += s;
c_sum += c;
if (print) {
commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
}
if (i > 3 && fabsf(utils_angle_difference_rad(m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
break;
}
}
first = m_phase_now_encoder;
for (int i = 0; i < it_rat; i++) {
float phase_old = m_phase_now_encoder;
float phase_ovr_tmp = m_phase_now_override;
for (float j = phase_ovr_tmp; j > phase_ovr_tmp - (2.0 / 3.0) * M_PI;
j -= (2.0 * M_PI) / 500.0) {
m_phase_now_override = j;
chThdSleepMilliseconds(1);
2015-12-08 12:01:23 -08:00
}
utils_norm_angle_rad((float*)&m_phase_now_override);
chThdSleepMilliseconds(300);
float diff = utils_angle_difference_rad(phase_old, m_phase_now_encoder);
2015-12-08 12:01:23 -08:00
float s, c;
sincosf(diff, &s, &c);
2015-12-08 12:01:23 -08:00
s_sum += s;
c_sum += c;
if (print) {
commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
}
if (i > 3 && fabsf(utils_angle_difference_rad(m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
break;
}
2015-12-08 12:01:23 -08:00
}
float diff = atan2f(s_sum, c_sum) * 180.0 / M_PI;
*inverted = diff < 0.0;
*ratio = roundf(((2.0 / 3.0) * 180.0) /
2015-12-08 12:01:23 -08:00
fabsf(diff));
m_conf->foc_encoder_inverted = *inverted;
m_conf->foc_encoder_ratio = *ratio;
if (print) {
commands_printf("Inversion and ratio detected");
}
2015-12-08 12:01:23 -08:00
// Rotate
for (float i = m_phase_now_override;i < 2.0 * M_PI;i += (2.0 * M_PI) / 500.0) {
m_phase_now_override = i;
chThdSleepMilliseconds(2);
}
if (print) {
commands_printf("Rotated for sync");
commands_printf("Enc: %.2f", (double)encoder_read_deg());
}
2015-12-08 12:01:23 -08:00
const int it_ofs = m_conf->foc_encoder_ratio * 3.0;
2015-12-08 12:01:23 -08:00
s_sum = 0.0;
c_sum = 0.0;
for (int i = 0;i < it_ofs;i++) {
2019-02-18 10:30:19 -08:00
float step = (2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
float override = (float)i * step;
while (m_phase_now_override != override) {
utils_step_towards((float*)&m_phase_now_override, override, step / 100.0);
chThdSleepMilliseconds(4);
}
chThdSleepMilliseconds(100);
float angle_diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
2015-12-08 12:01:23 -08:00
float s, c;
sincosf(angle_diff, &s, &c);
2015-12-08 12:01:23 -08:00
s_sum += s;
c_sum += c;
if (print) {
commands_printf("%.2f", (double)(angle_diff * 180.0 / M_PI));
}
2015-12-08 12:01:23 -08:00
}
for (int i = it_ofs;i > 0;i--) {
2019-02-18 10:30:19 -08:00
float step = (2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
float override = (float)i * step;
while (m_phase_now_override != override) {
utils_step_towards((float*)&m_phase_now_override, override, step / 100.0);
chThdSleepMilliseconds(4);
}
chThdSleepMilliseconds(100);
2015-12-08 12:01:23 -08:00
float angle_diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
2015-12-08 12:01:23 -08:00
float s, c;
sincosf(angle_diff, &s, &c);
2015-12-08 12:01:23 -08:00
s_sum += s;
c_sum += c;
if (print) {
commands_printf("%.2f", (double)(angle_diff * 180.0 / M_PI));
}
2015-12-08 12:01:23 -08:00
}
*offset = atan2f(s_sum, c_sum) * 180.0 / M_PI;
if (print) {
commands_printf("Avg: %.2f", (double)*offset);
}
2015-12-08 12:01:23 -08:00
utils_norm_angle(offset);
if (print) {
commands_printf("Offset detected");
}
2015-12-08 12:01:23 -08:00
m_id_set = 0.0;
m_iq_set = 0.0;
m_phase_override = false;
m_control_mode = CONTROL_MODE_NONE;
m_state = MC_STATE_OFF;
stop_pwm_hw();
// Restore configuration
m_conf->foc_encoder_inverted = inverted_old;
m_conf->foc_encoder_offset = offset_old;
m_conf->foc_encoder_ratio = ratio_old;
// Enable timeout
timeout_configure(tout, tout_c);
mc_interface_unlock();
2015-12-08 12:01:23 -08:00
}
/**
* Lock the motor with a current and sample the voiltage and current to
* calculate the motor resistance.
*
* @param current
* The locking current.
*
* @param samples
* The number of samples to take.
*
* @return
* The calculated motor resistance.
*/
float mcpwm_foc_measure_resistance(float current, int samples) {
mc_interface_lock();
m_phase_override = true;
m_phase_now_override = 0.0;
m_id_set = 0.0;
m_iq_set = current;
m_control_mode = CONTROL_MODE_CURRENT;
m_state = MC_STATE_RUNNING;
// Disable timeout
systime_t tout = timeout_get_timeout_msec();
float tout_c = timeout_get_brake_current();
2016-11-04 07:18:34 -07:00
timeout_reset();
2015-12-08 12:01:23 -08:00
timeout_configure(60000, 0.0);
// Wait for the current to rise and the motor to lock.
2016-11-04 07:18:34 -07:00
chThdSleepMilliseconds(500);
2015-12-08 12:01:23 -08:00
// Sample
m_samples.avg_current_tot = 0.0;
m_samples.avg_voltage_tot = 0.0;
m_samples.sample_num = 0;
int cnt = 0;
while (m_samples.sample_num < samples) {
chThdSleepMilliseconds(1);
cnt++;
// Timeout
if (cnt > 10000) {
break;
}
2019-02-18 10:30:19 -08:00
if (mc_interface_get_fault() != FAULT_CODE_NONE) {
m_id_set = 0.0;
m_iq_set = 0.0;
m_phase_override = false;
m_control_mode = CONTROL_MODE_NONE;
m_state = MC_STATE_OFF;
stop_pwm_hw();
timeout_configure(tout, tout_c);
mc_interface_unlock();
return 0.0;
}
2015-12-08 12:01:23 -08:00
}
const float current_avg = m_samples.avg_current_tot / (float)m_samples.sample_num;
const float voltage_avg = m_samples.avg_voltage_tot / (float)m_samples.sample_num;
// Stop
m_id_set = 0.0;
m_iq_set = 0.0;
m_phase_override = false;
m_control_mode = CONTROL_MODE_NONE;
m_state = MC_STATE_OFF;
stop_pwm_hw();
// Enable timeout
timeout_configure(tout, tout_c);
mc_interface_unlock();
return (voltage_avg / current_avg) * (2.0 / 3.0);
}
2015-12-08 12:01:23 -08:00
/**
* Measure the motor inductance with short voltage pulses.
*
* @param duty
* The duty cycle to use in the pulses.
*
* @param samples
* The number of samples to average over.
*
* @param
* The current that was used for this measurement.
*
* @return
* The average d and q axis inductance in microhenry.
*/
float mcpwm_foc_measure_inductance(float duty, int samples, float *curr) {
m_samples.avg_current_tot = 0.0;
m_samples.avg_voltage_tot = 0.0;
m_samples.sample_num = 0;
m_samples.measure_inductance_duty = duty;
// Disable timeout
systime_t tout = timeout_get_timeout_msec();
float tout_c = timeout_get_brake_current();
2016-11-04 07:18:34 -07:00
timeout_reset();
2015-12-08 12:01:23 -08:00
timeout_configure(60000, 0.0);
mc_interface_lock();
2019-02-18 10:30:19 -08:00
CURRENT_FILTER_OFF();
2015-12-08 12:01:23 -08:00
int to_cnt = 0;
for (int i = 0;i < samples;i++) {
m_samples.measure_inductance_now = true;
2016-11-04 07:18:34 -07:00
do {
chThdSleepMicroseconds(100);
2015-12-08 12:01:23 -08:00
to_cnt++;
2016-11-04 07:18:34 -07:00
if (to_cnt > 50000) {
2015-12-08 12:01:23 -08:00
break;
}
2016-11-04 07:18:34 -07:00
} while (m_samples.measure_inductance_now);
2015-12-08 12:01:23 -08:00
2016-11-04 07:18:34 -07:00
if (to_cnt > 50000) {
2015-12-08 12:01:23 -08:00
break;
}
}
2019-02-18 10:30:19 -08:00
CURRENT_FILTER_ON();
2015-12-08 12:01:23 -08:00
// Enable timeout
timeout_configure(tout, tout_c);
mc_interface_unlock();
float avg_current = m_samples.avg_current_tot / (float)m_samples.sample_num;
float avg_voltage = m_samples.avg_voltage_tot / (float)m_samples.sample_num;
float t = (float)TIM1->ARR * m_samples.measure_inductance_duty / (float)SYSTEM_CORE_CLOCK -
(float)(MCPWM_FOC_INDUCTANCE_SAMPLE_CNT_OFFSET + MCPWM_FOC_INDUCTANCE_SAMPLE_RISE_COMP) / (float)SYSTEM_CORE_CLOCK;
if (curr) {
*curr = avg_current;
}
return ((avg_voltage * t) / avg_current) * 1e6 * (2.0 / 3.0);
}
2019-02-18 10:30:19 -08:00
/**
* Measure the motor inductance with short voltage pulses. The difference from the
* other function is that this one will aim for a specific measurement current. It
* will also use an appropriate switching frequency.
*
* @param curr_goal
* The measurement current to aim for.
*
* @param samples
* The number of samples to average over.
*
* @param *curr
* The current that was used for this measurement.
*
* @return
* The average d and q axis inductance in microhenry.
*/
float mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *curr) {
const float f_sw_old = m_conf->foc_f_sw;
m_conf->foc_f_sw = 3000.0;
uint32_t top = SYSTEM_CORE_CLOCK / (int)m_conf->foc_f_sw;
TIMER_UPDATE_SAMP_TOP(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
float duty_last = 0.0;
for (float i = 0.02;i < 0.5;i *= 1.5) {
float i_tmp;
mcpwm_foc_measure_inductance(i, 10, &i_tmp);
duty_last = i;
if (i_tmp >= curr_goal) {
break;
}
}
float ind = mcpwm_foc_measure_inductance(duty_last, samples, curr);
m_conf->foc_f_sw = f_sw_old;
top = SYSTEM_CORE_CLOCK / (int)m_conf->foc_f_sw;
TIMER_UPDATE_SAMP_TOP(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
return ind;
}
2015-12-08 12:01:23 -08:00
/**
* Automatically measure the resistance and inductance of the motor with small steps.
*
* @param res
* The measured resistance in ohm.
*
* @param ind
* The measured inductance in microhenry.
*
* @return
* True if the measurement succeeded, false otherwise.
*/
bool mcpwm_foc_measure_res_ind(float *res, float *ind) {
const float f_sw_old = m_conf->foc_f_sw;
const float kp_old = m_conf->foc_current_kp;
const float ki_old = m_conf->foc_current_ki;
m_conf->foc_f_sw = 10000.0;
2019-02-18 10:30:19 -08:00
m_conf->foc_current_kp = 0.001;
m_conf->foc_current_ki = 1.0;
2015-12-08 12:01:23 -08:00
uint32_t top = SYSTEM_CORE_CLOCK / (int)m_conf->foc_f_sw;
2016-11-04 07:18:34 -07:00
TIMER_UPDATE_SAMP_TOP(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
2015-12-08 12:01:23 -08:00
float i_last = 0.0;
for (float i = 2.0;i < (m_conf->l_current_max / 2.0);i *= 1.5) {
2019-02-18 10:30:19 -08:00
if (i > (1.0 / mcpwm_foc_measure_resistance(i, 20))) {
2015-12-08 12:01:23 -08:00
i_last = i;
break;
}
}
2019-02-19 09:55:18 -08:00
if (i_last < 0.01) {
i_last = (m_conf->l_current_max / 2.0);
}
2015-12-08 12:01:23 -08:00
#ifdef HW_AXIOM_FORCE_HIGH_CURRENT_MEASUREMENTS
i_last = (m_conf->l_current_max / 2.0);
#endif
2016-11-04 07:18:34 -07:00
*res = mcpwm_foc_measure_resistance(i_last, 200);
2019-02-18 10:30:19 -08:00
*ind = mcpwm_foc_measure_inductance_current(i_last, 200, 0);
2015-12-08 12:01:23 -08:00
m_conf->foc_f_sw = f_sw_old;
m_conf->foc_current_kp = kp_old;
m_conf->foc_current_ki = ki_old;
top = SYSTEM_CORE_CLOCK / (int)m_conf->foc_f_sw;
2016-11-04 07:18:34 -07:00
TIMER_UPDATE_SAMP_TOP(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
2015-12-08 12:01:23 -08:00
return true;
}
/**
* Run the motor in open loop and figure out at which angles the hall sensors are.
*
* @param current
* Current to use.
*
* @param hall_table
* Table to store the result to.
*
* @return
* true: Success
* false: Something went wrong
*/
bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
mc_interface_lock();
m_phase_override = true;
m_id_set = current;
m_iq_set = 0.0;
m_control_mode = CONTROL_MODE_CURRENT;
m_state = MC_STATE_RUNNING;
// Disable timeout
systime_t tout = timeout_get_timeout_msec();
float tout_c = timeout_get_brake_current();
2016-11-04 07:18:34 -07:00
timeout_reset();
timeout_configure(60000, 0.0);
// Lock the motor
m_phase_now_override = 0;
chThdSleepMilliseconds(1000);
float sin_hall[8];
float cos_hall[8];
int hall_iterations[8];
memset(sin_hall, 0, sizeof(sin_hall));
memset(cos_hall, 0, sizeof(cos_hall));
memset(hall_iterations, 0, sizeof(hall_iterations));
// Forwards
for (int i = 0;i < 3;i++) {
for (int j = 0;j < 360;j++) {
m_phase_now_override = (float)j * M_PI / 180.0;
chThdSleepMilliseconds(5);
int hall = read_hall();
float s, c;
sincosf(m_phase_now_override, &s, &c);
sin_hall[hall] += s;
cos_hall[hall] += c;
hall_iterations[hall]++;
}
}
// Reverse
for (int i = 0;i < 3;i++) {
for (int j = 360;j >= 0;j--) {
m_phase_now_override = (float)j * M_PI / 180.0;
chThdSleepMilliseconds(5);
int hall = read_hall();
float s, c;
sincosf(m_phase_now_override, &s, &c);
sin_hall[hall] += s;
cos_hall[hall] += c;
hall_iterations[hall]++;
}
}
m_id_set = 0.0;
m_iq_set = 0.0;
m_phase_override = false;
m_control_mode = CONTROL_MODE_NONE;
m_state = MC_STATE_OFF;
stop_pwm_hw();
// Enable timeout
timeout_configure(tout, tout_c);
int fails = 0;
for(int i = 0;i < 8;i++) {
if (hall_iterations[i] > 30) {
float ang = atan2f(sin_hall[i], cos_hall[i]) * 180.0 / M_PI;
utils_norm_angle(&ang);
hall_table[i] = (uint8_t)(ang * 200.0 / 360.0);
} else {
hall_table[i] = 255;
fails++;
}
}
mc_interface_unlock();
return fails == 2;
}
2015-12-08 12:01:23 -08:00
void mcpwm_foc_print_state(void) {
commands_printf("Mod d: %.2f", (double)m_motor_state.mod_d);
commands_printf("Mod q: %.2f", (double)m_motor_state.mod_q);
commands_printf("Duty: %.2f", (double)m_motor_state.duty_now);
commands_printf("Vd: %.2f", (double)m_motor_state.vd);
commands_printf("Vq: %.2f", (double)m_motor_state.vq);
commands_printf("Phase: %.2f", (double)m_motor_state.phase);
commands_printf("V_alpha: %.2f", (double)m_motor_state.v_alpha);
commands_printf("V_beta: %.2f", (double)m_motor_state.v_beta);
commands_printf("id: %.2f", (double)m_motor_state.id);
commands_printf("iq: %.2f", (double)m_motor_state.iq);
commands_printf("id_filter: %.2f", (double)m_motor_state.id_filter);
commands_printf("iq_filter: %.2f", (double)m_motor_state.iq_filter);
commands_printf("id_target: %.2f", (double)m_motor_state.id_target);
commands_printf("iq_target: %.2f", (double)m_motor_state.iq_target);
commands_printf("i_abs: %.2f", (double)m_motor_state.i_abs);
commands_printf("i_abs_filter: %.2f", (double)m_motor_state.i_abs_filter);
commands_printf("Obs_x1: %.2f", (double)m_observer_x1);
commands_printf("Obs_x2: %.2f", (double)m_observer_x2);
}
2019-02-18 10:30:19 -08:00
float mcpwm_foc_get_last_adc_isr_duration(void) {
return m_last_adc_isr_duration;
2015-12-08 12:01:23 -08:00
}
void mcpwm_foc_tim_sample_int_handler(void) {
if (m_init_done) {
// Generate COM event here for synchronization
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
virtual_motor_int_handler(m_motor_state.v_alpha, m_motor_state.v_beta);
}
}
2016-11-04 07:18:34 -07:00
void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
(void)p;
(void)flags;
static int skip = 0;
2019-02-19 09:55:18 -08:00
if (++skip == FOC_CONTROL_LOOP_FREQ_DIVIDER) {
skip = 0;
2019-02-19 09:55:18 -08:00
} else {
return;
2019-02-19 09:55:18 -08:00
}
uint32_t t_start = timer_time_now();
bool is_v7 = !(TIM1->CR1 & TIM_CR1_DIR);
if (!m_samples.measure_inductance_now) {
#ifdef HW_HAS_PHASE_SHUNTS
if (!m_conf->foc_sample_v0_v7 && is_v7) {
return;
}
#else
if (is_v7) {
return;
}
#endif
}
2015-12-08 12:01:23 -08:00
// Reset the watchdog
timeout_feed_WDT(THREAD_MCPWM);
2015-12-08 12:01:23 -08:00
2019-02-18 10:30:19 -08:00
#ifdef AD2S1205_SAMPLE_GPIO
// force a position sample in the AD2S1205 resolver IC (falling edge)
palClearPad(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN);
#endif
int curr0 = GET_CURRENT1();
int curr1 = GET_CURRENT2();
#ifdef HW_HAS_3_SHUNTS
2019-02-18 10:30:19 -08:00
int curr2 = GET_CURRENT3();
#endif
2015-12-08 12:01:23 -08:00
m_curr0_sum += curr0;
m_curr1_sum += curr1;
#ifdef HW_HAS_3_SHUNTS
m_curr2_sum += curr2;
#endif
2015-12-08 12:01:23 -08:00
curr0 -= m_curr0_offset;
curr1 -= m_curr1_offset;
#ifdef HW_HAS_3_SHUNTS
curr2 -= m_curr2_offset;
m_curr_unbalance = curr0 + curr1 + curr2;
#endif
m_curr_samples++;
2015-12-08 12:01:23 -08:00
ADC_curr_norm_value[0] = curr0;
ADC_curr_norm_value[1] = curr1;
#ifdef HW_HAS_3_SHUNTS
ADC_curr_norm_value[2] = curr2;
#else
2015-12-08 12:01:23 -08:00
ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
#endif
2016-11-04 07:18:34 -07:00
// Use the best current samples depending on the modulation state.
#ifdef HW_HAS_3_SHUNTS
if (m_conf->foc_sample_high_current) {
// High current sampling mode. Choose the lower currents to derive the highest one
// in order to be able to measure higher currents.
const float i0_abs = fabsf(ADC_curr_norm_value[0]);
const float i1_abs = fabsf(ADC_curr_norm_value[1]);
const float i2_abs = fabsf(ADC_curr_norm_value[2]);
if (i0_abs > i1_abs && i0_abs > i2_abs) {
ADC_curr_norm_value[0] = -(ADC_curr_norm_value[1] + ADC_curr_norm_value[2]);
} else if (i1_abs > i0_abs && i1_abs > i2_abs) {
ADC_curr_norm_value[1] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[2]);
} else if (i2_abs > i0_abs && i2_abs > i1_abs) {
ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
}
} else {
#ifdef HW_HAS_PHASE_SHUNTS
if (m_conf->foc_sample_v0_v7 && is_v7) {
if (TIM1->CCR1 < TIM1->CCR2 && TIM1->CCR1 < TIM1->CCR3) {
ADC_curr_norm_value[0] = -(ADC_curr_norm_value[1] + ADC_curr_norm_value[2]);
} else if (TIM1->CCR2 < TIM1->CCR1 && TIM1->CCR2 < TIM1->CCR3) {
ADC_curr_norm_value[1] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[2]);
} else if (TIM1->CCR3 < TIM1->CCR1 && TIM1->CCR3 < TIM1->CCR2) {
ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
}
} else {
if (TIM1->CCR1 > TIM1->CCR2 && TIM1->CCR1 > TIM1->CCR3) {
ADC_curr_norm_value[0] = -(ADC_curr_norm_value[1] + ADC_curr_norm_value[2]);
} else if (TIM1->CCR2 > TIM1->CCR1 && TIM1->CCR2 > TIM1->CCR3) {
ADC_curr_norm_value[1] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[2]);
} else if (TIM1->CCR3 > TIM1->CCR1 && TIM1->CCR3 > TIM1->CCR2) {
ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
}
}
#else
if (TIM1->CCR1 > TIM1->CCR2 && TIM1->CCR1 > TIM1->CCR3) {
ADC_curr_norm_value[0] = -(ADC_curr_norm_value[1] + ADC_curr_norm_value[2]);
} else if (TIM1->CCR2 > TIM1->CCR1 && TIM1->CCR2 > TIM1->CCR3) {
ADC_curr_norm_value[1] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[2]);
} else if (TIM1->CCR3 > TIM1->CCR1 && TIM1->CCR3 > TIM1->CCR2) {
ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
}
#endif
}
#endif
float ia = ADC_curr_norm_value[0] * FAC_CURRENT;
float ib = ADC_curr_norm_value[1] * FAC_CURRENT;
2016-11-04 07:18:34 -07:00
// float ic = -(ia + ib);
2015-12-08 12:01:23 -08:00
if (m_samples.measure_inductance_now) {
if (!is_v7) {
2016-11-04 07:18:34 -07:00
return;
}
2015-12-08 12:01:23 -08:00
static int inductance_state = 0;
const uint32_t duty_cnt = (uint32_t)((float)TIM1->ARR * m_samples.measure_inductance_duty);
const uint32_t samp_time = duty_cnt - MCPWM_FOC_INDUCTANCE_SAMPLE_CNT_OFFSET;
if (inductance_state == 0) {
2016-11-04 07:18:34 -07:00
TIMER_UPDATE_DUTY_SAMP(0, 0, 0, samp_time);
2015-12-08 12:01:23 -08:00
start_pwm_hw();
} else if (inductance_state == 2) {
TIMER_UPDATE_DUTY(duty_cnt, 0, duty_cnt);
} else if (inductance_state == 3) {
m_samples.avg_current_tot += -((float)curr1 * FAC_CURRENT);
2015-12-08 12:01:23 -08:00
m_samples.avg_voltage_tot += GET_INPUT_VOLTAGE();
m_samples.sample_num++;
TIMER_UPDATE_DUTY(0, 0, 0);
} else if (inductance_state == 5) {
TIMER_UPDATE_DUTY(0, duty_cnt, duty_cnt);
} else if (inductance_state == 6) {
m_samples.avg_current_tot += -((float)curr0 * FAC_CURRENT);
2015-12-08 12:01:23 -08:00
m_samples.avg_voltage_tot += GET_INPUT_VOLTAGE();
m_samples.sample_num++;
TIMER_UPDATE_DUTY(0, 0, 0);
} else if (inductance_state == 8) {
#ifdef HW_HAS_3_SHUNTS
TIMER_UPDATE_DUTY(duty_cnt, duty_cnt, 0);
#else
2015-12-08 12:01:23 -08:00
TIMER_UPDATE_DUTY(0, 0, duty_cnt);
#endif
2015-12-08 12:01:23 -08:00
} else if (inductance_state == 9) {
#ifdef HW_HAS_3_SHUNTS
m_samples.avg_current_tot += -((float)curr2 * FAC_CURRENT);
#else
m_samples.avg_current_tot += -((float)curr0 * FAC_CURRENT + (float)curr1 * FAC_CURRENT);
#endif
2015-12-08 12:01:23 -08:00
m_samples.avg_voltage_tot += GET_INPUT_VOLTAGE();
m_samples.sample_num++;
stop_pwm_hw();
2016-11-04 07:18:34 -07:00
TIMER_UPDATE_SAMP(MCPWM_FOC_CURRENT_SAMP_OFFSET);
} else if (inductance_state == 10) {
2015-12-08 12:01:23 -08:00
inductance_state = 0;
m_samples.measure_inductance_now = false;
return;
}
inductance_state++;
return;
}
#ifdef HW_HAS_PHASE_SHUNTS
float dt;
if (m_conf->foc_sample_v0_v7) {
dt = 1.0 / m_conf->foc_f_sw;
} else {
dt = 1.0 / (m_conf->foc_f_sw / 2.0);
}
2016-11-04 07:18:34 -07:00
#else
2019-02-19 09:55:18 -08:00
float dt = 1.0 / (m_conf->foc_f_sw / 2.0);
2016-11-04 07:18:34 -07:00
#endif
2019-02-19 09:55:18 -08:00
// This has to be done for the skip function to have any chance at working with the
// observer and control loops.
// TODO: Test this.
dt /= (float)FOC_CONTROL_LOOP_FREQ_DIVIDER;
UTILS_LP_FAST(m_motor_state.v_bus, GET_INPUT_VOLTAGE(), 0.1);
2015-12-08 12:01:23 -08:00
float enc_ang = 0;
if (encoder_is_configured()) {
if(virtual_motor_is_connected()){
enc_ang = virtual_motor_get_angle_deg();
}else{
enc_ang = encoder_read_deg();
}
float phase_tmp = enc_ang;
if (m_conf->foc_encoder_inverted) {
phase_tmp = 360.0 - phase_tmp;
}
phase_tmp *= m_conf->foc_encoder_ratio;
phase_tmp -= m_conf->foc_encoder_offset;
utils_norm_angle((float*)&phase_tmp);
m_phase_now_encoder = phase_tmp * (M_PI / 180.0);
2015-12-08 12:01:23 -08:00
}
static float phase_before = 0.0;
const float phase_diff = utils_angle_difference_rad(m_motor_state.phase, phase_before);
phase_before = m_motor_state.phase;
if (m_state == MC_STATE_RUNNING) {
// Clarke transform assuming balanced currents
m_motor_state.i_alpha = ia;
m_motor_state.i_beta = ONE_BY_SQRT3 * ia + TWO_BY_SQRT3 * ib;
// Full Clarke transform in case there are current offsets
2016-11-04 07:18:34 -07:00
// m_motor_state.i_alpha = (2.0 / 3.0) * ia - (1.0 / 3.0) * ib - (1.0 / 3.0) * ic;
// m_motor_state.i_beta = ONE_BY_SQRT3 * ib - ONE_BY_SQRT3 * ic;
2015-12-08 12:01:23 -08:00
const float duty_abs = fabsf(m_motor_state.duty_now);
float id_set_tmp = m_id_set;
float iq_set_tmp = m_iq_set;
m_motor_state.max_duty = m_conf->l_max_duty;
static float duty_filtered = 0.0;
UTILS_LP_FAST(duty_filtered, m_motor_state.duty_now, 0.1);
2015-12-08 12:01:23 -08:00
utils_truncate_number(&duty_filtered, -1.0, 1.0);
float duty_set = m_duty_cycle_set;
bool control_duty = m_control_mode == CONTROL_MODE_DUTY ||
m_control_mode == CONTROL_MODE_OPENLOOP_DUTY ||
m_control_mode == CONTROL_MODE_OPENLOOP_DUTY_PHASE;
2015-12-08 12:01:23 -08:00
2019-02-18 10:30:19 -08:00
// When the modulation is low in brake mode and the set brake current
// cannot be reached, short all phases to get more braking without
// applying active braking. Use a bit of hysteresis when leaving
// the shorted mode.
static bool was_full_brake = false;
if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE &&
fabsf(duty_filtered) < m_conf->l_min_duty * 1.5 &&
(m_motor_state.i_abs * (was_full_brake ? 1.0 : 1.5)) < fabsf(m_iq_set)) {
2015-12-08 12:01:23 -08:00
control_duty = true;
duty_set = 0.0;
2019-02-18 10:30:19 -08:00
was_full_brake = true;
} else {
was_full_brake = false;
2015-12-08 12:01:23 -08:00
}
// Brake when set ERPM is below min ERPM
if (m_control_mode == CONTROL_MODE_SPEED &&
fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) {
control_duty = true;
duty_set = 0.0;
}
2015-12-08 12:01:23 -08:00
if (control_duty) {
// Duty cycle control
static float duty_i_term = 0.0;
if (fabsf(duty_set) < (duty_abs - 0.05) ||
(SIGN(m_motor_state.vq) * m_motor_state.iq) < m_conf->lo_current_min) {
2015-12-08 12:01:23 -08:00
// Truncating the duty cycle here would be dangerous, so run a PID controller.
// Compensation for supply voltage variations
float scale = 1.0 / GET_INPUT_VOLTAGE();
// Compute error
float error = duty_set - m_motor_state.duty_now;
// Compute parameters
float p_term = error * m_conf->foc_duty_dowmramp_kp * scale;
duty_i_term += error * (m_conf->foc_duty_dowmramp_ki * dt) * scale;
// I-term wind-up protection
utils_truncate_number(&duty_i_term, -1.0, 1.0);
// Calculate output
float output = p_term + duty_i_term;
utils_truncate_number(&output, -1.0, 1.0);
iq_set_tmp = output * m_conf->lo_current_max;
} else {
// If the duty cycle is less than or equal to the set duty cycle just limit
// the modulation and use the maximum allowed current.
duty_i_term = 0.0;
m_motor_state.max_duty = duty_set;
if (duty_set > 0.0) {
iq_set_tmp = m_conf->lo_current_max;
} else {
iq_set_tmp = -m_conf->lo_current_max;
}
}
} else if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE) {
// Braking
iq_set_tmp = fabsf(iq_set_tmp);
if (phase_diff > 0.0) {
iq_set_tmp = -iq_set_tmp;
} else if (phase_diff == 0.0) {
iq_set_tmp = 0.0;
}
}
// Run observer
if (!m_phase_override) {
observer_update(m_motor_state.v_alpha, m_motor_state.v_beta,
m_motor_state.i_alpha, m_motor_state.i_beta, dt,
&m_observer_x1, &m_observer_x2, &m_phase_now_observer);
}
switch (m_conf->foc_sensor_mode) {
case FOC_SENSOR_MODE_ENCODER:
if (encoder_index_found()) {
m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
2015-12-08 12:01:23 -08:00
} else {
// Rotate the motor in open loop if the index isn't found.
2015-12-08 12:01:23 -08:00
m_motor_state.phase = m_phase_now_encoder_no_index;
}
if (!m_phase_override) {
id_set_tmp = 0.0;
2015-12-08 12:01:23 -08:00
}
break;
case FOC_SENSOR_MODE_HALL:
m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
m_motor_state.phase = m_phase_now_observer;
if (!m_phase_override) {
id_set_tmp = 0.0;
}
break;
2015-12-08 12:01:23 -08:00
case FOC_SENSOR_MODE_SENSORLESS:
if (m_phase_observer_override) {
m_motor_state.phase = m_phase_now_observer_override;
2015-12-08 12:01:23 -08:00
} else {
m_motor_state.phase = m_phase_now_observer;
2015-12-08 12:01:23 -08:00
}
// Inject D axis current at low speed to make the observer track
// better. This does not seem to be necessary with dead time
// compensation.
2015-12-08 12:01:23 -08:00
// Note: this is done at high rate prevent noise.
if (!m_phase_override) {
if (duty_abs < m_conf->foc_sl_d_current_duty) {
id_set_tmp = utils_map(duty_abs, 0.0, m_conf->foc_sl_d_current_duty,
2015-12-08 12:01:23 -08:00
fabsf(m_motor_state.iq_target) * m_conf->foc_sl_d_current_factor, 0.0);
} else {
id_set_tmp = 0.0;
2015-12-08 12:01:23 -08:00
}
}
break;
}
2016-11-04 07:18:34 -07:00
if (m_control_mode == CONTROL_MODE_HANDBRAKE) {
2019-02-18 10:30:19 -08:00
// Force the phase to 0 in handbrake mode so that the current simply locks the rotor.
2016-11-04 07:18:34 -07:00
m_motor_state.phase = 0.0;
} else if (m_control_mode == CONTROL_MODE_OPENLOOP ||
m_control_mode == CONTROL_MODE_OPENLOOP_DUTY) {
2016-11-04 07:18:34 -07:00
static float openloop_angle = 0.0;
openloop_angle += dt * m_openloop_speed;
utils_norm_angle_rad(&openloop_angle);
m_motor_state.phase = openloop_angle;
} else if (m_control_mode == CONTROL_MODE_OPENLOOP_PHASE ||
m_control_mode == CONTROL_MODE_OPENLOOP_DUTY_PHASE) {
2019-02-18 10:30:19 -08:00
m_motor_state.phase = m_openloop_phase;
2016-11-04 07:18:34 -07:00
}
2015-12-08 12:01:23 -08:00
if (m_phase_override) {
m_motor_state.phase = m_phase_now_override;
}
// Apply current limits
// TODO: Consider D axis current for the input current as well.
const float mod_q = m_motor_state.mod_q;
if (mod_q > 0.001) {
utils_truncate_number(&iq_set_tmp, m_conf->lo_in_current_min / mod_q, m_conf->lo_in_current_max / mod_q);
} else if (mod_q < -0.001) {
utils_truncate_number(&iq_set_tmp, m_conf->lo_in_current_max / mod_q, m_conf->lo_in_current_min / mod_q);
}
2015-12-08 12:01:23 -08:00
2016-11-04 07:18:34 -07:00
if (mod_q > 0.0) {
utils_truncate_number(&iq_set_tmp, m_conf->lo_current_min, m_conf->lo_current_max);
} else {
utils_truncate_number(&iq_set_tmp, -m_conf->lo_current_max, -m_conf->lo_current_min);
}
utils_saturate_vector_2d(&id_set_tmp, &iq_set_tmp,
utils_max_abs(m_conf->lo_current_max, m_conf->lo_current_min));
2016-11-04 07:18:34 -07:00
m_motor_state.id_target = id_set_tmp;
m_motor_state.iq_target = iq_set_tmp;
control_current(&m_motor_state, dt);
2015-12-08 12:01:23 -08:00
} else {
// Track back emf
#ifdef HW_HAS_3_SHUNTS
float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
float Vb = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2);
float Vc = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2);
#else
2015-12-08 12:01:23 -08:00
float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
float Vb = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2);
float Vc = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2);
#endif
2015-12-08 12:01:23 -08:00
// Full Clarke transform (no balanced voltages)
2015-12-08 12:01:23 -08:00
m_motor_state.v_alpha = (2.0 / 3.0) * Va - (1.0 / 3.0) * Vb - (1.0 / 3.0) * Vc;
m_motor_state.v_beta = ONE_BY_SQRT3 * Vb - ONE_BY_SQRT3 * Vc;
float c, s;
2015-12-08 12:01:23 -08:00
utils_fast_sincos_better(m_motor_state.phase, &s, &c);
#ifdef HW_USE_LINE_TO_LINE
// rotate alpha-beta 30 degrees to compensate for line-to-line phase voltage sensing
float x_tmp = m_motor_state.v_alpha;
float y_tmp = m_motor_state.v_beta;
2019-02-19 09:55:18 -08:00
m_motor_state.v_alpha = x_tmp * COS_MINUS_30_DEG - y_tmp * SIN_MINUS_30_DEG;
m_motor_state.v_beta = x_tmp * SIN_MINUS_30_DEG + y_tmp * COS_MINUS_30_DEG;
// compensate voltage amplitude
m_motor_state.v_alpha *= ONE_BY_SQRT3;
m_motor_state.v_beta *= ONE_BY_SQRT3;
#endif
// Park transform
float vd_tmp = c * m_motor_state.v_alpha + s * m_motor_state.v_beta;
float vq_tmp = c * m_motor_state.v_beta - s * m_motor_state.v_alpha;
UTILS_NAN_ZERO(m_motor_state.vd);
UTILS_NAN_ZERO(m_motor_state.vq);
UTILS_LP_FAST(m_motor_state.vd, vd_tmp, 0.2);
UTILS_LP_FAST(m_motor_state.vq, vq_tmp, 0.2);
2016-11-04 07:18:34 -07:00
m_motor_state.vd_int = m_motor_state.vd;
m_motor_state.vq_int = m_motor_state.vq;
2015-12-08 12:01:23 -08:00
// Update corresponding modulation
m_motor_state.mod_d = m_motor_state.vd / ((2.0 / 3.0) * m_motor_state.v_bus);
m_motor_state.mod_q = m_motor_state.vq / ((2.0 / 3.0) * m_motor_state.v_bus);
// The current is 0 when the motor is undriven
m_motor_state.i_alpha = 0.0;
m_motor_state.i_beta = 0.0;
m_motor_state.id = 0.0;
m_motor_state.iq = 0.0;
m_motor_state.id_filter = 0.0;
m_motor_state.iq_filter = 0.0;
m_motor_state.i_bus = 0.0;
m_motor_state.i_abs = 0.0;
m_motor_state.i_abs_filter = 0.0;
2015-12-08 12:01:23 -08:00
// Run observer
observer_update(m_motor_state.v_alpha, m_motor_state.v_beta,
m_motor_state.i_alpha, m_motor_state.i_beta, dt, &m_observer_x1,
&m_observer_x2, &m_phase_now_observer);
switch (m_conf->foc_sensor_mode) {
case FOC_SENSOR_MODE_ENCODER:
m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
break;
case FOC_SENSOR_MODE_HALL:
m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
m_motor_state.phase = m_phase_now_observer;
break;
case FOC_SENSOR_MODE_SENSORLESS:
m_motor_state.phase = m_phase_now_observer;
break;
2015-12-08 12:01:23 -08:00
}
}
// Calculate duty cycle
m_motor_state.duty_now = SIGN(m_motor_state.vq) *
sqrtf(m_motor_state.mod_d * m_motor_state.mod_d +
m_motor_state.mod_q * m_motor_state.mod_q) / SQRT3_BY_2;
// Run PLL for speed estimation
pll_run(m_motor_state.phase, dt, &m_pll_phase, &m_pll_speed);
// Update tachometer (resolution = 60 deg as for BLDC)
2016-11-04 07:18:34 -07:00
float ph_tmp = m_motor_state.phase;
utils_norm_angle_rad(&ph_tmp);
int step = (int)floorf((ph_tmp + M_PI) / (2.0 * M_PI) * 6.0);
utils_truncate_number_int(&step, 0, 5);
static int step_last = 0;
int diff = step - step_last;
step_last = step;
if (diff > 3) {
diff -= 6;
} else if (diff < -2) {
diff += 6;
2015-12-08 12:01:23 -08:00
}
2016-11-04 07:18:34 -07:00
m_tachometer += diff;
m_tachometer_abs += abs(diff);
// Track position control angle
// TODO: Have another look at this.
float angle_now = 0.0;
if (encoder_is_configured()) {
angle_now = enc_ang;
} else {
angle_now = m_motor_state.phase * (180.0 / M_PI);
}
if (m_conf->p_pid_ang_div > 0.98 && m_conf->p_pid_ang_div < 1.02) {
m_pos_pid_now = angle_now;
} else {
static float angle_last = 0.0;
float diff_f = utils_angle_difference(angle_now, angle_last);
angle_last = angle_now;
m_pos_pid_now += diff_f / m_conf->p_pid_ang_div;
utils_norm_angle((float*)&m_pos_pid_now);
}
// Run position control
if (m_state == MC_STATE_RUNNING) {
run_pid_control_pos(m_pos_pid_now, m_pos_pid_set, dt);
}
2019-02-18 10:30:19 -08:00
#ifdef AD2S1205_SAMPLE_GPIO
// Release sample in the AD2S1205 resolver IC.
palSetPad(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN);
#endif
2015-12-08 12:01:23 -08:00
// MCIF handler
mc_interface_mc_timer_isr();
m_last_adc_isr_duration = timer_seconds_elapsed_since(t_start);
2015-12-08 12:01:23 -08:00
}
// Private functions
static THD_FUNCTION(timer_thread, arg) {
(void)arg;
chRegSetThreadName("mcpwm_foc timer");
for(;;) {
if (timer_thd_stop) {
timer_thd_stop = false;
return;
}
float openloop_rpm = utils_map(fabsf(m_motor_state.iq_target),
0.0, m_conf->l_current_max,
0.0, m_conf->foc_openloop_rpm);
utils_truncate_number_abs(&openloop_rpm, m_conf->foc_openloop_rpm);
2015-12-08 12:01:23 -08:00
const float dt = 0.001;
const float min_rads = (openloop_rpm * 2.0 * M_PI) / 60.0;
2015-12-08 12:01:23 -08:00
static float min_rpm_hyst_timer = 0.0;
static float min_rpm_timer = 0.0;
float add_min_speed = 0.0;
if (m_motor_state.duty_now > 0.0) {
add_min_speed = min_rads * dt;
} else {
add_min_speed = -min_rads * dt;
}
// Open loop encoder angle for when the index is not found
m_phase_now_encoder_no_index += add_min_speed;
utils_norm_angle_rad((float*)&m_phase_now_encoder_no_index);
// Output a minimum speed from the observer
if (fabsf(m_pll_speed) < min_rads) {
min_rpm_hyst_timer += dt;
} else if (min_rpm_hyst_timer > 0.0) {
min_rpm_hyst_timer -= dt;
}
// Don't use this in brake mode.
if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE || fabsf(m_motor_state.duty_now) < 0.001) {
2015-12-08 12:01:23 -08:00
min_rpm_hyst_timer = 0.0;
m_phase_observer_override = false;
}
bool started_now = false;
2015-12-08 12:01:23 -08:00
if (min_rpm_hyst_timer > m_conf->foc_sl_openloop_hyst && min_rpm_timer <= 0.0001) {
min_rpm_timer = m_conf->foc_sl_openloop_time;
started_now = true;
2015-12-08 12:01:23 -08:00
}
if (min_rpm_timer > 0.0) {
m_phase_now_observer_override += add_min_speed;
// When the motor gets stuck it tends to be 90 degrees off, so start the open loop
// sequence by correcting with 90 degrees.
if (started_now) {
if (m_motor_state.duty_now > 0.0) {
m_phase_now_observer_override += M_PI / 2.0;
} else {
m_phase_now_observer_override -= M_PI / 2.0;
}
}
2015-12-08 12:01:23 -08:00
utils_norm_angle_rad((float*)&m_phase_now_observer_override);
m_phase_observer_override = true;
min_rpm_timer -= dt;
min_rpm_hyst_timer = 0.0;
} else {
m_phase_now_observer_override = m_phase_now_observer;
m_phase_observer_override = false;
}
// Samples
if (m_state == MC_STATE_RUNNING) {
const volatile float vd_tmp = m_motor_state.vd;
const volatile float vq_tmp = m_motor_state.vq;
const volatile float id_tmp = m_motor_state.id;
const volatile float iq_tmp = m_motor_state.iq;
m_samples.avg_current_tot += sqrtf(SQ(id_tmp) + SQ(iq_tmp));
m_samples.avg_voltage_tot += sqrtf(SQ(vd_tmp) + SQ(vq_tmp));
2015-12-08 12:01:23 -08:00
m_samples.sample_num++;
}
// Update and the observer gain.
m_gamma_now = utils_map(fabsf(m_motor_state.duty_now), 0.0, 1.0,
m_conf->foc_observer_gain * m_conf->foc_observer_gain_slow, m_conf->foc_observer_gain);
2015-12-08 12:01:23 -08:00
run_pid_control_speed(dt);
chThdSleepMilliseconds(1);
}
}
static void do_dc_cal(void) {
DCCAL_ON();
// Wait max 5 seconds
int cnt = 0;
while(IS_DRV_FAULT()){
chThdSleepMilliseconds(1);
cnt++;
if (cnt > 5000) {
break;
}
};
2015-12-08 12:01:23 -08:00
chThdSleepMilliseconds(1000);
m_curr0_sum = 0;
m_curr1_sum = 0;
#ifdef HW_HAS_3_SHUNTS
m_curr2_sum = 0;
#endif
2015-12-08 12:01:23 -08:00
m_curr_samples = 0;
while(m_curr_samples < 4000) {};
m_curr0_offset = m_curr0_sum / m_curr_samples;
m_curr1_offset = m_curr1_sum / m_curr_samples;
#ifdef HW_HAS_3_SHUNTS
m_curr2_offset = m_curr2_sum / m_curr_samples;
#endif
2015-12-08 12:01:23 -08:00
DCCAL_OFF();
m_dccal_done = true;
}
// See http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
void observer_update(float v_alpha, float v_beta, float i_alpha, float i_beta,
float dt, volatile float *x1, volatile float *x2, volatile float *phase) {
const float L = (3.0 / 2.0) * m_conf->foc_motor_l;
const float lambda = m_conf->foc_motor_flux_linkage;
float R = (3.0 / 2.0) * m_conf->foc_motor_r;
2015-12-08 12:01:23 -08:00
// Saturation compensation
const float sign = (m_motor_state.iq * m_motor_state.vq) >= 0.0 ? 1.0 : -1.0;
R -= R * sign * m_conf->foc_sat_comp * (m_motor_state.i_abs_filter / m_conf->l_current_max);
2015-12-08 12:01:23 -08:00
// Temperature compensation
const float t = mc_interface_temp_motor_filtered();
if (m_conf->foc_temp_comp && t > -5.0) {
R += R * 0.00386 * (t - m_conf->foc_temp_comp_base_temp);
}
2015-12-08 12:01:23 -08:00
const float L_ia = L * i_alpha;
const float L_ib = L * i_beta;
const float R_ia = R * i_alpha;
const float R_ib = R * i_beta;
const float lambda_2 = SQ(lambda);
const float gamma_half = m_gamma_now * 0.5;
// Original
// float err = lambda_2 - (SQ(*x1 - L_ia) + SQ(*x2 - L_ib));
// float x1_dot = -R_ia + v_alpha + gamma_half * (*x1 - L_ia) * err;
// float x2_dot = -R_ib + v_beta + gamma_half * (*x2 - L_ib) * err;
// *x1 += x1_dot * dt;
// *x2 += x2_dot * dt;
2015-12-08 12:01:23 -08:00
// Iterative with some trial and error
const int iterations = 6;
const float dt_iteration = dt / (float)iterations;
for (int i = 0;i < iterations;i++) {
float err = lambda_2 - (SQ(*x1 - L_ia) + SQ(*x2 - L_ib));
float gamma_tmp = gamma_half;
if (utils_truncate_number_abs(&err, lambda_2 * 0.2)) {
gamma_tmp *= 10.0;
}
float x1_dot = -R_ia + v_alpha + gamma_tmp * (*x1 - L_ia) * err;
float x2_dot = -R_ib + v_beta + gamma_tmp * (*x2 - L_ib) * err;
*x1 += x1_dot * dt_iteration;
*x2 += x2_dot * dt_iteration;
}
// Same as above, but without iterations.
// float err = lambda_2 - (SQ(*x1 - L_ia) + SQ(*x2 - L_ib));
// float gamma_tmp = gamma_half;
// if (utils_truncate_number_abs(&err, lambda_2 * 0.2)) {
// gamma_tmp *= 10.0;
// }
// float x1_dot = -R_ia + v_alpha + gamma_tmp * (*x1 - L_ia) * err;
// float x2_dot = -R_ib + v_beta + gamma_tmp * (*x2 - L_ib) * err;
// *x1 += x1_dot * dt;
// *x2 += x2_dot * dt;
UTILS_NAN_ZERO(*x1);
UTILS_NAN_ZERO(*x2);
*phase = utils_fast_atan2(*x2 - L_ib, *x1 - L_ia);
2015-12-08 12:01:23 -08:00
}
static void pll_run(float phase, float dt, volatile float *phase_var,
volatile float *speed_var) {
UTILS_NAN_ZERO(*phase_var);
2015-12-08 12:01:23 -08:00
float delta_theta = phase - *phase_var;
utils_norm_angle_rad(&delta_theta);
UTILS_NAN_ZERO(*speed_var);
2015-12-08 12:01:23 -08:00
*phase_var += (*speed_var + m_conf->foc_pll_kp * delta_theta) * dt;
utils_norm_angle_rad((float*)phase_var);
*speed_var += m_conf->foc_pll_ki * delta_theta * dt;
}
/**
* Run the current control loop.
*
* @param state_m
* The motor state.
*
* Parameters that shall be set before calling this function:
* id_target
* iq_target
* max_duty
* phase
* i_alpha
* i_beta
* v_bus
*
* Parameters that will be updated in this function:
* i_bus
* i_abs
* i_abs_filter
2015-12-08 12:01:23 -08:00
* v_alpha
* v_beta
* mod_d
* mod_q
* id
* iq
* id_filter
* iq_filter
2015-12-08 12:01:23 -08:00
* vd
* vq
2016-11-04 07:18:34 -07:00
* vd_int
* vq_int
* svm_sector
2015-12-08 12:01:23 -08:00
*
* @param dt
* The time step in seconds.
*/
static void control_current(volatile motor_state_t *state_m, float dt) {
float c,s;
utils_fast_sincos_better(state_m->phase, &s, &c);
2016-11-04 07:18:34 -07:00
float max_duty = fabsf(state_m->max_duty);
utils_truncate_number(&max_duty, 0.0, m_conf->l_max_duty);
2015-12-08 12:01:23 -08:00
state_m->id = c * state_m->i_alpha + s * state_m->i_beta;
state_m->iq = c * state_m->i_beta - s * state_m->i_alpha;
UTILS_LP_FAST(state_m->id_filter, state_m->id, m_conf->foc_current_filter_const);
UTILS_LP_FAST(state_m->iq_filter, state_m->iq, m_conf->foc_current_filter_const);
2015-12-08 12:01:23 -08:00
float Ierr_d = state_m->id_target - state_m->id;
float Ierr_q = state_m->iq_target - state_m->iq;
2016-11-04 07:18:34 -07:00
state_m->vd = state_m->vd_int + Ierr_d * m_conf->foc_current_kp;
state_m->vq = state_m->vq_int + Ierr_q * m_conf->foc_current_kp;
// Temperature compensation
const float t = mc_interface_temp_motor_filtered();
float ki = m_conf->foc_current_ki;
if (m_conf->foc_temp_comp && t > -5.0) {
ki += ki * 0.00386 * (t - m_conf->foc_temp_comp_base_temp);
}
state_m->vd_int += Ierr_d * (ki * dt);
state_m->vq_int += Ierr_q * (ki * dt);
2015-12-08 12:01:23 -08:00
2016-11-04 07:18:34 -07:00
// Saturation
2015-12-08 12:01:23 -08:00
utils_saturate_vector_2d((float*)&state_m->vd, (float*)&state_m->vq,
(2.0 / 3.0) * max_duty * SQRT3_BY_2 * state_m->v_bus);
2016-11-04 07:18:34 -07:00
state_m->mod_d = state_m->vd / ((2.0 / 3.0) * state_m->v_bus);
state_m->mod_q = state_m->vq / ((2.0 / 3.0) * state_m->v_bus);
// Windup protection
// utils_saturate_vector_2d((float*)&state_m->vd_int, (float*)&state_m->vq_int,
// (2.0 / 3.0) * max_duty * SQRT3_BY_2 * state_m->v_bus);
utils_truncate_number_abs((float*)&state_m->vd_int, (2.0 / 3.0) * max_duty * SQRT3_BY_2 * state_m->v_bus);
utils_truncate_number_abs((float*)&state_m->vq_int, (2.0 / 3.0) * max_duty * SQRT3_BY_2 * state_m->v_bus);
// TODO: Have a look at this?
2015-12-08 12:01:23 -08:00
state_m->i_bus = state_m->mod_d * state_m->id + state_m->mod_q * state_m->iq;
state_m->i_abs = sqrtf(SQ(state_m->id) + SQ(state_m->iq));
state_m->i_abs_filter = sqrtf(SQ(state_m->id_filter) + SQ(state_m->iq_filter));
2015-12-08 12:01:23 -08:00
float mod_alpha = c * state_m->mod_d - s * state_m->mod_q;
float mod_beta = c * state_m->mod_q + s * state_m->mod_d;
// Deadtime compensation
2016-11-04 07:18:34 -07:00
const float i_alpha_filter = c * state_m->id_target - s * state_m->iq_target;
const float i_beta_filter = c * state_m->iq_target + s * state_m->id_target;
const float ia_filter = i_alpha_filter;
const float ib_filter = -0.5 * i_alpha_filter + SQRT3_BY_2 * i_beta_filter;
const float ic_filter = -0.5 * i_alpha_filter - SQRT3_BY_2 * i_beta_filter;
const float mod_alpha_filter_sgn = (2.0 / 3.0) * SIGN(ia_filter) - (1.0 / 3.0) * SIGN(ib_filter) - (1.0 / 3.0) * SIGN(ic_filter);
const float mod_beta_filter_sgn = ONE_BY_SQRT3 * SIGN(ib_filter) - ONE_BY_SQRT3 * SIGN(ic_filter);
const float mod_comp_fact = m_conf->foc_dt_us * 1e-6 * m_conf->foc_f_sw;
const float mod_alpha_comp = mod_alpha_filter_sgn * mod_comp_fact;
const float mod_beta_comp = mod_beta_filter_sgn * mod_comp_fact;
2015-12-08 12:01:23 -08:00
2016-11-04 07:18:34 -07:00
// Apply compensation here so that 0 duty cycle has no glitches.
state_m->v_alpha = (mod_alpha - mod_alpha_comp) * (2.0 / 3.0) * state_m->v_bus;
state_m->v_beta = (mod_beta - mod_beta_comp) * (2.0 / 3.0) * state_m->v_bus;
// Set output (HW Dependent)
uint32_t duty1, duty2, duty3, top;
top = TIM1->ARR;
svm(-mod_alpha, -mod_beta, top, &duty1, &duty2, &duty3, (uint32_t*)&state_m->svm_sector);
2015-12-08 12:01:23 -08:00
TIMER_UPDATE_DUTY(duty1, duty2, duty3);
if(virtual_motor_is_connected() == false){//do not allow to turn on PWM outputs if virtual motor is used
if (!m_output_on) {
start_pwm_hw();
}
2015-12-08 12:01:23 -08:00
}
}
// Magnitude must not be larger than sqrt(3)/2, or 0.866
static void svm(float alpha, float beta, uint32_t PWMHalfPeriod,
uint32_t* tAout, uint32_t* tBout, uint32_t* tCout, uint32_t *svm_sector) {
2015-12-08 12:01:23 -08:00
uint32_t sector;
if (beta >= 0.0f) {
if (alpha >= 0.0f) {
//quadrant I
if (ONE_BY_SQRT3 * beta > alpha) {
2015-12-08 12:01:23 -08:00
sector = 2;
} else {
2015-12-08 12:01:23 -08:00
sector = 1;
}
2015-12-08 12:01:23 -08:00
} else {
//quadrant II
if (-ONE_BY_SQRT3 * beta > alpha) {
2015-12-08 12:01:23 -08:00
sector = 3;
} else {
2015-12-08 12:01:23 -08:00
sector = 2;
}
2015-12-08 12:01:23 -08:00
}
} else {
if (alpha >= 0.0f) {
//quadrant IV5
if (-ONE_BY_SQRT3 * beta > alpha) {
2015-12-08 12:01:23 -08:00
sector = 5;
} else {
2015-12-08 12:01:23 -08:00
sector = 6;
}
2015-12-08 12:01:23 -08:00
} else {
//quadrant III
if (ONE_BY_SQRT3 * beta > alpha) {
2015-12-08 12:01:23 -08:00
sector = 4;
} else {
2015-12-08 12:01:23 -08:00
sector = 5;
}
2015-12-08 12:01:23 -08:00
}
}
// PWM timings
uint32_t tA, tB, tC;
switch (sector) {
// sector 1-2
case 1: {
// Vector on-times
uint32_t t1 = (alpha - ONE_BY_SQRT3 * beta) * PWMHalfPeriod;
uint32_t t2 = (TWO_BY_SQRT3 * beta) * PWMHalfPeriod;
// PWM timings
tA = (PWMHalfPeriod - t1 - t2) / 2;
tB = tA + t1;
tC = tB + t2;
break;
}
// sector 2-3
case 2: {
// Vector on-times
uint32_t t2 = (alpha + ONE_BY_SQRT3 * beta) * PWMHalfPeriod;
uint32_t t3 = (-alpha + ONE_BY_SQRT3 * beta) * PWMHalfPeriod;
// PWM timings
tB = (PWMHalfPeriod - t2 - t3) / 2;
tA = tB + t3;
tC = tA + t2;
break;
}
// sector 3-4
case 3: {
// Vector on-times
uint32_t t3 = (TWO_BY_SQRT3 * beta) * PWMHalfPeriod;
uint32_t t4 = (-alpha - ONE_BY_SQRT3 * beta) * PWMHalfPeriod;
// PWM timings
tB = (PWMHalfPeriod - t3 - t4) / 2;
tC = tB + t3;
tA = tC + t4;
break;
}
// sector 4-5
case 4: {
// Vector on-times
uint32_t t4 = (-alpha + ONE_BY_SQRT3 * beta) * PWMHalfPeriod;
uint32_t t5 = (-TWO_BY_SQRT3 * beta) * PWMHalfPeriod;
// PWM timings
tC = (PWMHalfPeriod - t4 - t5) / 2;
tB = tC + t5;
tA = tB + t4;
break;
}
// sector 5-6
case 5: {
// Vector on-times
uint32_t t5 = (-alpha - ONE_BY_SQRT3 * beta) * PWMHalfPeriod;
uint32_t t6 = (alpha - ONE_BY_SQRT3 * beta) * PWMHalfPeriod;
// PWM timings
tC = (PWMHalfPeriod - t5 - t6) / 2;
tA = tC + t5;
tB = tA + t6;
break;
}
// sector 6-1
case 6: {
// Vector on-times
uint32_t t6 = (-TWO_BY_SQRT3 * beta) * PWMHalfPeriod;
uint32_t t1 = (alpha + ONE_BY_SQRT3 * beta) * PWMHalfPeriod;
// PWM timings
tA = (PWMHalfPeriod - t6 - t1) / 2;
tC = tA + t1;
tB = tC + t6;
break;
}
}
*tAout = tA;
*tBout = tB;
*tCout = tC;
*svm_sector = sector;
2015-12-08 12:01:23 -08:00
}
static void run_pid_control_pos(float angle_now, float angle_set, float dt) {
2015-12-08 12:01:23 -08:00
static float i_term = 0;
static float prev_error = 0;
float p_term;
float d_term;
// PID is off. Return.
if (m_control_mode != CONTROL_MODE_POS) {
i_term = 0;
prev_error = 0;
return;
}
// Compute parameters
float error = utils_angle_difference(angle_set, angle_now);
2015-12-28 17:37:09 -08:00
if (encoder_is_configured()) {
if (m_conf->foc_encoder_inverted) {
error = -error;
}
2015-12-28 17:37:09 -08:00
}
2015-12-08 12:01:23 -08:00
p_term = error * m_conf->p_pid_kp;
i_term += error * (m_conf->p_pid_ki * dt);
// Average DT for the D term when the error does not change. This likely
// happens at low speed when the position resolution is low and several
// control iterations run without position updates.
// TODO: Are there problems with this approach?
static float dt_int = 0.0;
dt_int += dt;
if (error == prev_error) {
d_term = 0.0;
} else {
d_term = (error - prev_error) * (m_conf->p_pid_kd / dt_int);
dt_int = 0.0;
}
2015-12-08 12:01:23 -08:00
// Filter D
static float d_filter = 0.0;
UTILS_LP_FAST(d_filter, d_term, m_conf->p_pid_kd_filter);
d_term = d_filter;
2015-12-08 12:01:23 -08:00
// I-term wind-up protection
utils_truncate_number_abs(&p_term, 1.0);
utils_truncate_number_abs(&i_term, 1.0 - fabsf(p_term));
2015-12-08 12:01:23 -08:00
// Store previous error
prev_error = error;
// Calculate output
float output = p_term + i_term + d_term;
utils_truncate_number(&output, -1.0, 1.0);
if (encoder_is_configured()) {
if (encoder_index_found()) {
m_iq_set = output * m_conf->lo_current_max;
} else {
// Rotate the motor with 40 % power until the encoder index is found.
m_iq_set = 0.4 * m_conf->lo_current_max;
}
} else {
m_iq_set = output * m_conf->lo_current_max;
}
2015-12-08 12:01:23 -08:00
}
static void run_pid_control_speed(float dt) {
static float i_term = 0.0;
static float prev_error = 0.0;
float p_term;
float d_term;
// PID is off. Return.
if (m_control_mode != CONTROL_MODE_SPEED) {
i_term = 0.0;
prev_error = 0.0;
return;
}
const float rpm = mcpwm_foc_get_rpm();
float error = m_speed_pid_set_rpm - rpm;
// Too low RPM set. Reset state and return.
2015-12-08 12:01:23 -08:00
if (fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) {
i_term = 0.0;
prev_error = error;
2015-12-08 12:01:23 -08:00
return;
}
// Compute parameters
p_term = error * m_conf->s_pid_kp * (1.0 / 20.0);
i_term += error * (m_conf->s_pid_ki * dt) * (1.0 / 20.0);
d_term = (error - prev_error) * (m_conf->s_pid_kd / dt) * (1.0 / 20.0);
2015-12-08 12:01:23 -08:00
// Filter D
static float d_filter = 0.0;
UTILS_LP_FAST(d_filter, d_term, m_conf->s_pid_kd_filter);
d_term = d_filter;
2015-12-08 12:01:23 -08:00
// I-term wind-up protection
utils_truncate_number(&i_term, -1.0, 1.0);
// Store previous error
prev_error = error;
// Calculate output
float output = p_term + i_term + d_term;
utils_truncate_number(&output, -1.0, 1.0);
// Optionally disable braking
if (!m_conf->s_pid_allow_braking) {
2019-02-18 10:30:19 -08:00
if (rpm > 20.0 && output < 0.0) {
output = 0.0;
}
2019-02-18 10:30:19 -08:00
if (rpm < -20.0 && output > 0.0) {
output = 0.0;
}
}
2015-12-08 12:01:23 -08:00
m_iq_set = output * m_conf->lo_current_max;
}
static void stop_pwm_hw(void) {
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_ForcedAction_InActive);
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable);
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_ForcedAction_InActive);
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable);
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_ForcedAction_InActive);
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable);
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
2016-11-04 07:18:34 -07:00
#ifdef HW_HAS_DRV8313
DISABLE_BR();
#endif
2015-12-08 12:01:23 -08:00
m_output_on = false;
}
static void start_pwm_hw(void) {
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_PWM1);
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_PWM1);
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Enable);
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_PWM1);
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable);
// Generate COM event in ADC interrupt to get better synchronization
// TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
2015-12-08 12:01:23 -08:00
2016-11-04 07:18:34 -07:00
#ifdef HW_HAS_DRV8313
ENABLE_BR();
#endif
2015-12-08 12:01:23 -08:00
m_output_on = true;
}
static int read_hall(void) {
2016-11-04 07:18:34 -07:00
int h1_1 = READ_HALL1();
int h2_1 = READ_HALL2();
int h3_1 = READ_HALL3();
int h1_2 = READ_HALL1();
int h2_2 = READ_HALL2();
int h3_2 = READ_HALL3();
int h1_3 = READ_HALL1();
int h2_3 = READ_HALL2();
int h3_3 = READ_HALL3();
return utils_middle_of_3_int(h1_1, h1_2, h1_3) |
(utils_middle_of_3_int(h2_1, h2_2, h2_3) << 1) |
(utils_middle_of_3_int(h3_1, h3_2, h3_3) << 2);
}
static float correct_encoder(float obs_angle, float enc_angle, float speed) {
float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
// Hysteresis 5 % of total speed
float hyst = m_conf->foc_sl_erpm * 0.05;
if (m_using_encoder) {
if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
m_using_encoder = false;
}
} else {
if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
m_using_encoder = true;
}
}
return m_using_encoder ? enc_angle : obs_angle;
}
static float correct_hall(float angle, float speed, float dt) {
static int ang_hall_int_prev = -1;
float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
static bool using_hall = true;
// Hysteresis 5 % of total speed
2016-11-04 07:18:34 -07:00
float hyst = m_conf->foc_sl_erpm * 0.1;
if (using_hall) {
if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
using_hall = false;
}
} else {
if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
using_hall = true;
}
}
if (using_hall) {
int ang_hall_int = m_conf->foc_hall_table[read_hall()];
// Only override the observer if the hall sensor value is valid.
if (ang_hall_int < 201) {
static float ang_hall = 0.0;
float ang_hall_now = (((float)ang_hall_int / 200.0) * 360.0) * M_PI / 180.0;
if (ang_hall_int_prev < 0) {
// Previous angle not valid
ang_hall_int_prev = ang_hall_int;
if (ang_hall_int_prev == -2) {
// Before was sensorless, initialize with the provided angle
ang_hall = angle;
} else {
// A boot or error has occurred. Use center of hall sensor angle.
ang_hall = ((ang_hall_int / 200.0) * 360.0) * M_PI / 180.0;
}
} else if (ang_hall_int != ang_hall_int_prev) {
// A transition was just made. The angle is in the middle of the new and old angle.
int ang_avg = abs(ang_hall_int - ang_hall_int_prev);
if (ang_avg < 100) {
ang_avg = (ang_hall_int + ang_hall_int_prev) / 2;
} else if (ang_avg != 100) {
ang_avg = (ang_hall_int + ang_hall_int_prev) / 2 + 100;
}
ang_avg %= 200;
ang_hall = (((float)ang_avg / 200.0) * 360.0) * M_PI / 180.0;
}
ang_hall_int_prev = ang_hall_int;
if (rpm_abs < 100) {
// Don't interpolate on very low speed, just use the closest hall sensor
ang_hall = ang_hall_now;
} else {
// Interpolate
float diff = utils_angle_difference_rad(ang_hall, ang_hall_now);
if (fabsf(diff) < ((2.0 * M_PI) / 12.0)) {
// Do interpolation
ang_hall += speed * dt;
} else {
// We are too far away with the interpolation
ang_hall -= diff / 100.0;
}
}
utils_norm_angle_rad(&ang_hall);
angle = ang_hall;
} else {
// Invalid hall reading. Don't update angle.
ang_hall_int_prev = -1;
// Also allow open loop in order to behave like normal sensorless
// operation. Then the motor works even if the hall sensor cable
// gets disconnected (when the sensor spacing is 120 degrees).
if (m_phase_observer_override && m_state == MC_STATE_RUNNING) {
angle = m_phase_now_observer_override;
}
}
} else {
// We are running sensorless.
ang_hall_int_prev = -2;
}
return angle;
}