2015-12-08 12:01:23 -08:00
|
|
|
|
/*
|
2022-03-23 07:04:30 -07:00
|
|
|
|
Copyright 2016 - 2022 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/>.
|
2020-03-28 11:21:45 -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
|
2017-05-28 12:47:25 -07:00
|
|
|
|
|
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"
|
2022-03-16 10:40:51 -07:00
|
|
|
|
#include "utils_math.h"
|
|
|
|
|
#include "utils_sys.h"
|
2015-12-08 12:01:23 -08:00
|
|
|
|
#include "ledpwm.h"
|
|
|
|
|
#include "terminal.h"
|
2022-01-09 08:10:40 -08:00
|
|
|
|
#include "encoder/encoder.h"
|
2015-12-08 12:01:23 -08:00
|
|
|
|
#include "commands.h"
|
|
|
|
|
#include "timeout.h"
|
2019-03-10 06:57:42 -07:00
|
|
|
|
#include "timer.h"
|
2015-12-08 12:01:23 -08:00
|
|
|
|
#include <math.h>
|
|
|
|
|
#include <string.h>
|
|
|
|
|
#include <stdlib.h>
|
2020-01-20 00:39:33 -08:00
|
|
|
|
#include <stdio.h>
|
2019-04-06 06:36:00 -07:00
|
|
|
|
#include "virtual_motor.h"
|
2022-04-06 06:12:24 -07:00
|
|
|
|
#include "foc_math.h"
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Private variables
|
2020-03-16 10:32:39 -07:00
|
|
|
|
static volatile bool m_dccal_done = false;
|
2019-02-18 10:30:19 -08:00
|
|
|
|
static volatile float m_last_adc_isr_duration;
|
|
|
|
|
static volatile bool m_init_done = false;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
static volatile motor_all_state_t m_motor_1;
|
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
static volatile motor_all_state_t m_motor_2;
|
|
|
|
|
#endif
|
2020-03-16 14:42:44 -07:00
|
|
|
|
static volatile int m_isr_motor = 0;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Private functions
|
2022-04-06 07:33:44 -07:00
|
|
|
|
static void control_current(motor_all_state_t *motor, float dt);
|
|
|
|
|
static void update_valpha_vbeta(motor_all_state_t *motor, float mod_alpha, float mod_beta);
|
|
|
|
|
static void stop_pwm_hw(motor_all_state_t *motor);
|
|
|
|
|
static void start_pwm_hw(motor_all_state_t *motor);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
static void terminal_tmp(int argc, const char **argv);
|
2020-01-20 00:39:33 -08:00
|
|
|
|
static void terminal_plot_hfi(int argc, const char **argv);
|
2022-04-06 07:33:44 -07:00
|
|
|
|
static void timer_update(motor_all_state_t *motor, float dt);
|
2020-02-16 06:07:08 -08:00
|
|
|
|
static void input_current_offset_measurement( void );
|
2022-03-23 07:04:30 -07:00
|
|
|
|
static void hfi_update(volatile motor_all_state_t *motor, float dt);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Threads
|
2022-01-16 12:57:12 -08:00
|
|
|
|
static THD_WORKING_AREA(timer_thread_wa, 512);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
static THD_FUNCTION(timer_thread, arg);
|
|
|
|
|
static volatile bool timer_thd_stop;
|
|
|
|
|
|
2022-01-16 12:57:12 -08:00
|
|
|
|
static THD_WORKING_AREA(hfi_thread_wa, 512);
|
2020-01-20 00:39:33 -08:00
|
|
|
|
static THD_FUNCTION(hfi_thread, arg);
|
|
|
|
|
static volatile bool hfi_thd_stop;
|
|
|
|
|
|
2022-01-16 12:57:12 -08:00
|
|
|
|
static THD_WORKING_AREA(pid_thread_wa, 256);
|
2021-07-12 05:31:01 -07:00
|
|
|
|
static THD_FUNCTION(pid_thread, arg);
|
|
|
|
|
static volatile bool pid_thd_stop;
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Macros
|
2016-06-27 08:29:09 -07:00
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#define TIMER_UPDATE_DUTY_M1(duty1, duty2, duty3) \
|
2016-06-27 08:29:09 -07:00
|
|
|
|
TIM1->CR1 |= TIM_CR1_UDIS; \
|
|
|
|
|
TIM1->CCR1 = duty1; \
|
|
|
|
|
TIM1->CCR2 = duty2; \
|
|
|
|
|
TIM1->CCR3 = duty3; \
|
|
|
|
|
TIM1->CR1 &= ~TIM_CR1_UDIS;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
#define TIMER_UPDATE_DUTY_M2(duty1, duty2, duty3) \
|
|
|
|
|
TIM8->CR1 |= TIM_CR1_UDIS; \
|
|
|
|
|
TIM8->CCR1 = duty1; \
|
|
|
|
|
TIM8->CCR2 = duty2; \
|
|
|
|
|
TIM8->CCR3 = duty3; \
|
|
|
|
|
TIM8->CR1 &= ~TIM_CR1_UDIS;
|
2016-06-27 08:29:09 -07:00
|
|
|
|
#else
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#define TIMER_UPDATE_DUTY_M1(duty1, duty2, duty3) \
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM1->CR1 |= TIM_CR1_UDIS; \
|
|
|
|
|
TIM1->CCR1 = duty1; \
|
|
|
|
|
TIM1->CCR2 = duty3; \
|
|
|
|
|
TIM1->CCR3 = duty2; \
|
|
|
|
|
TIM1->CR1 &= ~TIM_CR1_UDIS;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#define TIMER_UPDATE_DUTY_M2(duty1, duty2, duty3) \
|
|
|
|
|
TIM8->CR1 |= TIM_CR1_UDIS; \
|
|
|
|
|
TIM8->CCR1 = duty1; \
|
|
|
|
|
TIM8->CCR2 = duty3; \
|
|
|
|
|
TIM8->CCR3 = duty2; \
|
|
|
|
|
TIM8->CR1 &= ~TIM_CR1_UDIS;
|
2016-06-27 08:29:09 -07:00
|
|
|
|
#endif
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
|
#define TIMER_UPDATE_SAMP(samp) \
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM2->CCR2 = (samp / 2);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#define TIMER_UPDATE_SAMP_TOP_M1(samp, top) \
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM1->CR1 |= TIM_CR1_UDIS; \
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM2->CR1 |= TIM_CR1_UDIS; \
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM1->ARR = top; \
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM2->CCR2 = samp / 2; \
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM1->CR1 &= ~TIM_CR1_UDIS; \
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM2->CR1 &= ~TIM_CR1_UDIS;
|
|
|
|
|
#define TIMER_UPDATE_SAMP_TOP_M2(samp, top) \
|
|
|
|
|
TIM8->CR1 |= TIM_CR1_UDIS; \
|
|
|
|
|
TIM2->CR1 |= TIM_CR1_UDIS; \
|
|
|
|
|
TIM8->ARR = top; \
|
|
|
|
|
TIM2->CCR2 = samp / 2; \
|
|
|
|
|
TIM8->CR1 &= ~TIM_CR1_UDIS; \
|
|
|
|
|
TIM2->CR1 &= ~TIM_CR1_UDIS;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-10-27 10:44:58 -07:00
|
|
|
|
// #define M_MOTOR: For single motor compilation, expands to &m_motor_1.
|
|
|
|
|
// For dual motors, expands to &m_motor_1 or _2, depending on is_second_motor.
|
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
#define M_MOTOR(is_second_motor) (is_second_motor ? &m_motor_2 : &m_motor_1)
|
|
|
|
|
#else
|
|
|
|
|
#define M_MOTOR(is_second_motor) (((void)is_second_motor), &m_motor_1)
|
|
|
|
|
#endif
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
static void update_hfi_samples(foc_hfi_samples samples, volatile motor_all_state_t *motor) {
|
2020-01-28 10:46:19 -08:00
|
|
|
|
utils_sys_lock_cnt();
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
memset((void*)&motor->m_hfi, 0, sizeof(motor->m_hfi));
|
2020-01-28 10:46:19 -08:00
|
|
|
|
switch (samples) {
|
|
|
|
|
case HFI_SAMPLES_8:
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_hfi.samples = 8;
|
|
|
|
|
motor->m_hfi.table_fact = 4;
|
|
|
|
|
motor->m_hfi.fft_bin0_func = utils_fft8_bin0;
|
|
|
|
|
motor->m_hfi.fft_bin1_func = utils_fft8_bin1;
|
|
|
|
|
motor->m_hfi.fft_bin2_func = utils_fft8_bin2;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case HFI_SAMPLES_16:
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_hfi.samples = 16;
|
|
|
|
|
motor->m_hfi.table_fact = 2;
|
|
|
|
|
motor->m_hfi.fft_bin0_func = utils_fft16_bin0;
|
|
|
|
|
motor->m_hfi.fft_bin1_func = utils_fft16_bin1;
|
|
|
|
|
motor->m_hfi.fft_bin2_func = utils_fft16_bin2;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case HFI_SAMPLES_32:
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_hfi.samples = 32;
|
|
|
|
|
motor->m_hfi.table_fact = 1;
|
|
|
|
|
motor->m_hfi.fft_bin0_func = utils_fft32_bin0;
|
|
|
|
|
motor->m_hfi.fft_bin1_func = utils_fft32_bin1;
|
|
|
|
|
motor->m_hfi.fft_bin2_func = utils_fft32_bin2;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
utils_sys_unlock_cnt();
|
|
|
|
|
}
|
|
|
|
|
|
2022-01-06 11:44:00 -08:00
|
|
|
|
static void timer_reinit(int f_zv) {
|
2015-12-08 12:01:23 -08:00
|
|
|
|
utils_sys_lock_cnt();
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM_DeInit(TIM1);
|
|
|
|
|
TIM_DeInit(TIM8);
|
|
|
|
|
TIM_DeInit(TIM2);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
|
|
|
|
TIM_OCInitTypeDef TIM_OCInitStructure;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM_BDTRInitTypeDef TIM_BDTRInitStructure;
|
|
|
|
|
|
|
|
|
|
TIM1->CNT = 0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM2->CNT = 0;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM8->CNT = 0;
|
|
|
|
|
|
|
|
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
TIM_TimeBaseStructure.TIM_Prescaler = 0;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1;
|
2022-01-06 11:44:00 -08:00
|
|
|
|
TIM_TimeBaseStructure.TIM_Period = (SYSTEM_CORE_CLOCK / f_zv);
|
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;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
|
|
|
|
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
|
2015-12-19 12:24:46 -08:00
|
|
|
|
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
|
2020-03-17 01:38:09 -07:00
|
|
|
|
TIM_OCInitStructure.TIM_Pulse = TIM1->ARR / 2;
|
2022-03-02 01:12:00 -08:00
|
|
|
|
|
|
|
|
|
#ifndef INVERTED_TOP_DRIVER_INPUT
|
|
|
|
|
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; // gpio high = top fets on
|
|
|
|
|
#else
|
|
|
|
|
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
|
|
|
|
|
#endif
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
|
2022-03-02 01:12:00 -08:00
|
|
|
|
|
|
|
|
|
#ifndef INVERTED_BOTTOM_DRIVER_INPUT
|
|
|
|
|
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; // gpio high = bottom fets on
|
|
|
|
|
#else
|
|
|
|
|
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_Low;
|
|
|
|
|
#endif
|
2015-12-08 12:01:23 -08:00
|
|
|
|
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);
|
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
|
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
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM_OC1Init(TIM8, &TIM_OCInitStructure);
|
|
|
|
|
TIM_OC2Init(TIM8, &TIM_OCInitStructure);
|
|
|
|
|
TIM_OC3Init(TIM8, &TIM_OCInitStructure);
|
|
|
|
|
TIM_OC4Init(TIM8, &TIM_OCInitStructure);
|
|
|
|
|
|
|
|
|
|
TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
|
|
|
|
|
TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);
|
|
|
|
|
TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable);
|
|
|
|
|
TIM_OC4PreloadConfig(TIM8, 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;
|
2015-12-19 12:24:46 -08:00
|
|
|
|
TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
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_AutomaticOutput = TIM_AutomaticOutput_Disable;
|
|
|
|
|
|
2020-01-09 15:38:06 -08:00
|
|
|
|
#ifdef HW_USE_BRK
|
|
|
|
|
// Enable BRK function. Hardware will asynchronously stop any PWM activity upon an
|
|
|
|
|
// external fault signal. PWM outputs remain disabled until MCU is reset.
|
|
|
|
|
// software will catch the BRK flag to report the fault code
|
|
|
|
|
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Enable;
|
|
|
|
|
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_Low;
|
2020-01-10 14:11:46 -08:00
|
|
|
|
#else
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
|
|
|
|
|
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
|
2020-01-09 15:38:06 -08:00
|
|
|
|
#endif
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
TIM_BDTRConfig(TIM1, &TIM_BDTRInitStructure);
|
2017-09-04 12:12:43 -07:00
|
|
|
|
TIM_CCPreloadControl(TIM1, ENABLE);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM_ARRPreloadConfig(TIM1, ENABLE);
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM_BDTRConfig(TIM8, &TIM_BDTRInitStructure);
|
|
|
|
|
TIM_CCPreloadControl(TIM8, ENABLE);
|
|
|
|
|
TIM_ARRPreloadConfig(TIM8, ENABLE);
|
|
|
|
|
|
|
|
|
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
|
|
|
|
|
|
|
|
|
|
TIM_TimeBaseStructure.TIM_Prescaler = 0;
|
|
|
|
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
|
|
|
|
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
|
|
|
|
|
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
|
|
|
|
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
|
|
|
|
|
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
|
|
|
|
|
|
|
|
|
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
|
|
|
|
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
|
|
|
|
|
TIM_OCInitStructure.TIM_Pulse = 250;
|
|
|
|
|
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(TIM2, &TIM_OCInitStructure);
|
|
|
|
|
TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
|
|
|
|
|
TIM_OC2Init(TIM2, &TIM_OCInitStructure);
|
|
|
|
|
TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
|
|
|
|
|
TIM_OC3Init(TIM2, &TIM_OCInitStructure);
|
|
|
|
|
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
|
|
|
|
|
|
|
|
|
|
TIM_ARRPreloadConfig(TIM2, ENABLE);
|
|
|
|
|
TIM_CCPreloadControl(TIM2, ENABLE);
|
|
|
|
|
|
|
|
|
|
// PWM outputs have to be enabled in order to trigger ADC on CCx
|
|
|
|
|
TIM_CtrlPWMOutputs(TIM2, ENABLE);
|
|
|
|
|
|
2020-04-01 17:58:37 -07:00
|
|
|
|
#if defined HW_HAS_DUAL_MOTORS || defined HW_HAS_DUAL_PARALLEL
|
2021-03-13 06:41:57 -08:00
|
|
|
|
// See: https://www.cnblogs.com/shangdawei/p/4758988.html
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM_SelectOutputTrigger(TIM1, TIM_TRGOSource_Enable);
|
|
|
|
|
TIM_SelectMasterSlaveMode(TIM1, TIM_MasterSlaveMode_Enable);
|
|
|
|
|
TIM_SelectInputTrigger(TIM8, TIM_TS_ITR0);
|
|
|
|
|
TIM_SelectSlaveMode(TIM8, TIM_SlaveMode_Trigger);
|
|
|
|
|
TIM_SelectOutputTrigger(TIM8, TIM_TRGOSource_Enable);
|
|
|
|
|
TIM_SelectOutputTrigger(TIM8, TIM_TRGOSource_Update);
|
|
|
|
|
TIM_SelectInputTrigger(TIM2, TIM_TS_ITR1);
|
|
|
|
|
TIM_SelectSlaveMode(TIM2, TIM_SlaveMode_Reset);
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#else
|
|
|
|
|
TIM_SelectOutputTrigger(TIM1, TIM_TRGOSource_Update);
|
|
|
|
|
TIM_SelectMasterSlaveMode(TIM1, TIM_MasterSlaveMode_Enable);
|
|
|
|
|
TIM_SelectInputTrigger(TIM2, TIM_TS_ITR0);
|
|
|
|
|
TIM_SelectSlaveMode(TIM2, TIM_SlaveMode_Reset);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2020-04-01 17:58:37 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM8->CNT = TIM1->ARR;
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#else
|
|
|
|
|
TIM8->CNT = 0;
|
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM1->CNT = 0;
|
|
|
|
|
TIM_Cmd(TIM1, ENABLE);
|
|
|
|
|
TIM_Cmd(TIM2, ENABLE);
|
|
|
|
|
|
|
|
|
|
// Prevent all low side FETs from switching on
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_2);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
TIM_CtrlPWMOutputs(TIM1, ENABLE);
|
|
|
|
|
TIM_CtrlPWMOutputs(TIM8, ENABLE);
|
|
|
|
|
|
|
|
|
|
TIMER_UPDATE_SAMP(MCPWM_FOC_CURRENT_SAMP_OFFSET);
|
|
|
|
|
|
2020-03-17 01:38:09 -07:00
|
|
|
|
// Enable CC2 interrupt, which will be fired in V0 and V7
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM_ITConfig(TIM2, TIM_IT_CC2, ENABLE);
|
|
|
|
|
utils_sys_unlock_cnt();
|
|
|
|
|
|
|
|
|
|
nvicEnableVector(TIM2_IRQn, 6);
|
|
|
|
|
}
|
|
|
|
|
|
2023-12-26 13:37:41 -08:00
|
|
|
|
static void init_audio_state(volatile mc_audio_state *s) {
|
|
|
|
|
memset((void*)s, 0, sizeof(mc_audio_state));
|
|
|
|
|
|
|
|
|
|
s->mode = MC_AUDIO_OFF;
|
|
|
|
|
for (int i = 0;i < MC_AUDIO_CHANNELS;i++) {
|
|
|
|
|
s->table[i] = utils_tab_sin_32_1;
|
|
|
|
|
s->table_len[i] = 32;
|
|
|
|
|
s->table_voltage[i] = 0.0;
|
|
|
|
|
s->table_freq[i] = 1000.0;
|
|
|
|
|
s->table_pos[i] = 0.0;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
void mcpwm_foc_init(mc_configuration *conf_m1, mc_configuration *conf_m2) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
utils_sys_lock_cnt();
|
|
|
|
|
|
|
|
|
|
#ifndef HW_HAS_DUAL_MOTORS
|
|
|
|
|
(void)conf_m2;
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
m_init_done = false;
|
|
|
|
|
|
|
|
|
|
memset((void*)&m_motor_1, 0, sizeof(motor_all_state_t));
|
2020-03-16 14:42:44 -07:00
|
|
|
|
m_isr_motor = 0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
m_motor_1.m_conf = conf_m1;
|
|
|
|
|
m_motor_1.m_state = MC_STATE_OFF;
|
|
|
|
|
m_motor_1.m_control_mode = CONTROL_MODE_NONE;
|
2020-04-16 15:38:36 -07:00
|
|
|
|
m_motor_1.m_hall_dt_diff_last = 1.0;
|
2022-04-07 12:53:41 -07:00
|
|
|
|
foc_precalc_values((motor_all_state_t*)&m_motor_1);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
update_hfi_samples(m_motor_1.m_conf->foc_hfi_samples, &m_motor_1);
|
2023-12-26 13:37:41 -08:00
|
|
|
|
init_audio_state(&m_motor_1.m_audio);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
memset((void*)&m_motor_2, 0, sizeof(motor_all_state_t));
|
|
|
|
|
m_motor_2.m_conf = conf_m2;
|
|
|
|
|
m_motor_2.m_state = MC_STATE_OFF;
|
|
|
|
|
m_motor_2.m_control_mode = CONTROL_MODE_NONE;
|
2020-04-16 15:38:36 -07:00
|
|
|
|
m_motor_2.m_hall_dt_diff_last = 1.0;
|
2022-04-07 12:53:41 -07:00
|
|
|
|
foc_precalc_values((motor_all_state_t*)&m_motor_2);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
update_hfi_samples(m_motor_2.m_conf->foc_hfi_samples, &m_motor_2);
|
2023-12-26 13:37:41 -08:00
|
|
|
|
init_audio_state(&m_motor_2.m_audio);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2020-06-06 14:21:53 -07:00
|
|
|
|
virtual_motor_init(conf_m1);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
TIM_DeInit(TIM1);
|
|
|
|
|
TIM_DeInit(TIM2);
|
|
|
|
|
TIM_DeInit(TIM8);
|
2023-07-27 09:01:16 -07:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM1->CNT = 0;
|
|
|
|
|
TIM2->CNT = 0;
|
|
|
|
|
TIM8->CNT = 0;
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
ADC_CommonInitTypeDef ADC_CommonInitStructure;
|
|
|
|
|
DMA_InitTypeDef DMA_InitStructure;
|
|
|
|
|
ADC_InitTypeDef ADC_InitStructure;
|
|
|
|
|
|
|
|
|
|
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)),
|
2020-03-28 11:21:45 -07:00
|
|
|
|
5,
|
|
|
|
|
(stm32_dmaisr_t)mcpwm_foc_adc_int_handler,
|
|
|
|
|
(void *)0);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
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);
|
|
|
|
|
|
|
|
|
|
DMA_Cmd(DMA2_Stream4, ENABLE);
|
2022-03-23 07:04:30 -07:00
|
|
|
|
|
|
|
|
|
// Note: The half transfer interrupt is used as we already have all current and voltage
|
|
|
|
|
// samples by then and we can start processing them. Entering the interrupt earlier gives
|
|
|
|
|
// more cycles to finish it and update the timer before the next zero vector. This helps
|
2022-03-23 14:26:05 -07:00
|
|
|
|
// at higher f_zv. Only use this if the three first samples are current samples.
|
|
|
|
|
#if ADC_IND_CURR1 < 3 && ADC_IND_CURR2 < 3 && ADC_IND_CURR3 < 3
|
2022-03-23 07:04:30 -07:00
|
|
|
|
DMA_ITConfig(DMA2_Stream4, DMA_IT_HT, ENABLE);
|
2022-03-23 14:26:05 -07:00
|
|
|
|
#else
|
|
|
|
|
DMA_ITConfig(DMA2_Stream4, DMA_IT_TC, ENABLE);
|
|
|
|
|
#endif
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// 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);
|
|
|
|
|
|
|
|
|
|
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
|
|
|
|
|
ADC_InitStructure.ADC_ScanConvMode = ENABLE;
|
|
|
|
|
ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
|
|
|
|
|
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Falling;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T2_CC2;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
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);
|
|
|
|
|
|
|
|
|
|
ADC_TempSensorVrefintCmd(ENABLE);
|
|
|
|
|
ADC_MultiModeDMARequestAfterLastTransferCmd(ENABLE);
|
|
|
|
|
|
|
|
|
|
hw_setup_adc_channels();
|
|
|
|
|
|
|
|
|
|
ADC_Cmd(ADC1, ENABLE);
|
|
|
|
|
ADC_Cmd(ADC2, ENABLE);
|
|
|
|
|
ADC_Cmd(ADC3, ENABLE);
|
|
|
|
|
|
2022-01-06 11:44:00 -08:00
|
|
|
|
timer_reinit((int)m_motor_1.m_conf->foc_f_zv);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_2);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
utils_sys_unlock_cnt();
|
|
|
|
|
|
2019-02-18 10:30:19 -08:00
|
|
|
|
CURRENT_FILTER_ON();
|
2022-06-08 05:00:43 -07:00
|
|
|
|
CURRENT_FILTER_ON_M2();
|
2015-12-08 12:01:23 -08:00
|
|
|
|
ENABLE_GATE();
|
|
|
|
|
DCCAL_OFF();
|
2023-08-24 15:45:58 -07:00
|
|
|
|
#ifdef HW_USE_ALTERNATIVE_DC_CAL
|
|
|
|
|
m_dccal_done = true;
|
|
|
|
|
#else
|
2021-03-22 04:13:19 -07:00
|
|
|
|
if (m_motor_1.m_conf->foc_offsets_cal_on_boot) {
|
2021-05-13 02:45:30 -07:00
|
|
|
|
systime_t cal_start_time = chVTGetSystemTimeX();
|
|
|
|
|
float cal_start_timeout = 10.0;
|
|
|
|
|
|
|
|
|
|
// Wait for input voltage to rise above minimum voltage
|
|
|
|
|
while (mc_interface_get_input_voltage_filtered() < m_motor_1.m_conf->l_min_vin) {
|
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
if (UTILS_AGE_S(cal_start_time) >= cal_start_timeout) {
|
|
|
|
|
m_dccal_done = true;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Wait for input voltage to settle
|
|
|
|
|
if (!m_dccal_done) {
|
|
|
|
|
float v_in_last = mc_interface_get_input_voltage_filtered();
|
|
|
|
|
systime_t v_in_stable_time = chVTGetSystemTimeX();
|
|
|
|
|
while (UTILS_AGE_S(v_in_stable_time) < 2.0) {
|
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
|
|
|
|
|
float v_in_now = mc_interface_get_input_voltage_filtered();
|
|
|
|
|
if (fabsf(v_in_now - v_in_last) > 1.5) {
|
|
|
|
|
v_in_last = v_in_now;
|
|
|
|
|
v_in_stable_time = chVTGetSystemTimeX();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (UTILS_AGE_S(cal_start_time) >= cal_start_timeout) {
|
|
|
|
|
m_dccal_done = true;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2022-05-06 13:20:32 -07:00
|
|
|
|
// Wait for fault codes to go away
|
|
|
|
|
if (!m_dccal_done) {
|
|
|
|
|
while (mc_interface_get_fault() != FAULT_CODE_NONE) {
|
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
if (UTILS_AGE_S(cal_start_time) >= cal_start_timeout) {
|
|
|
|
|
m_dccal_done = true;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-05-13 02:45:30 -07:00
|
|
|
|
if (!m_dccal_done) {
|
2022-05-06 13:08:41 -07:00
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage[0] = MCCONF_FOC_OFFSETS_VOLTAGE_0;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage[1] = MCCONF_FOC_OFFSETS_VOLTAGE_1;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage[2] = MCCONF_FOC_OFFSETS_VOLTAGE_2;
|
|
|
|
|
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[0] = MCCONF_FOC_OFFSETS_VOLTAGE_UNDRIVEN_0;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[1] = MCCONF_FOC_OFFSETS_VOLTAGE_UNDRIVEN_1;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[2] = MCCONF_FOC_OFFSETS_VOLTAGE_UNDRIVEN_2;
|
|
|
|
|
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_current[0] = MCCONF_FOC_OFFSETS_CURRENT_0;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_current[1] = MCCONF_FOC_OFFSETS_CURRENT_1;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_current[2] = MCCONF_FOC_OFFSETS_CURRENT_2;
|
2021-03-24 05:09:05 -07:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-05-06 13:08:41 -07:00
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage[0] = MCCONF_FOC_OFFSETS_VOLTAGE_0;
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage[1] = MCCONF_FOC_OFFSETS_VOLTAGE_1;
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage[2] = MCCONF_FOC_OFFSETS_VOLTAGE_2;
|
|
|
|
|
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage_undriven[0] = MCCONF_FOC_OFFSETS_VOLTAGE_UNDRIVEN_0;
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage_undriven[1] = MCCONF_FOC_OFFSETS_VOLTAGE_UNDRIVEN_1;
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage_undriven[2] = MCCONF_FOC_OFFSETS_VOLTAGE_UNDRIVEN_2;
|
|
|
|
|
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_current[0] = MCCONF_FOC_OFFSETS_CURRENT_0;
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_current[1] = MCCONF_FOC_OFFSETS_CURRENT_1;
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_current[2] = MCCONF_FOC_OFFSETS_CURRENT_2;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#endif
|
|
|
|
|
|
2021-05-13 02:45:30 -07:00
|
|
|
|
mcpwm_foc_dc_cal(false);
|
|
|
|
|
}
|
2021-02-28 11:36:02 -08:00
|
|
|
|
} else {
|
|
|
|
|
m_dccal_done = true;
|
|
|
|
|
}
|
2023-08-24 15:45:58 -07:00
|
|
|
|
#endif
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Start threads
|
|
|
|
|
timer_thd_stop = false;
|
|
|
|
|
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
|
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
|
hfi_thd_stop = false;
|
|
|
|
|
chThdCreateStatic(hfi_thread_wa, sizeof(hfi_thread_wa), NORMALPRIO, hfi_thread, NULL);
|
|
|
|
|
|
2021-07-12 05:31:01 -07:00
|
|
|
|
pid_thd_stop = false;
|
|
|
|
|
chThdCreateStatic(pid_thread_wa, sizeof(pid_thread_wa), NORMALPRIO, pid_thread, NULL);
|
|
|
|
|
|
2022-04-11 11:35:24 -07:00
|
|
|
|
// Check if the system has resumed from IWDG reset and generate fault if it has. This can be used to
|
|
|
|
|
// tell if some frozen thread caused a watchdog reset. Note that this also will trigger after running
|
|
|
|
|
// the bootloader and after the reset command.
|
2019-02-19 09:55:18 -08:00
|
|
|
|
if (timeout_had_IWDG_reset()) {
|
2020-04-03 11:12:12 -07:00
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, false, false);
|
2019-02-19 09:55:18 -08:00
|
|
|
|
}
|
2019-01-24 07:19:44 -08:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
terminal_register_command_callback(
|
|
|
|
|
"foc_tmp",
|
|
|
|
|
"FOC Test Print",
|
|
|
|
|
0,
|
|
|
|
|
terminal_tmp);
|
|
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
|
terminal_register_command_callback(
|
|
|
|
|
"foc_plot_hfi_en",
|
2020-01-28 10:46:19 -08:00
|
|
|
|
"Enable HFI plotting. 0: off, 1: DFT, 2: Raw",
|
2020-01-20 00:39:33 -08:00
|
|
|
|
"[en]",
|
|
|
|
|
terminal_plot_hfi);
|
|
|
|
|
|
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);
|
|
|
|
|
}
|
|
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
|
hfi_thd_stop = true;
|
|
|
|
|
while (hfi_thd_stop) {
|
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
}
|
|
|
|
|
|
2021-07-12 05:31:01 -07:00
|
|
|
|
pid_thd_stop = true;
|
|
|
|
|
while (pid_thd_stop) {
|
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM_DeInit(TIM1);
|
2020-03-17 01:38:09 -07:00
|
|
|
|
TIM_DeInit(TIM2);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
TIM_DeInit(TIM8);
|
|
|
|
|
ADC_DeInit();
|
|
|
|
|
DMA_DeInit(DMA2_Stream4);
|
|
|
|
|
nvicDisableVector(ADC_IRQn);
|
|
|
|
|
dmaStreamRelease(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)));
|
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
static volatile motor_all_state_t *get_motor_now(void) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2020-03-16 14:42:44 -07:00
|
|
|
|
return mc_interface_motor_now() == 1 ? &m_motor_1 : &m_motor_2;
|
|
|
|
|
#else
|
|
|
|
|
return &m_motor_1;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
|
bool mcpwm_foc_init_done(void) {
|
|
|
|
|
return m_init_done;
|
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
void mcpwm_foc_set_configuration(mc_configuration *configuration) {
|
|
|
|
|
get_motor_now()->m_conf = configuration;
|
2022-04-07 12:53:41 -07:00
|
|
|
|
foc_precalc_values((motor_all_state_t*)get_motor_now());
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
// Below we check if anything in the configuration changed that requires stopping the motor.
|
|
|
|
|
|
2022-01-06 11:44:00 -08:00
|
|
|
|
uint32_t top = SYSTEM_CORE_CLOCK / (int)configuration->foc_f_zv;
|
2019-09-27 03:56:49 -07:00
|
|
|
|
if (TIM1->ARR != top) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
m_motor_1.m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
m_motor_1.m_state = MC_STATE_OFF;
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
m_motor_2.m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
m_motor_2.m_state = MC_STATE_OFF;
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_2);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
2022-01-06 11:44:00 -08:00
|
|
|
|
timer_reinit((int)configuration->foc_f_zv);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#else
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
get_motor_now()->m_state = MC_STATE_OFF;
|
|
|
|
|
stop_pwm_hw((motor_all_state_t*)get_motor_now());
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIMER_UPDATE_SAMP_TOP_M1(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_PARALLEL
|
|
|
|
|
TIMER_UPDATE_SAMP_TOP_M2(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
|
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
2019-09-27 03:56:49 -07:00
|
|
|
|
}
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (((1 << get_motor_now()->m_conf->foc_hfi_samples) * 8) != get_motor_now()->m_hfi.samples) {
|
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
get_motor_now()->m_state = MC_STATE_OFF;
|
|
|
|
|
stop_pwm_hw((motor_all_state_t*)get_motor_now());
|
|
|
|
|
update_hfi_samples(get_motor_now()->m_conf->foc_hfi_samples, get_motor_now());
|
2020-01-28 10:46:19 -08:00
|
|
|
|
}
|
2020-06-06 14:21:53 -07:00
|
|
|
|
|
|
|
|
|
virtual_motor_set_configuration(configuration);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
mc_state mcpwm_foc_get_state(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_state;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-07 00:09:31 -07:00
|
|
|
|
mc_control_mode mcpwm_foc_control_mode(void) {
|
|
|
|
|
return get_motor_now()->m_control_mode;
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
bool mcpwm_foc_is_dccal_done(void) {
|
|
|
|
|
return m_dccal_done;
|
|
|
|
|
}
|
|
|
|
|
|
2020-03-16 14:42:44 -07:00
|
|
|
|
/**
|
|
|
|
|
* Get the current motor used in the mcpwm ISR
|
|
|
|
|
*
|
|
|
|
|
* @return
|
|
|
|
|
* 0: Not in ISR
|
|
|
|
|
* 1: Motor 1
|
|
|
|
|
* 2: Motor 2
|
|
|
|
|
*/
|
|
|
|
|
int mcpwm_foc_isr_motor(void) {
|
|
|
|
|
return m_isr_motor;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
/**
|
2019-03-10 06:57:42 -07:00
|
|
|
|
* Switch off all FETs.
|
2015-12-08 12:01:23 -08:00
|
|
|
|
*/
|
2020-03-16 10:32:39 -07:00
|
|
|
|
void mcpwm_foc_stop_pwm(bool is_second_motor) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
motor_all_state_t *motor = (motor_all_state_t*)M_MOTOR(is_second_motor);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
motor->m_state = MC_STATE_OFF;
|
|
|
|
|
stop_pwm_hw(motor);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Use duty cycle control. Absolute values less than MCPWM_MIN_DUTY_CYCLE will
|
|
|
|
|
* stop the motor.
|
|
|
|
|
*
|
|
|
|
|
* @param dutyCycle
|
2022-11-08 02:46:35 -08:00
|
|
|
|
* The duty cycle to use
|
2015-12-08 12:01:23 -08:00
|
|
|
|
*/
|
2022-11-08 02:46:35 -08:00
|
|
|
|
void mcpwm_foc_set_duty(float dutyCycle) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_DUTY;
|
|
|
|
|
get_motor_now()->m_duty_cycle_set = dutyCycle;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (get_motor_now()->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
get_motor_now()->m_motor_released = false;
|
|
|
|
|
get_motor_now()->m_state = MC_STATE_RUNNING;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* 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.
|
|
|
|
|
*/
|
2022-11-08 02:46:35 -08:00
|
|
|
|
void mcpwm_foc_set_duty_noramp(float dutyCycle) {
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// TODO: Actually do this without ramping
|
2022-11-08 02:46:35 -08:00
|
|
|
|
mcpwm_foc_set_duty(dutyCycle);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* 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.
|
|
|
|
|
*/
|
2022-11-08 02:46:35 -08:00
|
|
|
|
void mcpwm_foc_set_pid_speed(float rpm) {
|
2022-11-21 09:52:02 -08:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
|
|
|
|
|
|
|
|
|
if (motor->m_conf->s_pid_ramp_erpms_s > 0.0 ) {
|
|
|
|
|
if (motor->m_control_mode != CONTROL_MODE_SPEED ||
|
|
|
|
|
motor->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
motor->m_speed_pid_set_rpm = mcpwm_foc_get_rpm();
|
2020-05-12 06:53:32 -07:00
|
|
|
|
}
|
|
|
|
|
|
2022-11-21 09:52:02 -08:00
|
|
|
|
motor->m_speed_command_rpm = rpm;
|
2020-05-12 06:53:32 -07:00
|
|
|
|
} else {
|
2022-11-21 09:52:02 -08:00
|
|
|
|
motor->m_speed_pid_set_rpm = rpm;
|
2020-05-12 06:53:32 -07:00
|
|
|
|
}
|
|
|
|
|
|
2022-11-21 09:52:02 -08:00
|
|
|
|
motor->m_control_mode = CONTROL_MODE_SPEED;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-11-21 09:52:02 -08:00
|
|
|
|
if (motor->m_state != MC_STATE_RUNNING &&
|
2022-11-21 11:38:40 -08:00
|
|
|
|
fabsf(rpm) >= motor->m_conf->s_pid_min_erpm) {
|
2022-11-21 09:52:02 -08:00
|
|
|
|
motor->m_motor_released = false;
|
|
|
|
|
motor->m_state = MC_STATE_RUNNING;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Use PID position control. Note that this only works when encoder support
|
|
|
|
|
* is enabled.
|
|
|
|
|
*
|
|
|
|
|
* @param pos
|
|
|
|
|
* The desired position of the motor in degrees.
|
2022-09-27 13:52:40 -07:00
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
*/
|
|
|
|
|
void mcpwm_foc_set_pid_pos(float pos) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_POS;
|
|
|
|
|
get_motor_now()->m_pos_pid_set = pos;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (get_motor_now()->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
get_motor_now()->m_motor_released = false;
|
|
|
|
|
get_motor_now()->m_state = MC_STATE_RUNNING;
|
2022-11-08 09:13:27 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* 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.
|
|
|
|
|
*/
|
2022-11-08 02:46:35 -08:00
|
|
|
|
void mcpwm_foc_set_current(float current) {
|
2023-12-26 13:37:41 -08:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
|
|
|
|
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_CURRENT;
|
|
|
|
|
motor->m_iq_set = current;
|
|
|
|
|
motor->m_id_set = 0;
|
2021-12-23 12:25:31 -08:00
|
|
|
|
|
2023-12-26 13:37:41 -08:00
|
|
|
|
if (fabsf(current) < motor->m_conf->cc_min_current) {
|
2022-11-08 03:46:08 -08:00
|
|
|
|
return;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2023-12-26 13:37:41 -08:00
|
|
|
|
if (motor->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
motor->m_motor_released = false;
|
|
|
|
|
motor->m_state = MC_STATE_RUNNING;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2022-01-19 09:52:45 -08:00
|
|
|
|
void mcpwm_foc_release_motor(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_CURRENT;
|
|
|
|
|
get_motor_now()->m_iq_set = 0.0;
|
|
|
|
|
get_motor_now()->m_id_set = 0.0;
|
|
|
|
|
get_motor_now()->m_motor_released = true;
|
2022-01-19 09:52:45 -08:00
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
/**
|
|
|
|
|
* 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.
|
|
|
|
|
*/
|
2022-11-08 02:46:35 -08:00
|
|
|
|
void mcpwm_foc_set_brake_current(float current) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_CURRENT_BRAKE;
|
|
|
|
|
get_motor_now()->m_iq_set = current;
|
2021-04-10 02:37:35 -07:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) {
|
2022-11-08 03:46:08 -08:00
|
|
|
|
return;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (get_motor_now()->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
get_motor_now()->m_motor_released = false;
|
|
|
|
|
get_motor_now()->m_state = MC_STATE_RUNNING;
|
2022-11-08 09:13:27 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
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.
|
|
|
|
|
*/
|
2022-11-08 02:46:35 -08:00
|
|
|
|
void mcpwm_foc_set_handbrake(float current) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_HANDBRAKE;
|
|
|
|
|
get_motor_now()->m_iq_set = current;
|
2021-04-10 02:37:35 -07:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) {
|
2022-11-08 03:46:08 -08:00
|
|
|
|
return;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (get_motor_now()->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
get_motor_now()->m_motor_released = false;
|
|
|
|
|
get_motor_now()->m_state = MC_STATE_RUNNING;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Produce an openloop rotating current.
|
|
|
|
|
*
|
|
|
|
|
* @param current
|
|
|
|
|
* The current to use.
|
|
|
|
|
*
|
|
|
|
|
* @param rpm
|
|
|
|
|
* The RPM to use.
|
2022-09-27 13:52:40 -07:00
|
|
|
|
*
|
2016-11-04 07:18:34 -07:00
|
|
|
|
*/
|
2022-11-08 09:13:27 -08:00
|
|
|
|
void mcpwm_foc_set_openloop_current(float current, float rpm) {
|
|
|
|
|
utils_truncate_number(¤t, -get_motor_now()->m_conf->l_current_max * get_motor_now()->m_conf->l_current_max_scale,
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_conf->l_current_max * get_motor_now()->m_conf->l_current_max_scale);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP;
|
|
|
|
|
get_motor_now()->m_iq_set = current;
|
|
|
|
|
get_motor_now()->m_openloop_speed = RPM2RADPS_f(rpm);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) {
|
2022-11-08 09:13:27 -08:00
|
|
|
|
return;
|
2021-04-10 02:37:35 -07:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (get_motor_now()->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
get_motor_now()->m_motor_released = false;
|
|
|
|
|
get_motor_now()->m_state = MC_STATE_RUNNING;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
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]
|
|
|
|
|
*/
|
2022-11-08 02:46:35 -08:00
|
|
|
|
void mcpwm_foc_set_openloop_phase(float current, float phase) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
utils_truncate_number(¤t, -get_motor_now()->m_conf->l_current_max * get_motor_now()->m_conf->l_current_max_scale,
|
|
|
|
|
get_motor_now()->m_conf->l_current_max * get_motor_now()->m_conf->l_current_max_scale);
|
2019-02-18 10:30:19 -08:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP_PHASE;
|
|
|
|
|
get_motor_now()->m_id_set = current;
|
|
|
|
|
get_motor_now()->m_iq_set = 0;
|
2019-02-18 10:30:19 -08:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_openloop_phase = DEG2RAD_f(phase);
|
|
|
|
|
utils_norm_angle_rad((float*)&get_motor_now()->m_openloop_phase);
|
2019-02-18 10:30:19 -08:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) {
|
2022-11-08 03:46:08 -08:00
|
|
|
|
return;
|
2021-04-10 02:37:35 -07:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (get_motor_now()->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
get_motor_now()->m_motor_released = false;
|
|
|
|
|
get_motor_now()->m_state = MC_STATE_RUNNING;
|
2019-02-18 10:30:19 -08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
/**
|
|
|
|
|
* 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 float *curr0_offset,
|
|
|
|
|
volatile float *curr1_offset,
|
|
|
|
|
volatile float *curr2_offset,
|
|
|
|
|
bool is_second_motor) {
|
|
|
|
|
volatile motor_all_state_t *motor = M_MOTOR(is_second_motor);
|
|
|
|
|
*curr0_offset = motor->m_conf->foc_offsets_current[0];
|
|
|
|
|
*curr1_offset = motor->m_conf->foc_offsets_current[1];
|
|
|
|
|
*curr2_offset = motor->m_conf->foc_offsets_current[2];
|
|
|
|
|
}
|
|
|
|
|
|
2019-04-06 06:36:00 -07:00
|
|
|
|
/**
|
|
|
|
|
* Set current offsets values,
|
|
|
|
|
* this is used by the virtual motor to set the previously saved offsets back,
|
|
|
|
|
* when it is disconnected
|
|
|
|
|
*/
|
2021-02-28 11:36:02 -08:00
|
|
|
|
void mcpwm_foc_set_current_offsets(volatile float curr0_offset,
|
|
|
|
|
volatile float curr1_offset,
|
|
|
|
|
volatile float curr2_offset) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_conf->foc_offsets_current[0] = curr0_offset;
|
|
|
|
|
get_motor_now()->m_conf->foc_offsets_current[1] = curr1_offset;
|
|
|
|
|
get_motor_now()->m_conf->foc_offsets_current[2] = curr2_offset;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void mcpwm_foc_get_voltage_offsets(
|
|
|
|
|
float *v0_offset,
|
|
|
|
|
float *v1_offset,
|
|
|
|
|
float *v2_offset,
|
|
|
|
|
bool is_second_motor) {
|
|
|
|
|
volatile motor_all_state_t *motor = M_MOTOR(is_second_motor);
|
|
|
|
|
*v0_offset = motor->m_conf->foc_offsets_voltage[0];
|
|
|
|
|
*v1_offset = motor->m_conf->foc_offsets_voltage[1];
|
|
|
|
|
*v2_offset = motor->m_conf->foc_offsets_voltage[2];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void mcpwm_foc_get_voltage_offsets_undriven(
|
|
|
|
|
float *v0_offset,
|
|
|
|
|
float *v1_offset,
|
|
|
|
|
float *v2_offset,
|
|
|
|
|
bool is_second_motor) {
|
|
|
|
|
volatile motor_all_state_t *motor = M_MOTOR(is_second_motor);
|
|
|
|
|
*v0_offset = motor->m_conf->foc_offsets_voltage_undriven[0];
|
|
|
|
|
*v1_offset = motor->m_conf->foc_offsets_voltage_undriven[1];
|
|
|
|
|
*v2_offset = motor->m_conf->foc_offsets_voltage_undriven[2];
|
2019-04-06 06:36:00 -07:00
|
|
|
|
}
|
|
|
|
|
|
2023-02-07 12:40:26 -08:00
|
|
|
|
void mcpwm_foc_get_currents_adc(
|
|
|
|
|
float *ph0,
|
|
|
|
|
float *ph1,
|
|
|
|
|
float *ph2,
|
|
|
|
|
bool is_second_motor) {
|
|
|
|
|
volatile motor_all_state_t *motor = M_MOTOR(is_second_motor);
|
|
|
|
|
*ph0 = motor->m_currents_adc[0];
|
|
|
|
|
*ph1 = motor->m_currents_adc[1];
|
|
|
|
|
*ph2 = motor->m_currents_adc[2];
|
|
|
|
|
}
|
|
|
|
|
|
2019-03-31 01:49:18 -07:00
|
|
|
|
/**
|
|
|
|
|
* Produce an openloop rotating voltage.
|
|
|
|
|
*
|
|
|
|
|
* @param dutyCycle
|
|
|
|
|
* The duty cycle to use.
|
|
|
|
|
*
|
|
|
|
|
* @param rpm
|
|
|
|
|
* The RPM to use.
|
|
|
|
|
*/
|
2022-11-08 02:46:35 -08:00
|
|
|
|
void mcpwm_foc_set_openloop_duty(float dutyCycle, float rpm) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP_DUTY;
|
|
|
|
|
get_motor_now()->m_duty_cycle_set = dutyCycle;
|
|
|
|
|
get_motor_now()->m_openloop_speed = RPM2RADPS_f(rpm);
|
2019-03-31 01:49:18 -07:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (get_motor_now()->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
get_motor_now()->m_motor_released = false;
|
|
|
|
|
get_motor_now()->m_state = MC_STATE_RUNNING;
|
2022-11-08 09:13:27 -08:00
|
|
|
|
}
|
2019-03-31 01:49:18 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* 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]
|
|
|
|
|
*/
|
2022-11-08 02:46:35 -08:00
|
|
|
|
void mcpwm_foc_set_openloop_duty_phase(float dutyCycle, float phase) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP_DUTY_PHASE;
|
|
|
|
|
get_motor_now()->m_duty_cycle_set = dutyCycle;
|
|
|
|
|
get_motor_now()->m_openloop_phase = DEG2RAD_f(phase);
|
|
|
|
|
utils_norm_angle_rad((float*)&get_motor_now()->m_openloop_phase);
|
|
|
|
|
|
|
|
|
|
if (get_motor_now()->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
get_motor_now()->m_motor_released = false;
|
|
|
|
|
get_motor_now()->m_state = MC_STATE_RUNNING;
|
2019-03-31 01:49:18 -07:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
float mcpwm_foc_get_duty_cycle_set(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_duty_cycle_set;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_duty_cycle_now(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.duty_now;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2016-02-24 12:17:39 -08:00
|
|
|
|
float mcpwm_foc_get_pid_pos_set(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_pos_pid_set;
|
2016-02-24 12:17:39 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_pid_pos_now(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_pos_pid_now;
|
2016-02-24 12:17:39 -08:00
|
|
|
|
}
|
|
|
|
|
|
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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_conf->foc_f_zv;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
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) {
|
2017-09-04 12:12:43 -07:00
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (get_motor_now()->m_conf->foc_control_sample_mode == FOC_CONTROL_SAMPLE_MODE_V0_V7) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_conf->foc_f_zv;
|
2017-09-04 12:12:43 -07:00
|
|
|
|
} else {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_conf->foc_f_zv / 2.0;
|
2017-09-04 12:12:43 -07:00
|
|
|
|
}
|
2016-11-04 07:18:34 -07:00
|
|
|
|
#else
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_conf->foc_f_zv / 2.0;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
2019-04-06 06:36:00 -07:00
|
|
|
|
/**
|
|
|
|
|
* Returns Ts used for virtual motor sync
|
|
|
|
|
*/
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float mcpwm_foc_get_ts(void) {
|
2019-04-06 06:36:00 -07:00
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (get_motor_now()->m_conf->foc_control_sample_mode == FOC_CONTROL_SAMPLE_MODE_V0_V7) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return (1.0 / get_motor_now()->m_conf->foc_f_zv) ;
|
2019-04-06 06:36:00 -07:00
|
|
|
|
} else {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return (1.0 / (get_motor_now()->m_conf->foc_f_zv / 2.0));
|
2019-04-06 06:36:00 -07:00
|
|
|
|
}
|
|
|
|
|
#else
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return (1.0 / get_motor_now()->m_conf->foc_f_zv) ;
|
2019-04-06 06:36:00 -07:00
|
|
|
|
#endif
|
2019-09-03 11:39:05 -07:00
|
|
|
|
}
|
2019-04-06 06:36:00 -07:00
|
|
|
|
|
2019-09-03 11:39:05 -07:00
|
|
|
|
bool mcpwm_foc_is_using_encoder(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_using_encoder;
|
2019-04-06 06:36:00 -07:00
|
|
|
|
}
|
2019-09-03 11:39:05 -07:00
|
|
|
|
|
2021-03-13 02:42:23 -08:00
|
|
|
|
void mcpwm_foc_get_observer_state(float *x1, float *x2) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
2022-05-21 13:54:12 -07:00
|
|
|
|
*x1 = motor->m_observer_state.x1;
|
|
|
|
|
*x2 = motor->m_observer_state.x2;
|
2021-03-13 02:42:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2021-04-14 13:29:50 -07:00
|
|
|
|
/**
|
|
|
|
|
* Set current off delay. Prevent the current controller from switching off modulation
|
|
|
|
|
* for target currents < cc_min_current for this amount of time.
|
|
|
|
|
*/
|
|
|
|
|
void mcpwm_foc_set_current_off_delay(float delay_sec) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (get_motor_now()->m_current_off_delay < delay_sec) {
|
|
|
|
|
get_motor_now()->m_current_off_delay = delay_sec;
|
2021-04-14 13:29:50 -07:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2020-03-20 13:48:21 -07:00
|
|
|
|
float mcpwm_foc_get_tot_current_motor(bool is_second_motor) {
|
2020-10-27 10:44:58 -07:00
|
|
|
|
volatile motor_all_state_t *motor = M_MOTOR(is_second_motor);
|
2020-07-21 00:23:48 -07:00
|
|
|
|
return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq) * motor->m_motor_state.i_abs;
|
2020-03-20 13:48:21 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_tot_current_filtered_motor(bool is_second_motor) {
|
2020-10-27 10:44:58 -07:00
|
|
|
|
volatile motor_all_state_t *motor = M_MOTOR(is_second_motor);
|
2020-07-21 00:23:48 -07:00
|
|
|
|
return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq_filter) * motor->m_motor_state.i_abs_filter;
|
2020-03-20 13:48:21 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_tot_current_in_motor(bool is_second_motor) {
|
2020-10-27 10:44:58 -07:00
|
|
|
|
return M_MOTOR(is_second_motor)->m_motor_state.i_bus;
|
2020-03-20 13:48:21 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_tot_current_in_filtered_motor(bool is_second_motor) {
|
|
|
|
|
// TODO: Filter current?
|
2020-10-27 10:44:58 -07:00
|
|
|
|
return M_MOTOR(is_second_motor)->m_motor_state.i_bus;
|
2020-03-20 13:48:21 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_abs_motor_current_motor(bool is_second_motor) {
|
2020-10-27 10:44:58 -07:00
|
|
|
|
return M_MOTOR(is_second_motor)->m_motor_state.i_abs;
|
2020-03-20 13:48:21 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_abs_motor_current_filtered_motor(bool is_second_motor) {
|
2020-10-27 10:44:58 -07:00
|
|
|
|
return M_MOTOR(is_second_motor)->m_motor_state.i_abs_filter;
|
2020-03-20 13:48:21 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
mc_state mcpwm_foc_get_state_motor(bool is_second_motor) {
|
2020-10-27 10:44:58 -07:00
|
|
|
|
return M_MOTOR(is_second_motor)->m_state;
|
2020-03-20 13:48:21 -07:00
|
|
|
|
}
|
|
|
|
|
|
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) {
|
2023-07-01 05:51:08 -07:00
|
|
|
|
return RADPS2RPM_f(get_motor_now()->m_pll_speed);
|
2022-04-06 07:33:44 -07:00
|
|
|
|
// return get_motor_now()->m_speed_est_fast * RADPS2RPM_f;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2020-04-16 00:12:08 -07:00
|
|
|
|
/**
|
|
|
|
|
* Same as above, but uses the fast and noisier estimator.
|
|
|
|
|
*/
|
|
|
|
|
float mcpwm_foc_get_rpm_fast(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return RADPS2RPM_f(get_motor_now()->m_speed_est_fast);
|
2020-04-16 00:12:08 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Same as above, but uses the faster and noisier estimator.
|
|
|
|
|
*/
|
|
|
|
|
float mcpwm_foc_get_rpm_faster(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return RADPS2RPM_f(get_motor_now()->m_speed_est_faster);
|
2020-04-16 00:12:08 -07:00
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
/**
|
|
|
|
|
* 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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
2020-07-21 00:23:48 -07:00
|
|
|
|
return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq) * motor->m_motor_state.i_abs;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
2015-12-23 15:43:31 -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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
2020-07-21 00:23:48 -07:00
|
|
|
|
return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq_filter) * motor->m_motor_state.i_abs_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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.i_abs;
|
2015-12-23 15:43:31 -08:00
|
|
|
|
}
|
|
|
|
|
|
2019-04-20 05:24:32 -07:00
|
|
|
|
/**
|
|
|
|
|
* 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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_curr_unbalance * FAC_CURRENT;
|
2019-04-20 05:24:32 -07:00
|
|
|
|
}
|
|
|
|
|
|
2016-06-27 08:29:09 -07:00
|
|
|
|
/**
|
|
|
|
|
* Get the magnitude of the motor voltage.
|
|
|
|
|
*
|
|
|
|
|
* @return
|
|
|
|
|
* The magnitude of the motor voltage.
|
|
|
|
|
*/
|
|
|
|
|
float mcpwm_foc_get_abs_motor_voltage(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
const float vd_tmp = get_motor_now()->m_motor_state.vd;
|
|
|
|
|
const float vq_tmp = get_motor_now()->m_motor_state.vq;
|
2022-03-07 08:33:51 -08:00
|
|
|
|
return NORM2_f(vd_tmp, vq_tmp);
|
2016-06-27 08:29:09 -07:00
|
|
|
|
}
|
|
|
|
|
|
2015-12-23 15:43:31 -08:00
|
|
|
|
/**
|
|
|
|
|
* 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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.id;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Get the quadrature axis motor current.
|
|
|
|
|
*
|
|
|
|
|
* @return
|
|
|
|
|
* The Q axis current.
|
|
|
|
|
*/
|
|
|
|
|
float mcpwm_foc_get_iq(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.iq;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
}
|
|
|
|
|
|
2023-10-15 11:56:49 -07:00
|
|
|
|
float mcpwm_foc_get_id_set(void) {
|
|
|
|
|
return get_motor_now()->m_id_set;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_iq_set(void) {
|
|
|
|
|
return get_motor_now()->m_iq_set;
|
|
|
|
|
}
|
|
|
|
|
|
2022-10-23 09:35:26 -07:00
|
|
|
|
/**
|
|
|
|
|
* Get the filtered direct axis motor current.
|
|
|
|
|
*
|
|
|
|
|
* @return
|
|
|
|
|
* The D axis current.
|
|
|
|
|
*/
|
|
|
|
|
float mcpwm_foc_get_id_filter(void) {
|
|
|
|
|
return get_motor_now()->m_motor_state.id_filter;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Get the filtered quadrature axis motor current.
|
|
|
|
|
*
|
|
|
|
|
* @return
|
|
|
|
|
* The Q axis current.
|
|
|
|
|
*/
|
|
|
|
|
float mcpwm_foc_get_iq_filter(void) {
|
|
|
|
|
return get_motor_now()->m_motor_state.iq_filter;
|
|
|
|
|
}
|
|
|
|
|
|
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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.i_bus;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
2015-12-23 15:43:31 -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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.i_bus; // TODO: Calculate filtered current?
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2019-12-07 08:07:05 -08:00
|
|
|
|
/**
|
|
|
|
|
* Set the number of steps the motor has rotated. This number is signed and
|
|
|
|
|
* becomes a negative when the motor is rotating backwards.
|
|
|
|
|
*
|
|
|
|
|
* @param steps
|
|
|
|
|
* New number of steps will be set after this call.
|
|
|
|
|
*
|
|
|
|
|
* @return
|
|
|
|
|
* The previous tachometer value in motor steps. The number of motor revolutions will
|
|
|
|
|
* be this number divided by (3 * MOTOR_POLE_NUMBER).
|
|
|
|
|
*/
|
2020-03-16 10:32:39 -07:00
|
|
|
|
int mcpwm_foc_set_tachometer_value(int steps) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
int val = get_motor_now()->m_tachometer;
|
|
|
|
|
get_motor_now()->m_tachometer = steps;
|
2019-12-07 08:07:05 -08:00
|
|
|
|
return val;
|
|
|
|
|
}
|
|
|
|
|
|
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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
int val = get_motor_now()->m_tachometer;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
if (reset) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_tachometer = 0;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
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) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
int val = get_motor_now()->m_tachometer_abs;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
if (reset) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_tachometer_abs = 0;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return val;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_phase(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
float angle = RAD2DEG_f(get_motor_now()->m_motor_state.phase);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
utils_norm_angle(&angle);
|
|
|
|
|
return angle;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_phase_observer(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
float angle = RAD2DEG_f(get_motor_now()->m_phase_now_observer);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
utils_norm_angle(&angle);
|
|
|
|
|
return angle;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_phase_encoder(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
float angle = RAD2DEG_f(get_motor_now()->m_phase_now_encoder);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
utils_norm_angle(&angle);
|
|
|
|
|
return angle;
|
|
|
|
|
}
|
|
|
|
|
|
2023-08-08 10:56:48 -07:00
|
|
|
|
float mcpwm_foc_get_phase_hall(void) {
|
|
|
|
|
float angle = RAD2DEG_f(get_motor_now()->m_ang_hall_rate_limited);
|
|
|
|
|
utils_norm_angle(&angle);
|
|
|
|
|
return angle;
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-19 12:24:46 -08:00
|
|
|
|
float mcpwm_foc_get_vd(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.vd;
|
2015-12-19 12:24:46 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_vq(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.vq;
|
2015-12-19 12:24:46 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-02-06 14:12:05 -08:00
|
|
|
|
float mcpwm_foc_get_mod_alpha_raw(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.mod_alpha_raw;
|
2022-02-06 14:12:05 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_mod_beta_raw(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.mod_beta_raw;
|
2022-02-06 14:12:05 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_mod_alpha_measured(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.mod_alpha_measured;
|
2022-02-06 14:12:05 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_mod_beta_measured(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
return get_motor_now()->m_motor_state.mod_beta_measured;
|
2022-02-06 14:12:05 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-12-18 15:14:57 -08:00
|
|
|
|
float mcpwm_foc_get_est_lambda(void) {
|
|
|
|
|
return get_motor_now()->m_observer_state.lambda_est;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float mcpwm_foc_get_est_res(void) {
|
|
|
|
|
return get_motor_now()->m_res_est;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// NOTE: Requires the regular HFI sensor mode to run
|
|
|
|
|
float mcpwm_foc_get_est_ind(void) {
|
|
|
|
|
float real_bin0, imag_bin0;
|
|
|
|
|
get_motor_now()->m_hfi.fft_bin0_func((float*)get_motor_now()->m_hfi.buffer, &real_bin0, &imag_bin0);
|
|
|
|
|
return real_bin0;
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
/**
|
|
|
|
|
* Measure encoder offset and direction.
|
|
|
|
|
*
|
|
|
|
|
* @param current
|
|
|
|
|
* The locking open loop current for the motor.
|
|
|
|
|
*
|
2021-05-07 15:27:50 -07:00
|
|
|
|
* @param print
|
|
|
|
|
* Controls logging during the detection procedure. Set to true to enable
|
|
|
|
|
* logging.
|
|
|
|
|
*
|
2015-12-08 12:01:23 -08:00
|
|
|
|
* @param offset
|
|
|
|
|
* The detected offset.
|
|
|
|
|
*
|
|
|
|
|
* @param ratio
|
|
|
|
|
* The ratio between electrical and mechanical revolutions
|
|
|
|
|
*
|
|
|
|
|
* @param direction
|
|
|
|
|
* The detected direction.
|
2021-05-07 15:27:50 -07:00
|
|
|
|
*
|
|
|
|
|
* @param inverted
|
|
|
|
|
* Is set to true if the encoder reports an increase in angle in the opposite
|
|
|
|
|
* direction of the motor.
|
2022-09-27 13:52:40 -07:00
|
|
|
|
*
|
|
|
|
|
* @return
|
|
|
|
|
* The fault code
|
2015-12-08 12:01:23 -08:00
|
|
|
|
*/
|
2022-09-27 13:52:40 -07:00
|
|
|
|
int mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ratio, bool *inverted) {
|
2022-11-08 02:46:35 -08:00
|
|
|
|
int fault = FAULT_CODE_NONE;
|
|
|
|
|
mc_interface_lock();
|
2016-02-02 08:19:37 -08:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
motor->m_phase_override = true;
|
|
|
|
|
motor->m_id_set = current;
|
|
|
|
|
motor->m_iq_set = 0.0;
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_CURRENT;
|
2022-01-19 09:52:45 -08:00
|
|
|
|
motor->m_motor_released = false;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_state = MC_STATE_RUNNING;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Disable timeout
|
|
|
|
|
systime_t tout = timeout_get_timeout_msec();
|
|
|
|
|
float tout_c = timeout_get_brake_current();
|
2021-06-13 04:46:27 -07:00
|
|
|
|
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
|
2016-11-04 07:18:34 -07:00
|
|
|
|
timeout_reset();
|
2021-06-13 04:46:27 -07:00
|
|
|
|
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Save configuration
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float offset_old = motor->m_conf->foc_encoder_offset;
|
|
|
|
|
float inverted_old = motor->m_conf->foc_encoder_inverted;
|
|
|
|
|
float ratio_old = motor->m_conf->foc_encoder_ratio;
|
2020-05-16 14:39:58 -07:00
|
|
|
|
float ldiff_old = motor->m_conf->foc_motor_ld_lq_diff;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_conf->foc_encoder_offset = 0.0;
|
|
|
|
|
motor->m_conf->foc_encoder_inverted = false;
|
|
|
|
|
motor->m_conf->foc_encoder_ratio = 1.0;
|
2020-05-16 14:39:58 -07:00
|
|
|
|
motor->m_conf->foc_motor_ld_lq_diff = 0.0;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Find index
|
|
|
|
|
int cnt = 0;
|
2022-01-09 08:10:40 -08:00
|
|
|
|
while(!encoder_index_found()) {
|
2015-12-08 12:01:23 -08:00
|
|
|
|
for (float i = 0.0;i < 2.0 * M_PI;i += (2.0 * M_PI) / 500.0) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_phase_now_override = i;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_encoder_detect;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
}
|
|
|
|
|
chThdSleepMilliseconds(1);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
cnt++;
|
|
|
|
|
if (cnt > 30) {
|
|
|
|
|
// Give up
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-27 16:50:23 -08:00
|
|
|
|
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) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_phase_now_override = i;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_encoder_detect;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
}
|
2015-12-27 16:50:23 -08:00
|
|
|
|
|
|
|
|
|
if (print) {
|
|
|
|
|
commands_printf("Rotated for sync");
|
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Inverted and ratio
|
|
|
|
|
chThdSleepMilliseconds(1000);
|
|
|
|
|
|
2022-01-25 04:57:25 -08:00
|
|
|
|
const int it_rat = 30;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
float s_sum = 0.0;
|
|
|
|
|
float c_sum = 0.0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float first = motor->m_phase_now_encoder;
|
2016-04-27 06:32:32 -07:00
|
|
|
|
|
|
|
|
|
for (int i = 0; i < it_rat; i++) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float phase_old = motor->m_phase_now_encoder;
|
|
|
|
|
float phase_ovr_tmp = motor->m_phase_now_override;
|
2019-01-28 11:52:02 -08:00
|
|
|
|
for (float j = phase_ovr_tmp; j < phase_ovr_tmp + (2.0 / 3.0) * M_PI;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
j += (2.0 * M_PI) / 500.0) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_phase_now_override = j;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_encoder_detect;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
}
|
2016-04-27 06:32:32 -07:00
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
}
|
2022-09-27 13:52:40 -07:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
utils_norm_angle_rad((float*)&motor->m_phase_now_override);
|
2016-04-27 06:32:32 -07:00
|
|
|
|
chThdSleepMilliseconds(300);
|
2022-01-25 04:57:25 -08:00
|
|
|
|
timeout_reset();
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float diff = utils_angle_difference_rad(motor->m_phase_now_encoder, phase_old);
|
2016-04-27 06:32:32 -07:00
|
|
|
|
|
|
|
|
|
float s, c;
|
|
|
|
|
sincosf(diff, &s, &c);
|
|
|
|
|
s_sum += s;
|
|
|
|
|
c_sum += c;
|
|
|
|
|
|
|
|
|
|
if (print) {
|
2022-01-25 04:57:25 -08:00
|
|
|
|
commands_printf("Diff: %.2f", (double)RAD2DEG_f(diff));
|
2016-04-27 06:32:32 -07:00
|
|
|
|
}
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (i > 3 && fabsf(utils_angle_difference_rad(motor->m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
|
2016-04-27 06:32:32 -07:00
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
first = motor->m_phase_now_encoder;
|
2016-04-27 06:32:32 -07:00
|
|
|
|
|
|
|
|
|
for (int i = 0; i < it_rat; i++) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float phase_old = motor->m_phase_now_encoder;
|
|
|
|
|
float phase_ovr_tmp = motor->m_phase_now_override;
|
2022-11-08 09:13:27 -08:00
|
|
|
|
for (float j = phase_ovr_tmp; j > phase_ovr_tmp - (2.0 / 3.0) * M_PI; j -= (2.0 * M_PI) / 500.0) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_phase_now_override = j;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_encoder_detect;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
}
|
2016-04-27 06:32:32 -07:00
|
|
|
|
chThdSleepMilliseconds(1);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
2020-03-16 10:32:39 -07:00
|
|
|
|
utils_norm_angle_rad((float*)&motor->m_phase_now_override);
|
2016-04-27 06:32:32 -07:00
|
|
|
|
chThdSleepMilliseconds(300);
|
2022-01-25 04:57:25 -08:00
|
|
|
|
timeout_reset();
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float diff = utils_angle_difference_rad(phase_old, motor->m_phase_now_encoder);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
float s, c;
|
2016-04-27 06:32:32 -07:00
|
|
|
|
sincosf(diff, &s, &c);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
s_sum += s;
|
|
|
|
|
c_sum += c;
|
2015-12-27 16:50:23 -08:00
|
|
|
|
|
|
|
|
|
if (print) {
|
2022-01-25 04:57:25 -08:00
|
|
|
|
commands_printf("Diff: %.2f", (double)RAD2DEG_f(diff));
|
2016-04-27 06:32:32 -07:00
|
|
|
|
}
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (i > 3 && fabsf(utils_angle_difference_rad(motor->m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
|
2016-04-27 06:32:32 -07:00
|
|
|
|
break;
|
2015-12-27 16:50:23 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2021-10-13 09:13:51 -07:00
|
|
|
|
float diff = RAD2DEG_f(atan2f(s_sum, c_sum));
|
2015-12-08 12:01:23 -08:00
|
|
|
|
*inverted = diff < 0.0;
|
2020-03-29 08:00:43 -07:00
|
|
|
|
*ratio = roundf(((2.0 / 3.0) * 180.0) / fabsf(diff));
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_conf->foc_encoder_inverted = *inverted;
|
|
|
|
|
motor->m_conf->foc_encoder_ratio = *ratio;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2015-12-27 16:50:23 -08:00
|
|
|
|
if (print) {
|
|
|
|
|
commands_printf("Inversion and ratio detected");
|
2022-01-25 04:57:25 -08:00
|
|
|
|
commands_printf("Ratio: %.2f", (double)*ratio);
|
2015-12-27 16:50:23 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Rotate
|
2020-03-16 10:32:39 -07:00
|
|
|
|
for (float i = motor->m_phase_now_override;i < 2.0 * M_PI;i += (2.0 * M_PI) / 500.0) {
|
|
|
|
|
motor->m_phase_now_override = i;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_encoder_detect;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
chThdSleepMilliseconds(2);
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-27 16:50:23 -08:00
|
|
|
|
if (print) {
|
|
|
|
|
commands_printf("Rotated for sync");
|
2022-01-09 08:10:40 -08:00
|
|
|
|
commands_printf("Enc: %.2f", (double)encoder_read_deg());
|
2015-12-27 16:50:23 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
const int it_ofs = motor->m_conf->foc_encoder_ratio * 3.0;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
s_sum = 0.0;
|
|
|
|
|
c_sum = 0.0;
|
|
|
|
|
|
2016-04-27 06:32:32 -07:00
|
|
|
|
for (int i = 0;i < it_ofs;i++) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float step = (2.0 * M_PI * motor->m_conf->foc_encoder_ratio) / ((float)it_ofs);
|
2019-02-18 10:30:19 -08:00
|
|
|
|
float override = (float)i * step;
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
while (motor->m_phase_now_override != override) {
|
|
|
|
|
utils_step_towards((float*)&motor->m_phase_now_override, override, step / 100.0);
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_encoder_detect;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
}
|
|
|
|
|
chThdSleepMilliseconds(4);
|
2019-02-18 10:30:19 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
chThdSleepMilliseconds(100);
|
2022-01-25 04:57:25 -08:00
|
|
|
|
timeout_reset();
|
2016-04-27 06:32:32 -07:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float angle_diff = utils_angle_difference_rad(motor->m_phase_now_encoder, motor->m_phase_now_override);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
float s, c;
|
2019-01-28 11:52:02 -08:00
|
|
|
|
sincosf(angle_diff, &s, &c);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
s_sum += s;
|
|
|
|
|
c_sum += c;
|
2015-12-27 16:50:23 -08:00
|
|
|
|
|
|
|
|
|
if (print) {
|
2022-01-25 04:57:25 -08:00
|
|
|
|
commands_printf("Ovr: %.2f/%.2f Diff: %.2f", (double)override, (double)(it_ofs * step), (double)RAD2DEG_f(angle_diff));
|
2015-12-27 16:50:23 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2016-04-27 06:32:32 -07:00
|
|
|
|
for (int i = it_ofs;i > 0;i--) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float step = (2.0 * M_PI * motor->m_conf->foc_encoder_ratio) / ((float)it_ofs);
|
2019-02-18 10:30:19 -08:00
|
|
|
|
float override = (float)i * step;
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
while (motor->m_phase_now_override != override) {
|
|
|
|
|
utils_step_towards((float*)&motor->m_phase_now_override, override, step / 100.0);
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_encoder_detect;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
}
|
|
|
|
|
chThdSleepMilliseconds(4);
|
2019-02-18 10:30:19 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
chThdSleepMilliseconds(100);
|
2022-01-25 04:57:25 -08:00
|
|
|
|
timeout_reset();
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float angle_diff = utils_angle_difference_rad(motor->m_phase_now_encoder, motor->m_phase_now_override);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
float s, c;
|
2019-01-28 11:52:02 -08:00
|
|
|
|
sincosf(angle_diff, &s, &c);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
s_sum += s;
|
|
|
|
|
c_sum += c;
|
2015-12-27 16:50:23 -08:00
|
|
|
|
|
|
|
|
|
if (print) {
|
2022-01-25 04:57:25 -08:00
|
|
|
|
commands_printf("Ovr: %.2f/%.2f Diff: %.2f", (double)override, (double)(it_ofs * step), (double)RAD2DEG_f(angle_diff));
|
2015-12-27 16:50:23 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2021-10-13 09:13:51 -07:00
|
|
|
|
*offset = RAD2DEG_f(atan2f(s_sum, c_sum));
|
2015-12-27 16:50:23 -08:00
|
|
|
|
|
|
|
|
|
if (print) {
|
|
|
|
|
commands_printf("Avg: %.2f", (double)*offset);
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
utils_norm_angle(offset);
|
2015-12-27 16:50:23 -08:00
|
|
|
|
|
|
|
|
|
if (print) {
|
|
|
|
|
commands_printf("Offset detected");
|
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-11-12 01:44:50 -08:00
|
|
|
|
exit_encoder_detect:
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_id_set = 0.0;
|
|
|
|
|
motor->m_iq_set = 0.0;
|
|
|
|
|
motor->m_phase_override = false;
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
motor->m_state = MC_STATE_OFF;
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)motor);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Restore configuration
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_conf->foc_encoder_inverted = inverted_old;
|
|
|
|
|
motor->m_conf->foc_encoder_offset = offset_old;
|
|
|
|
|
motor->m_conf->foc_encoder_ratio = ratio_old;
|
2020-05-16 14:39:58 -07:00
|
|
|
|
motor->m_conf->foc_motor_ld_lq_diff = ldiff_old;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Enable timeout
|
2021-06-13 04:46:27 -07:00
|
|
|
|
timeout_configure(tout, tout_c, tout_ksw);
|
2016-02-02 08:19:37 -08:00
|
|
|
|
|
|
|
|
|
mc_interface_unlock();
|
2022-11-08 02:46:35 -08:00
|
|
|
|
return fault;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
2021-03-13 02:42:23 -08:00
|
|
|
|
* Lock the motor with a current and sample the voltage and current to
|
2015-12-08 12:01:23 -08:00
|
|
|
|
* calculate the motor resistance.
|
|
|
|
|
*
|
|
|
|
|
* @param current
|
|
|
|
|
* The locking current.
|
|
|
|
|
*
|
|
|
|
|
* @param samples
|
|
|
|
|
* The number of samples to take.
|
|
|
|
|
*
|
2020-04-02 11:43:11 -07:00
|
|
|
|
* @param stop_after
|
|
|
|
|
* Stop motor after finishing the measurement. Otherwise, the current will
|
|
|
|
|
* still be applied after returning. Setting this to false is useful if you want
|
|
|
|
|
* to run this function again right away, without stopping the motor in between.
|
|
|
|
|
*
|
2022-09-27 13:52:40 -07:00
|
|
|
|
* @param resistance
|
|
|
|
|
* The calculated motor resistance
|
|
|
|
|
*
|
2015-12-08 12:01:23 -08:00
|
|
|
|
* @return
|
2022-09-27 13:52:40 -07:00
|
|
|
|
* The fault code.
|
2015-12-08 12:01:23 -08:00
|
|
|
|
*/
|
2022-09-27 13:52:40 -07:00
|
|
|
|
int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, float *resistance) {
|
2015-12-08 12:01:23 -08:00
|
|
|
|
mc_interface_lock();
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
2022-11-08 02:46:35 -08:00
|
|
|
|
int fault = FAULT_CODE_NONE;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
motor->m_phase_override = true;
|
|
|
|
|
motor->m_phase_now_override = 0.0;
|
|
|
|
|
motor->m_id_set = 0.0;
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_CURRENT;
|
2022-01-19 09:52:45 -08:00
|
|
|
|
motor->m_motor_released = false;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_state = MC_STATE_RUNNING;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Disable timeout
|
|
|
|
|
systime_t tout = timeout_get_timeout_msec();
|
|
|
|
|
float tout_c = timeout_get_brake_current();
|
2021-06-13 04:46:27 -07:00
|
|
|
|
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
|
2016-11-04 07:18:34 -07:00
|
|
|
|
timeout_reset();
|
2021-06-13 04:46:27 -07:00
|
|
|
|
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-04-03 15:58:42 -07:00
|
|
|
|
// Ramp up the current slowly
|
2020-04-03 16:02:53 -07:00
|
|
|
|
while (fabsf(motor->m_iq_set - current) > 0.001) {
|
2022-11-08 08:37:21 -08:00
|
|
|
|
utils_step_towards((float*)&motor->m_iq_set, current, fabsf(current) / 200.0);
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-05-04 11:41:09 -07:00
|
|
|
|
motor->m_id_set = 0.0;
|
|
|
|
|
motor->m_iq_set = 0.0;
|
|
|
|
|
motor->m_phase_override = false;
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
motor->m_state = MC_STATE_OFF;
|
|
|
|
|
stop_pwm_hw((motor_all_state_t*)motor);
|
|
|
|
|
|
|
|
|
|
timeout_configure(tout, tout_c, tout_ksw);
|
|
|
|
|
mc_interface_unlock();
|
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
return fault;
|
2022-05-04 11:41:09 -07:00
|
|
|
|
}
|
2020-04-03 15:58:42 -07:00
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Wait for the current to rise and the motor to lock.
|
2022-10-02 00:45:56 -07:00
|
|
|
|
chThdSleepMilliseconds(50);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Sample
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_samples.avg_current_tot = 0.0;
|
|
|
|
|
motor->m_samples.avg_voltage_tot = 0.0;
|
|
|
|
|
motor->m_samples.sample_num = 0;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
int cnt = 0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
while (motor->m_samples.sample_num < samples) {
|
2015-12-08 12:01:23 -08:00
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
cnt++;
|
|
|
|
|
// Timeout
|
|
|
|
|
if (cnt > 10000) {
|
|
|
|
|
break;
|
|
|
|
|
}
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_id_set = 0.0;
|
|
|
|
|
motor->m_iq_set = 0.0;
|
|
|
|
|
motor->m_phase_override = false;
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
motor->m_state = MC_STATE_OFF;
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)motor);
|
2019-02-18 10:30:19 -08:00
|
|
|
|
|
2021-06-13 04:46:27 -07:00
|
|
|
|
timeout_configure(tout, tout_c, tout_ksw);
|
2019-02-18 10:30:19 -08:00
|
|
|
|
mc_interface_unlock();
|
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
return fault;
|
2019-02-18 10:30:19 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
const float current_avg = motor->m_samples.avg_current_tot / (float)motor->m_samples.sample_num;
|
|
|
|
|
const float voltage_avg = motor->m_samples.avg_voltage_tot / (float)motor->m_samples.sample_num;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Stop
|
2020-04-02 11:43:11 -07:00
|
|
|
|
if (stop_after) {
|
|
|
|
|
motor->m_id_set = 0.0;
|
|
|
|
|
motor->m_iq_set = 0.0;
|
|
|
|
|
motor->m_phase_override = false;
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
motor->m_state = MC_STATE_OFF;
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)motor);
|
2020-04-02 11:43:11 -07:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Enable timeout
|
2021-06-13 04:46:27 -07:00
|
|
|
|
timeout_configure(tout, tout_c, tout_ksw);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
mc_interface_unlock();
|
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
*resistance = voltage_avg / current_avg;
|
2022-09-27 13:52:40 -07:00
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
return fault;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
2015-12-23 15:43:31 -08:00
|
|
|
|
|
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.
|
|
|
|
|
*
|
2022-09-27 13:52:40 -07:00
|
|
|
|
* @inductance
|
2020-01-28 10:46:19 -08:00
|
|
|
|
* The average d and q axis inductance in uH.
|
2022-09-27 13:52:40 -07:00
|
|
|
|
*
|
|
|
|
|
* @return
|
|
|
|
|
* The fault code
|
2015-12-08 12:01:23 -08:00
|
|
|
|
*/
|
2022-09-27 13:52:40 -07:00
|
|
|
|
int mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float *ld_lq_diff, float *inductance) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
2022-11-08 02:46:35 -08:00
|
|
|
|
int fault = FAULT_CODE_NONE;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
2021-12-20 23:35:25 -08:00
|
|
|
|
mc_foc_sensor_mode sensor_mode_old = motor->m_conf->foc_sensor_mode;
|
2022-01-06 11:44:00 -08:00
|
|
|
|
float f_zv_old = motor->m_conf->foc_f_zv;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float hfi_voltage_start_old = motor->m_conf->foc_hfi_voltage_start;
|
|
|
|
|
float hfi_voltage_run_old = motor->m_conf->foc_hfi_voltage_run;
|
|
|
|
|
float hfi_voltage_max_old = motor->m_conf->foc_hfi_voltage_max;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
float sl_erpm_hfi_old = motor->m_conf->foc_sl_erpm_hfi;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
mc_foc_control_sample_mode foc_control_sample_mode_old = motor->m_conf->foc_control_sample_mode;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
foc_hfi_samples samples_old = motor->m_conf->foc_hfi_samples;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
mc_foc_current_sample_mode foc_current_sample_mode_old = motor->m_conf->foc_current_sample_mode;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
mc_interface_lock();
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
motor->m_state = MC_STATE_OFF;
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)motor);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
motor->m_conf->foc_sensor_mode = FOC_SENSOR_MODE_HFI;
|
2021-10-24 04:25:12 -07:00
|
|
|
|
motor->m_conf->foc_hfi_voltage_start = duty * mc_interface_get_input_voltage_filtered() * (2.0 / 3.0) * SQRT3_BY_2;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_run = duty * mc_interface_get_input_voltage_filtered() * (2.0 / 3.0) * SQRT3_BY_2;
|
2022-02-21 00:25:46 -08:00
|
|
|
|
motor->m_conf->foc_hfi_voltage_max = duty * mc_interface_get_input_voltage_filtered() * (2.0 / 3.0) * SQRT3_BY_2;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
motor->m_conf->foc_sl_erpm_hfi = 20000.0;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
motor->m_conf->foc_control_sample_mode = FOC_CONTROL_SAMPLE_MODE_V0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_conf->foc_hfi_samples = HFI_SAMPLES_32;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
motor->m_conf->foc_current_sample_mode = FOC_CURRENT_SAMPLE_MODE_LONGEST_ZERO;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
2022-01-06 11:44:00 -08:00
|
|
|
|
if (motor->m_conf->foc_f_zv > 30.0e3) {
|
|
|
|
|
motor->m_conf->foc_f_zv = 30.0e3;
|
2021-10-15 13:25:07 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
mcpwm_foc_set_configuration(motor->m_conf);
|
2019-02-18 10:30:19 -08:00
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
chThdSleepMilliseconds(1);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
timeout_reset();
|
|
|
|
|
mcpwm_foc_set_duty(0.0);
|
|
|
|
|
chThdSleepMilliseconds(1);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
int ready_cnt = 0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
while (!motor->m_hfi.ready) {
|
2020-01-28 10:46:19 -08:00
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
ready_cnt++;
|
|
|
|
|
if (ready_cnt > 100) {
|
2015-12-08 12:01:23 -08:00
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
if (samples < 10) {
|
|
|
|
|
samples = 10;
|
|
|
|
|
}
|
2019-02-18 10:30:19 -08:00
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
float l_sum = 0.0;
|
|
|
|
|
float ld_lq_diff_sum = 0.0;
|
|
|
|
|
float i_sum = 0.0;
|
|
|
|
|
float iterations = 0.0;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
for (int i = 0;i < (samples / 10);i++) {
|
2022-09-27 13:52:40 -07:00
|
|
|
|
timeout_reset();
|
|
|
|
|
mcpwm_foc_set_duty(0.0);
|
2020-04-02 07:26:59 -07:00
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
|
|
|
|
motor->m_id_set = 0.0;
|
|
|
|
|
motor->m_iq_set = 0.0;
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
motor->m_state = MC_STATE_OFF;
|
|
|
|
|
stop_pwm_hw((motor_all_state_t*)motor);
|
2020-04-02 07:26:59 -07:00
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
motor->m_conf->foc_sensor_mode = sensor_mode_old;
|
|
|
|
|
motor->m_conf->foc_f_zv = f_zv_old;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_start = hfi_voltage_start_old;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_run = hfi_voltage_run_old;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_max = hfi_voltage_max_old;
|
|
|
|
|
motor->m_conf->foc_sl_erpm_hfi = sl_erpm_hfi_old;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
motor->m_conf->foc_control_sample_mode = foc_control_sample_mode_old;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
motor->m_conf->foc_hfi_samples = samples_old;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
motor->m_conf->foc_current_sample_mode = foc_current_sample_mode_old;
|
2020-04-02 07:26:59 -07:00
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
mcpwm_foc_set_configuration(motor->m_conf);
|
2020-04-02 07:26:59 -07:00
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
mc_interface_unlock();
|
2022-09-27 13:52:40 -07:00
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
return fault;
|
|
|
|
|
}
|
2020-04-02 07:26:59 -07:00
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
chThdSleepMilliseconds(10);
|
|
|
|
|
|
|
|
|
|
float real_bin0, imag_bin0;
|
|
|
|
|
float real_bin2, imag_bin2;
|
|
|
|
|
float real_bin0_i, imag_bin0_i;
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_hfi.fft_bin0_func((float*)motor->m_hfi.buffer, &real_bin0, &imag_bin0);
|
|
|
|
|
motor->m_hfi.fft_bin2_func((float*)motor->m_hfi.buffer, &real_bin2, &imag_bin2);
|
|
|
|
|
motor->m_hfi.fft_bin0_func((float*)motor->m_hfi.buffer_current, &real_bin0_i, &imag_bin0_i);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
|
|
|
|
l_sum += real_bin0;
|
|
|
|
|
i_sum += real_bin0_i;
|
|
|
|
|
|
2021-10-25 03:39:54 -07:00
|
|
|
|
// See https://vesc-project.com/comment/8338#comment-8338
|
2022-03-07 08:33:51 -08:00
|
|
|
|
ld_lq_diff_sum += 4.0 * NORM2_f(real_bin2, imag_bin2);
|
2021-10-25 03:39:54 -07:00
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
iterations++;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
mcpwm_foc_set_current(0.0);
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_conf->foc_sensor_mode = sensor_mode_old;
|
2022-01-06 11:44:00 -08:00
|
|
|
|
motor->m_conf->foc_f_zv = f_zv_old;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_conf->foc_hfi_voltage_start = hfi_voltage_start_old;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_run = hfi_voltage_run_old;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_max = hfi_voltage_max_old;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
motor->m_conf->foc_sl_erpm_hfi = sl_erpm_hfi_old;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
motor->m_conf->foc_control_sample_mode = foc_control_sample_mode_old;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_conf->foc_hfi_samples = samples_old;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
motor->m_conf->foc_current_sample_mode = foc_current_sample_mode_old;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-10-15 13:25:07 -07:00
|
|
|
|
mcpwm_foc_set_configuration(motor->m_conf);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
|
|
|
|
mc_interface_unlock();
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-10-31 09:47:09 -07:00
|
|
|
|
// The observer is more stable when the inductance is underestimated compared to overestimated,
|
|
|
|
|
// so scale it by 0.8. This helps motors that start to saturate at higher currents and when
|
|
|
|
|
// the hardware has problems measuring the inductance correctly. Another reason for decreasing the
|
|
|
|
|
// measured value is that delays in the hardware and/or a high resistance compared to inductance
|
|
|
|
|
// will cause the value to be overestimated.
|
2022-03-23 07:04:30 -07:00
|
|
|
|
// NOTE: This used to be 0.8, but was changed to 0.9 as that works better with HFIv2 on most motors.
|
|
|
|
|
float ind_scale_factor = 0.9;
|
2021-10-31 09:47:09 -07:00
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
if (curr) {
|
2020-01-28 10:46:19 -08:00
|
|
|
|
*curr = i_sum / iterations;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
if (ld_lq_diff) {
|
2021-10-31 09:47:09 -07:00
|
|
|
|
*ld_lq_diff = (ld_lq_diff_sum / iterations) * 1e6 * ind_scale_factor;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
*inductance = (l_sum / iterations) * 1e6 * ind_scale_factor;
|
|
|
|
|
return fault;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
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.
|
|
|
|
|
*
|
2022-09-27 13:52:40 -07:00
|
|
|
|
* @inductance
|
2020-01-28 10:46:19 -08:00
|
|
|
|
* The average d and q axis inductance in uH.
|
2022-09-27 13:52:40 -07:00
|
|
|
|
*
|
|
|
|
|
* @return
|
|
|
|
|
* The fault code
|
2019-02-18 10:30:19 -08:00
|
|
|
|
*/
|
2022-09-27 13:52:40 -07:00
|
|
|
|
int mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *curr, float *ld_lq_diff, float *inductance) {
|
2022-11-08 02:46:35 -08:00
|
|
|
|
int fault = FAULT_CODE_NONE;
|
2019-02-18 10:30:19 -08:00
|
|
|
|
float duty_last = 0.0;
|
|
|
|
|
for (float i = 0.02;i < 0.5;i *= 1.5) {
|
2022-12-04 04:55:56 -08:00
|
|
|
|
utils_truncate_number_abs(&i, 0.6);
|
2019-02-18 10:30:19 -08:00
|
|
|
|
float i_tmp;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mcpwm_foc_measure_inductance(i, 10, &i_tmp, 0, 0);
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
|
|
|
|
return fault;
|
2022-05-04 11:41:09 -07:00
|
|
|
|
}
|
2019-02-18 10:30:19 -08:00
|
|
|
|
|
|
|
|
|
duty_last = i;
|
|
|
|
|
if (i_tmp >= curr_goal) {
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mcpwm_foc_measure_inductance(duty_last, samples, curr, ld_lq_diff, inductance);
|
|
|
|
|
return fault;
|
2019-02-18 10:30:19 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-06-17 01:43:19 -07:00
|
|
|
|
bool mcpwm_foc_beep(float freq, float time, float voltage) {
|
2023-12-26 13:37:41 -08:00
|
|
|
|
if (mc_interface_get_fault() != FAULT_CODE_NONE) {
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
2022-06-17 01:43:19 -07:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
|
|
|
|
|
|
|
|
|
mc_foc_sensor_mode sensor_mode_old = motor->m_conf->foc_sensor_mode;
|
|
|
|
|
float f_zv_old = motor->m_conf->foc_f_zv;
|
|
|
|
|
float hfi_voltage_start_old = motor->m_conf->foc_hfi_voltage_start;
|
|
|
|
|
float hfi_voltage_run_old = motor->m_conf->foc_hfi_voltage_run;
|
|
|
|
|
float hfi_voltage_max_old = motor->m_conf->foc_hfi_voltage_max;
|
|
|
|
|
float sl_erpm_hfi_old = motor->m_conf->foc_sl_erpm_hfi;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
mc_foc_control_sample_mode foc_control_sample_mode_old = motor->m_conf->foc_control_sample_mode;
|
2022-06-17 01:43:19 -07:00
|
|
|
|
foc_hfi_samples samples_old = motor->m_conf->foc_hfi_samples;
|
|
|
|
|
uint16_t start_samples_old = motor->m_conf->foc_hfi_start_samples;
|
|
|
|
|
|
|
|
|
|
mc_interface_lock();
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
motor->m_state = MC_STATE_OFF;
|
|
|
|
|
stop_pwm_hw((motor_all_state_t*)motor);
|
|
|
|
|
|
|
|
|
|
motor->m_conf->foc_sensor_mode = FOC_SENSOR_MODE_HFI;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_start = voltage;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_run = voltage;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_max = voltage;
|
|
|
|
|
motor->m_conf->foc_sl_erpm_hfi = 20000.0;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
motor->m_conf->foc_control_sample_mode = FOC_CONTROL_SAMPLE_MODE_V0;
|
2022-06-17 01:43:19 -07:00
|
|
|
|
motor->m_conf->foc_hfi_samples = HFI_SAMPLES_8;
|
|
|
|
|
motor->m_conf->foc_hfi_start_samples = 10;
|
|
|
|
|
|
2022-06-17 03:04:13 -07:00
|
|
|
|
freq *= 4.0;
|
|
|
|
|
|
|
|
|
|
if (freq > 3500) {
|
|
|
|
|
motor->m_conf->foc_sensor_mode = FOC_SENSOR_MODE_HFI_V3;
|
|
|
|
|
freq /= 8.0;
|
|
|
|
|
}
|
|
|
|
|
|
2022-06-17 01:43:19 -07:00
|
|
|
|
motor->m_conf->foc_f_zv = freq * 8.0;
|
2022-06-17 03:04:13 -07:00
|
|
|
|
|
2022-06-17 01:43:19 -07:00
|
|
|
|
utils_truncate_number(&motor->m_conf->foc_f_zv, 3.0e3, 30.0e3);
|
|
|
|
|
|
|
|
|
|
mcpwm_foc_set_configuration(motor->m_conf);
|
|
|
|
|
|
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
|
|
|
|
|
timeout_reset();
|
|
|
|
|
mcpwm_foc_set_duty(0.0);
|
|
|
|
|
|
|
|
|
|
int ms_sleep = (time * 1000.0) - 1;
|
|
|
|
|
if (ms_sleep > 0) {
|
|
|
|
|
chThdSleepMilliseconds(ms_sleep);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
mcpwm_foc_set_current(0.0);
|
|
|
|
|
|
|
|
|
|
motor->m_conf->foc_sensor_mode = sensor_mode_old;
|
|
|
|
|
motor->m_conf->foc_f_zv = f_zv_old;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_start = hfi_voltage_start_old;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_run = hfi_voltage_run_old;
|
|
|
|
|
motor->m_conf->foc_hfi_voltage_max = hfi_voltage_max_old;
|
|
|
|
|
motor->m_conf->foc_sl_erpm_hfi = sl_erpm_hfi_old;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
motor->m_conf->foc_control_sample_mode = foc_control_sample_mode_old;
|
2022-06-17 01:43:19 -07:00
|
|
|
|
motor->m_conf->foc_hfi_samples = samples_old;
|
|
|
|
|
motor->m_conf->foc_hfi_start_samples = start_samples_old;
|
|
|
|
|
|
|
|
|
|
mcpwm_foc_set_configuration(motor->m_conf);
|
|
|
|
|
|
|
|
|
|
mc_interface_unlock();
|
|
|
|
|
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
2023-12-26 13:37:41 -08:00
|
|
|
|
bool mcpwm_foc_play_tone(int channel, float freq, float voltage) {
|
|
|
|
|
if (mc_interface_get_fault() != FAULT_CODE_NONE) {
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (channel < 0 || channel >= MC_AUDIO_CHANNELS) {
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
|
|
|
|
|
|
|
|
|
if (freq <= 0.1 || freq > motor->m_conf->foc_f_zv * 0.5) {
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
motor->m_audio.table_freq[channel] = freq;
|
|
|
|
|
motor->m_audio.table_voltage[channel] = voltage;
|
|
|
|
|
motor->m_audio.mode = MC_AUDIO_TABLE;
|
|
|
|
|
|
|
|
|
|
if (voltage < 0.01) {
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
mcpwm_foc_set_current_off_delay(1.0);
|
|
|
|
|
|
|
|
|
|
if (motor->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_CURRENT;
|
|
|
|
|
motor->m_iq_set = 0.0;
|
|
|
|
|
motor->m_id_set = 0.0;
|
|
|
|
|
motor->m_motor_released = false;
|
|
|
|
|
motor->m_state = MC_STATE_RUNNING;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void mcpwm_foc_stop_audio(bool reset) {
|
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
|
|
|
|
motor->m_audio.mode = MC_AUDIO_OFF;
|
|
|
|
|
|
|
|
|
|
if (reset) {
|
|
|
|
|
init_audio_state(&motor->m_audio);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool mcpwm_foc_set_audio_sample_table(int channel, float *samples, int len) {
|
|
|
|
|
if (channel < 0 || channel >= MC_AUDIO_CHANNELS) {
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
|
|
|
|
|
|
|
|
|
motor->m_audio.table[channel] = samples;
|
|
|
|
|
motor->m_audio.table_len[channel] = len;
|
|
|
|
|
motor->m_audio.table_pos[channel] = 0.0;
|
|
|
|
|
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
const float *mcpwm_foc_get_audio_sample_table(int channel) {
|
|
|
|
|
if (channel < 0 || channel >= MC_AUDIO_CHANNELS) {
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
|
|
|
|
|
|
|
|
|
return motor->m_audio.table[channel];
|
|
|
|
|
}
|
|
|
|
|
|
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.
|
|
|
|
|
*
|
2021-11-21 09:17:40 -08:00
|
|
|
|
* @param ld_lq_diff
|
|
|
|
|
* The measured difference in D axis and Q axis inductance.
|
|
|
|
|
*
|
2015-12-08 12:01:23 -08:00
|
|
|
|
* @return
|
2022-09-27 13:52:40 -07:00
|
|
|
|
* The fault code
|
2015-12-08 12:01:23 -08:00
|
|
|
|
*/
|
2022-09-27 13:52:40 -07:00
|
|
|
|
int mcpwm_foc_measure_res_ind(float *res, float *ind, float *ld_lq_diff) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
2022-11-08 02:46:35 -08:00
|
|
|
|
int fault = FAULT_CODE_NONE;
|
2022-05-04 15:02:23 -07:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
const float kp_old = motor->m_conf->foc_current_kp;
|
|
|
|
|
const float ki_old = motor->m_conf->foc_current_ki;
|
|
|
|
|
const float res_old = motor->m_conf->foc_motor_r;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_conf->foc_current_kp = 0.001;
|
|
|
|
|
motor->m_conf->foc_current_ki = 1.0;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
float i_last = 0.0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
for (float i = 2.0;i < (motor->m_conf->l_current_max / 2.0);i *= 1.5) {
|
2022-11-08 02:46:35 -08:00
|
|
|
|
float r_tmp = 0.0;
|
|
|
|
|
fault = mcpwm_foc_measure_resistance(i, 20, false, &r_tmp);
|
|
|
|
|
if (fault != FAULT_CODE_NONE || r_tmp == 0.0) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_measure_res_ind;
|
2022-05-04 11:41:09 -07:00
|
|
|
|
}
|
|
|
|
|
if (i > (1.0 / r_tmp)) {
|
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) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
i_last = (motor->m_conf->l_current_max / 2.0);
|
2019-02-19 09:55:18 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2019-04-20 12:35:34 -07:00
|
|
|
|
#ifdef HW_AXIOM_FORCE_HIGH_CURRENT_MEASUREMENTS
|
2020-03-16 10:32:39 -07:00
|
|
|
|
i_last = (motor->m_conf->l_current_max / 2.0);
|
2019-03-28 16:33:21 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mcpwm_foc_measure_resistance(i_last, 200, true, res);
|
|
|
|
|
if (fault == FAULT_CODE_NONE && *res != 0.0) {
|
2022-05-04 11:41:09 -07:00
|
|
|
|
motor->m_conf->foc_motor_r = *res;
|
2022-12-04 04:55:56 -08:00
|
|
|
|
mcpwm_foc_set_current(0.0);
|
|
|
|
|
chThdSleepMilliseconds(10);
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mcpwm_foc_measure_inductance_current(i_last, 200, 0, ld_lq_diff, ind);
|
2022-05-04 11:41:09 -07:00
|
|
|
|
}
|
2022-05-04 15:02:23 -07:00
|
|
|
|
|
2022-11-12 01:44:50 -08:00
|
|
|
|
exit_measure_res_ind:
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_conf->foc_current_kp = kp_old;
|
|
|
|
|
motor->m_conf->foc_current_ki = ki_old;
|
|
|
|
|
motor->m_conf->foc_motor_r = res_old;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
return fault;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2016-01-27 14:57:23 -08:00
|
|
|
|
/**
|
|
|
|
|
* 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.
|
|
|
|
|
*
|
2022-09-27 13:52:40 -07:00
|
|
|
|
* @result
|
2016-01-27 14:57:23 -08:00
|
|
|
|
* true: Success
|
|
|
|
|
* false: Something went wrong
|
2022-09-27 13:52:40 -07:00
|
|
|
|
*
|
|
|
|
|
* @return
|
|
|
|
|
* The fault code
|
2016-01-27 14:57:23 -08:00
|
|
|
|
*/
|
2022-09-27 13:52:40 -07:00
|
|
|
|
int mcpwm_foc_hall_detect(float current, uint8_t *hall_table, bool *result) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
volatile motor_all_state_t *motor = get_motor_now();
|
2022-11-08 02:46:35 -08:00
|
|
|
|
int fault = FAULT_CODE_NONE;
|
2016-02-02 08:19:37 -08:00
|
|
|
|
mc_interface_lock();
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_phase_override = true;
|
2020-04-03 11:12:12 -07:00
|
|
|
|
motor->m_id_set = 0.0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_iq_set = 0.0;
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_CURRENT;
|
2022-01-19 09:52:45 -08:00
|
|
|
|
motor->m_motor_released = false;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_state = MC_STATE_RUNNING;
|
2016-01-27 14:57:23 -08:00
|
|
|
|
|
2020-07-21 11:07:46 -07:00
|
|
|
|
// MTPA overrides id target
|
2021-10-24 09:10:00 -07:00
|
|
|
|
MTPA_MODE mtpa_old = motor->m_conf->foc_mtpa_mode;
|
|
|
|
|
motor->m_conf->foc_mtpa_mode = MTPA_MODE_OFF;
|
2020-07-21 11:07:46 -07:00
|
|
|
|
|
2016-01-27 14:57:23 -08:00
|
|
|
|
// Disable timeout
|
|
|
|
|
systime_t tout = timeout_get_timeout_msec();
|
|
|
|
|
float tout_c = timeout_get_brake_current();
|
2021-06-13 04:46:27 -07:00
|
|
|
|
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
|
2016-11-04 07:18:34 -07:00
|
|
|
|
timeout_reset();
|
2021-06-13 04:46:27 -07:00
|
|
|
|
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
|
2016-01-27 14:57:23 -08:00
|
|
|
|
|
|
|
|
|
// Lock the motor
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_phase_now_override = 0;
|
2020-04-03 11:12:12 -07:00
|
|
|
|
|
2022-11-12 01:44:50 -08:00
|
|
|
|
*result = false;
|
|
|
|
|
|
2020-04-03 11:12:12 -07:00
|
|
|
|
for (int i = 0;i < 1000;i++) {
|
|
|
|
|
motor->m_id_set = (float)i * current / 1000.0;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_hall_detect;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
}
|
2020-04-03 11:12:12 -07:00
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
}
|
2016-01-27 14:57:23 -08:00
|
|
|
|
|
|
|
|
|
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++) {
|
2019-01-28 11:52:02 -08:00
|
|
|
|
for (int j = 0;j < 360;j++) {
|
2021-10-13 09:13:51 -07:00
|
|
|
|
motor->m_phase_now_override = DEG2RAD_f(j);
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_hall_detect;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
}
|
2016-01-27 14:57:23 -08:00
|
|
|
|
chThdSleepMilliseconds(5);
|
|
|
|
|
|
2020-10-09 12:08:48 -07:00
|
|
|
|
int hall = utils_read_hall(motor != &m_motor_1, motor->m_conf->m_hall_extra_samples);
|
2016-01-27 14:57:23 -08:00
|
|
|
|
float s, c;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
sincosf(motor->m_phase_now_override, &s, &c);
|
2016-01-27 14:57:23 -08:00
|
|
|
|
sin_hall[hall] += s;
|
|
|
|
|
cos_hall[hall] += c;
|
|
|
|
|
hall_iterations[hall]++;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Reverse
|
|
|
|
|
for (int i = 0;i < 3;i++) {
|
2019-01-28 11:52:02 -08:00
|
|
|
|
for (int j = 360;j >= 0;j--) {
|
2021-10-13 09:13:51 -07:00
|
|
|
|
motor->m_phase_now_override = DEG2RAD_f(j);
|
2022-11-08 02:46:35 -08:00
|
|
|
|
fault = mc_interface_get_fault();
|
|
|
|
|
if (fault != FAULT_CODE_NONE) {
|
2022-11-12 01:44:50 -08:00
|
|
|
|
goto exit_hall_detect;
|
2022-11-08 02:46:35 -08:00
|
|
|
|
}
|
2016-01-27 14:57:23 -08:00
|
|
|
|
chThdSleepMilliseconds(5);
|
|
|
|
|
|
2020-10-09 12:08:48 -07:00
|
|
|
|
int hall = utils_read_hall(motor != &m_motor_1, motor->m_conf->m_hall_extra_samples);
|
2016-01-27 14:57:23 -08:00
|
|
|
|
float s, c;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
sincosf(motor->m_phase_now_override, &s, &c);
|
2016-01-27 14:57:23 -08:00
|
|
|
|
sin_hall[hall] += s;
|
|
|
|
|
cos_hall[hall] += c;
|
|
|
|
|
hall_iterations[hall]++;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int fails = 0;
|
|
|
|
|
for(int i = 0;i < 8;i++) {
|
|
|
|
|
if (hall_iterations[i] > 30) {
|
2021-10-13 09:13:51 -07:00
|
|
|
|
float ang = RAD2DEG_f(atan2f(sin_hall[i], cos_hall[i]));
|
2016-01-27 14:57:23 -08:00
|
|
|
|
utils_norm_angle(&ang);
|
|
|
|
|
hall_table[i] = (uint8_t)(ang * 200.0 / 360.0);
|
|
|
|
|
} else {
|
|
|
|
|
hall_table[i] = 255;
|
|
|
|
|
fails++;
|
|
|
|
|
}
|
|
|
|
|
}
|
2022-11-12 01:44:50 -08:00
|
|
|
|
*result = (fails == 2);
|
2016-01-27 14:57:23 -08:00
|
|
|
|
|
2022-11-12 01:44:50 -08:00
|
|
|
|
exit_hall_detect:
|
|
|
|
|
motor->m_id_set = 0.0;
|
|
|
|
|
motor->m_iq_set = 0.0;
|
|
|
|
|
motor->m_phase_override = false;
|
|
|
|
|
motor->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
motor->m_state = MC_STATE_OFF;
|
|
|
|
|
stop_pwm_hw((motor_all_state_t*)motor);
|
|
|
|
|
motor->m_conf->foc_mtpa_mode = mtpa_old;
|
|
|
|
|
timeout_configure(tout, tout_c, tout_ksw);
|
2016-02-02 08:19:37 -08:00
|
|
|
|
mc_interface_unlock();
|
|
|
|
|
|
2022-11-08 02:46:35 -08:00
|
|
|
|
return fault;
|
2016-01-27 14:57:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
/**
|
|
|
|
|
* Calibrate voltage and current offsets. For the observer to work at low modulation it
|
|
|
|
|
* is very important to get all current and voltage offsets right. Therefore we store
|
|
|
|
|
* the offsets for when the motor is undriven and when it is driven separately. The
|
|
|
|
|
* motor is driven at 50% modulation on all phases when measuring the driven offset, which
|
|
|
|
|
* corresponds to space-vector modulation with 0 amplitude.
|
|
|
|
|
*
|
2021-03-22 04:13:19 -07:00
|
|
|
|
* cal_undriven:
|
|
|
|
|
* Calibrate undriven voltages too. This requires the motor to stand still.
|
|
|
|
|
*
|
|
|
|
|
* return:
|
|
|
|
|
* -1: Timed out while waiting for fault code to go away.
|
|
|
|
|
* 1: Success
|
|
|
|
|
*
|
2021-02-28 11:36:02 -08:00
|
|
|
|
*/
|
2023-08-24 15:45:58 -07:00
|
|
|
|
#ifndef HW_USE_ALTERNATIVE_DC_CAL
|
2021-03-22 04:13:19 -07:00
|
|
|
|
int mcpwm_foc_dc_cal(bool cal_undriven) {
|
2021-02-28 11:36:02 -08:00
|
|
|
|
// Wait max 5 seconds for DRV-fault to go away
|
|
|
|
|
int cnt = 0;
|
|
|
|
|
while(IS_DRV_FAULT()){
|
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
cnt++;
|
|
|
|
|
if (cnt > 5000) {
|
2021-03-22 04:13:19 -07:00
|
|
|
|
return -1;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
}
|
|
|
|
|
};
|
|
|
|
|
|
2021-03-22 04:13:19 -07:00
|
|
|
|
chThdSleepMilliseconds(1000);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
|
|
|
|
// Disable timeout
|
|
|
|
|
systime_t tout = timeout_get_timeout_msec();
|
|
|
|
|
float tout_c = timeout_get_brake_current();
|
2021-06-13 04:46:27 -07:00
|
|
|
|
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
|
2021-02-28 11:36:02 -08:00
|
|
|
|
timeout_reset();
|
2021-06-13 04:46:27 -07:00
|
|
|
|
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
|
|
|
|
// Measure driven offsets
|
2023-07-01 05:51:08 -07:00
|
|
|
|
// NOTE: One phase is measured at a time while the others are left
|
|
|
|
|
// floating so that no torque is generated in case the motor is spinning
|
|
|
|
|
// at boot.
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
2021-03-24 05:09:05 -07:00
|
|
|
|
const float samples = 1000.0;
|
2021-03-22 04:13:19 -07:00
|
|
|
|
float current_sum[3] = {0.0, 0.0, 0.0};
|
|
|
|
|
float voltage_sum[3] = {0.0, 0.0, 0.0};
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
TIMER_UPDATE_DUTY_M1(TIM1->ARR / 2, TIM1->ARR / 2, TIM1->ARR / 2);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
|
|
|
|
|
// Start PWM on phase 1
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
PHASE_FILTER_ON();
|
|
|
|
|
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);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2021-03-22 04:13:19 -07:00
|
|
|
|
float current_sum_m2[3] = {0.0, 0.0, 0.0};
|
|
|
|
|
float voltage_sum_m2[3] = {0.0, 0.0, 0.0};
|
2021-02-28 11:36:02 -08:00
|
|
|
|
TIMER_UPDATE_DUTY_M2(TIM8->ARR / 2, TIM8->ARR / 2, TIM8->ARR / 2);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_2);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
PHASE_FILTER_ON_M2();
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_OCMode_PWM1);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Enable);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
TIM_GenerateEvent(TIM8, TIM_EventSource_COM);
|
|
|
|
|
#endif
|
|
|
|
|
|
2022-02-19 23:07:36 -08:00
|
|
|
|
chThdSleep(1);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
2021-03-22 04:13:19 -07:00
|
|
|
|
for (float i = 0;i < samples;i++) {
|
2021-03-24 05:09:05 -07:00
|
|
|
|
current_sum[0] += m_motor_1.m_currents_adc[0];
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum[0] += ADC_VOLTS(ADC_IND_SENS1);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2021-03-24 05:09:05 -07:00
|
|
|
|
current_sum_m2[0] += m_motor_2.m_currents_adc[0];
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum_m2[0] += ADC_VOLTS(ADC_IND_SENS4);
|
|
|
|
|
#endif
|
|
|
|
|
chThdSleep(1);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Start PWM on phase 2
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
PHASE_FILTER_ON();
|
|
|
|
|
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_GenerateEvent(TIM1, TIM_EventSource_COM);
|
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_2);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
PHASE_FILTER_ON_M2();
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_OCMode_PWM1);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Enable);
|
|
|
|
|
TIM_GenerateEvent(TIM8, TIM_EventSource_COM);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#endif
|
|
|
|
|
|
2021-03-22 04:13:19 -07:00
|
|
|
|
chThdSleep(1);
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
for (float i = 0;i < samples;i++) {
|
2021-03-24 05:09:05 -07:00
|
|
|
|
current_sum[1] += m_motor_1.m_currents_adc[1];
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum[1] += ADC_VOLTS(ADC_IND_SENS2);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2021-03-24 05:09:05 -07:00
|
|
|
|
current_sum_m2[1] += m_motor_2.m_currents_adc[1];
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum_m2[1] += ADC_VOLTS(ADC_IND_SENS5);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#endif
|
|
|
|
|
chThdSleep(1);
|
|
|
|
|
}
|
|
|
|
|
|
2021-03-22 04:13:19 -07:00
|
|
|
|
// Start PWM on phase 3
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
PHASE_FILTER_ON();
|
|
|
|
|
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);
|
|
|
|
|
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_2);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
PHASE_FILTER_ON_M2();
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_OCMode_PWM1);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Enable);
|
|
|
|
|
TIM_GenerateEvent(TIM8, TIM_EventSource_COM);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
chThdSleep(1);
|
|
|
|
|
|
|
|
|
|
for (float i = 0;i < samples;i++) {
|
2021-03-24 05:09:05 -07:00
|
|
|
|
current_sum[2] += m_motor_1.m_currents_adc[2];
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum[2] += ADC_VOLTS(ADC_IND_SENS3);
|
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2021-03-24 05:09:05 -07:00
|
|
|
|
current_sum_m2[2] += m_motor_2.m_currents_adc[2];
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum_m2[2] += ADC_VOLTS(ADC_IND_SENS6);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#endif
|
2021-03-22 04:13:19 -07:00
|
|
|
|
chThdSleep(1);
|
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_current[0] = current_sum[0] / samples;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_current[1] = current_sum[1] / samples;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_current[2] = current_sum[2] / samples;
|
|
|
|
|
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum[0] /= samples;
|
|
|
|
|
voltage_sum[1] /= samples;
|
|
|
|
|
voltage_sum[2] /= samples;
|
|
|
|
|
float v_avg = (voltage_sum[0] + voltage_sum[1] + voltage_sum[2]) / 3.0;
|
|
|
|
|
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage[0] = voltage_sum[0] - v_avg;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage[1] = voltage_sum[1] - v_avg;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage[2] = voltage_sum[2] - v_avg;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_2);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
m_motor_2.m_conf->foc_offsets_current[0] = current_sum_m2[0] / samples;
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_current[1] = current_sum_m2[1] / samples;
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_current[2] = current_sum_m2[2] / samples;
|
|
|
|
|
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum_m2[0] /= samples;
|
|
|
|
|
voltage_sum_m2[1] /= samples;
|
|
|
|
|
voltage_sum_m2[2] /= samples;
|
|
|
|
|
v_avg = (voltage_sum_m2[0] + voltage_sum_m2[1] + voltage_sum_m2[2]) / 3.0;
|
|
|
|
|
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage[0] = voltage_sum_m2[0] - v_avg;
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage[1] = voltage_sum_m2[1] - v_avg;
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage[2] = voltage_sum_m2[2] - v_avg;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
// Measure undriven offsets
|
|
|
|
|
|
2021-03-22 04:13:19 -07:00
|
|
|
|
if (cal_undriven) {
|
|
|
|
|
chThdSleepMilliseconds(10);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum[0] = 0.0; voltage_sum[1] = 0.0; voltage_sum[2] = 0.0;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum_m2[0] = 0.0; voltage_sum_m2[1] = 0.0; voltage_sum_m2[2] = 0.0;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#endif
|
|
|
|
|
|
2021-03-22 04:13:19 -07:00
|
|
|
|
for (float i = 0;i < samples;i++) {
|
|
|
|
|
v_avg = (ADC_VOLTS(ADC_IND_SENS1) + ADC_VOLTS(ADC_IND_SENS2) + ADC_VOLTS(ADC_IND_SENS3)) / 3.0;
|
|
|
|
|
voltage_sum[0] += ADC_VOLTS(ADC_IND_SENS1) - v_avg;
|
|
|
|
|
voltage_sum[1] += ADC_VOLTS(ADC_IND_SENS2) - v_avg;
|
|
|
|
|
voltage_sum[2] += ADC_VOLTS(ADC_IND_SENS3) - v_avg;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2021-03-22 04:13:19 -07:00
|
|
|
|
v_avg = (ADC_VOLTS(ADC_IND_SENS4) + ADC_VOLTS(ADC_IND_SENS5) + ADC_VOLTS(ADC_IND_SENS6)) / 3.0;
|
|
|
|
|
voltage_sum_m2[0] += ADC_VOLTS(ADC_IND_SENS4) - v_avg;
|
|
|
|
|
voltage_sum_m2[1] += ADC_VOLTS(ADC_IND_SENS5) - v_avg;
|
|
|
|
|
voltage_sum_m2[2] += ADC_VOLTS(ADC_IND_SENS6) - v_avg;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#endif
|
2021-03-22 04:13:19 -07:00
|
|
|
|
chThdSleep(1);
|
|
|
|
|
}
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
2021-03-22 04:13:19 -07:00
|
|
|
|
voltage_sum[0] /= samples;
|
|
|
|
|
voltage_sum[1] /= samples;
|
|
|
|
|
voltage_sum[2] /= samples;
|
|
|
|
|
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[0] = voltage_sum[0];
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[1] = voltage_sum[1];
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[2] = voltage_sum[2];
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-04-06 07:33:44 -07:00
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_2);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
|
|
|
|
|
voltage_sum_m2[0] /= samples;
|
|
|
|
|
voltage_sum_m2[1] /= samples;
|
|
|
|
|
voltage_sum_m2[2] /= samples;
|
|
|
|
|
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage_undriven[0] = voltage_sum_m2[0];
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage_undriven[1] = voltage_sum_m2[1];
|
|
|
|
|
m_motor_2.m_conf->foc_offsets_voltage_undriven[2] = voltage_sum_m2[2];
|
2021-02-28 11:36:02 -08:00
|
|
|
|
#endif
|
2021-03-22 04:13:19 -07:00
|
|
|
|
}
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
|
|
|
|
// TODO: Make sure that offsets are no more than e.g. 5%, as larger values indicate hardware problems.
|
|
|
|
|
|
|
|
|
|
// Enable timeout
|
2021-06-13 04:46:27 -07:00
|
|
|
|
timeout_configure(tout, tout_c, tout_ksw);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
mc_interface_unlock();
|
|
|
|
|
|
|
|
|
|
m_dccal_done = true;
|
2021-03-22 04:13:19 -07:00
|
|
|
|
|
|
|
|
|
return 1;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
}
|
2023-08-24 15:45:58 -07:00
|
|
|
|
#else
|
|
|
|
|
// WARNING: This calibration routine can only be run when the motor is not spinning.
|
|
|
|
|
// For low side shunt hardware with high capacitance mosfets this works a lot better
|
|
|
|
|
int mcpwm_foc_dc_cal(bool cal_undriven) {
|
|
|
|
|
// Wait max 5 seconds for DRV-fault to go away
|
|
|
|
|
int cnt = 0;
|
|
|
|
|
while(IS_DRV_FAULT()){
|
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
cnt++;
|
|
|
|
|
if (cnt > 5000) {
|
|
|
|
|
return -1;
|
|
|
|
|
}
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
chThdSleepMilliseconds(1000);
|
|
|
|
|
|
|
|
|
|
// Disable timeout
|
|
|
|
|
systime_t tout = timeout_get_timeout_msec();
|
|
|
|
|
float tout_c = timeout_get_brake_current();
|
|
|
|
|
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
|
|
|
|
|
timeout_reset();
|
|
|
|
|
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
|
|
|
|
|
|
|
|
|
|
// Measure driven offsets
|
|
|
|
|
const float samples = 1000.0;
|
|
|
|
|
float current_sum[3] = {0.0, 0.0, 0.0};
|
|
|
|
|
float voltage_sum[3] = {0.0, 0.0, 0.0};
|
|
|
|
|
|
|
|
|
|
TIMER_UPDATE_DUTY_M1(TIM1->ARR / 2, TIM1->ARR / 2, TIM1->ARR / 2);
|
|
|
|
|
|
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
|
|
|
|
PHASE_FILTER_ON();
|
|
|
|
|
|
|
|
|
|
// Start PWM on all phases at 50% to get a V0 measurement
|
|
|
|
|
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);
|
|
|
|
|
|
|
|
|
|
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
|
|
|
|
|
|
|
|
|
|
chThdSleep(1);
|
|
|
|
|
|
|
|
|
|
for (float i = 0; i < samples; i++) {
|
|
|
|
|
current_sum[0] += m_motor_1.m_currents_adc[0];
|
|
|
|
|
voltage_sum[0] += ADC_VOLTS(ADC_IND_SENS1);
|
|
|
|
|
current_sum[1] += m_motor_1.m_currents_adc[1];
|
|
|
|
|
voltage_sum[1] += ADC_VOLTS(ADC_IND_SENS2);
|
|
|
|
|
current_sum[2] += m_motor_1.m_currents_adc[2];
|
|
|
|
|
voltage_sum[2] += ADC_VOLTS(ADC_IND_SENS3);
|
|
|
|
|
chThdSleep(1);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
|
|
|
|
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_current[0] = current_sum[0] / samples;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_current[1] = current_sum[1] / samples;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_current[2] = current_sum[2] / samples;
|
|
|
|
|
|
|
|
|
|
voltage_sum[0] /= samples;
|
|
|
|
|
voltage_sum[1] /= samples;
|
|
|
|
|
voltage_sum[2] /= samples;
|
|
|
|
|
float v_avg = (voltage_sum[0] + voltage_sum[1] + voltage_sum[2]) / 3.0;
|
|
|
|
|
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage[0] = voltage_sum[0] - v_avg;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage[1] = voltage_sum[1] - v_avg;
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage[2] = voltage_sum[2] - v_avg;
|
|
|
|
|
|
|
|
|
|
// Measure undriven offsets
|
|
|
|
|
|
|
|
|
|
if (cal_undriven) {
|
|
|
|
|
chThdSleepMilliseconds(10);
|
|
|
|
|
|
|
|
|
|
voltage_sum[0] = 0.0; voltage_sum[1] = 0.0; voltage_sum[2] = 0.0;
|
|
|
|
|
|
|
|
|
|
for (float i = 0;i < samples;i++) {
|
|
|
|
|
v_avg = (ADC_VOLTS(ADC_IND_SENS1) + ADC_VOLTS(ADC_IND_SENS2) + ADC_VOLTS(ADC_IND_SENS3)) / 3.0;
|
|
|
|
|
voltage_sum[0] += ADC_VOLTS(ADC_IND_SENS1) - v_avg;
|
|
|
|
|
voltage_sum[1] += ADC_VOLTS(ADC_IND_SENS2) - v_avg;
|
|
|
|
|
voltage_sum[2] += ADC_VOLTS(ADC_IND_SENS3) - v_avg;
|
|
|
|
|
|
|
|
|
|
chThdSleep(1);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
|
|
|
|
|
|
|
|
|
voltage_sum[0] /= samples;
|
|
|
|
|
voltage_sum[1] /= samples;
|
|
|
|
|
voltage_sum[2] /= samples;
|
|
|
|
|
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[0] = voltage_sum[0];
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[1] = voltage_sum[1];
|
|
|
|
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[2] = voltage_sum[2];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// TODO: Make sure that offsets are no more than e.g. 5%, as larger values indicate hardware problems.
|
|
|
|
|
|
|
|
|
|
// Enable timeout
|
|
|
|
|
timeout_configure(tout, tout_c, tout_ksw);
|
|
|
|
|
mc_interface_unlock();
|
|
|
|
|
|
|
|
|
|
m_dccal_done = true;
|
|
|
|
|
|
|
|
|
|
return 1;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
void mcpwm_foc_print_state(void) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
commands_printf("Mod d: %.2f", (double)get_motor_now()->m_motor_state.mod_d);
|
|
|
|
|
commands_printf("Mod q: %.2f", (double)get_motor_now()->m_motor_state.mod_q);
|
|
|
|
|
commands_printf("Mod q flt: %.2f", (double)get_motor_now()->m_motor_state.mod_q_filter);
|
|
|
|
|
commands_printf("Duty: %.2f", (double)get_motor_now()->m_motor_state.duty_now);
|
|
|
|
|
commands_printf("Vd: %.2f", (double)get_motor_now()->m_motor_state.vd);
|
|
|
|
|
commands_printf("Vq: %.2f", (double)get_motor_now()->m_motor_state.vq);
|
|
|
|
|
commands_printf("Phase: %.2f", (double)get_motor_now()->m_motor_state.phase);
|
|
|
|
|
commands_printf("V_alpha: %.2f", (double)get_motor_now()->m_motor_state.v_alpha);
|
|
|
|
|
commands_printf("V_beta: %.2f", (double)get_motor_now()->m_motor_state.v_beta);
|
|
|
|
|
commands_printf("id: %.2f", (double)get_motor_now()->m_motor_state.id);
|
|
|
|
|
commands_printf("iq: %.2f", (double)get_motor_now()->m_motor_state.iq);
|
|
|
|
|
commands_printf("id_filter: %.2f", (double)get_motor_now()->m_motor_state.id_filter);
|
|
|
|
|
commands_printf("iq_filter: %.2f", (double)get_motor_now()->m_motor_state.iq_filter);
|
|
|
|
|
commands_printf("id_target: %.2f", (double)get_motor_now()->m_motor_state.id_target);
|
|
|
|
|
commands_printf("iq_target: %.2f", (double)get_motor_now()->m_motor_state.iq_target);
|
|
|
|
|
commands_printf("i_abs: %.2f", (double)get_motor_now()->m_motor_state.i_abs);
|
|
|
|
|
commands_printf("i_abs_flt: %.2f", (double)get_motor_now()->m_motor_state.i_abs_filter);
|
2022-05-21 13:54:12 -07:00
|
|
|
|
commands_printf("Obs_x1: %.2f", (double)get_motor_now()->m_observer_state.x1);
|
|
|
|
|
commands_printf("Obs_x2: %.2f", (double)get_motor_now()->m_observer_state.x2);
|
2022-12-14 10:48:26 -08:00
|
|
|
|
commands_printf("lambda_est:%.4f", (double)get_motor_now()->m_observer_state.lambda_est);
|
2022-04-06 07:33:44 -07:00
|
|
|
|
commands_printf("vd_int: %.2f", (double)get_motor_now()->m_motor_state.vd_int);
|
|
|
|
|
commands_printf("vq_int: %.2f", (double)get_motor_now()->m_motor_state.vq_int);
|
|
|
|
|
commands_printf("off_delay: %.2f", (double)get_motor_now()->m_current_off_delay);
|
2015-12-23 15:43:31 -08:00
|
|
|
|
}
|
|
|
|
|
|
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
|
|
|
|
}
|
|
|
|
|
|
2017-09-04 12:12:43 -07: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);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM_GenerateEvent(TIM8, TIM_EventSource_COM);
|
2019-04-06 06:36:00 -07:00
|
|
|
|
|
2020-03-16 14:42:44 -07:00
|
|
|
|
virtual_motor_int_handler(
|
|
|
|
|
m_motor_1.m_motor_state.v_alpha,
|
|
|
|
|
m_motor_1.m_motor_state.v_beta);
|
2017-09-04 12:12:43 -07:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
|
void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
|
|
|
|
|
(void)p;
|
|
|
|
|
(void)flags;
|
|
|
|
|
|
2019-01-29 19:33:56 -08:00
|
|
|
|
static int skip = 0;
|
2019-02-19 09:55:18 -08:00
|
|
|
|
if (++skip == FOC_CONTROL_LOOP_FREQ_DIVIDER) {
|
2019-01-29 19:33:56 -08:00
|
|
|
|
skip = 0;
|
2019-02-19 09:55:18 -08:00
|
|
|
|
} else {
|
2019-01-29 19:33:56 -08:00
|
|
|
|
return;
|
2019-02-19 09:55:18 -08:00
|
|
|
|
}
|
2019-01-29 19:33:56 -08:00
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
uint32_t t_start = timer_time_now();
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
bool is_v7 = !(TIM1->CR1 & TIM_CR1_DIR);
|
|
|
|
|
int norm_curr_ofs = 0;
|
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
bool is_second_motor = is_v7;
|
|
|
|
|
norm_curr_ofs = is_second_motor ? 3 : 0;
|
2022-04-06 07:33:44 -07:00
|
|
|
|
motor_all_state_t *motor_now = is_second_motor ? (motor_all_state_t*)&m_motor_2 : (motor_all_state_t*)&m_motor_1;
|
|
|
|
|
motor_all_state_t *motor_other = is_second_motor ? (motor_all_state_t*)&m_motor_1 : (motor_all_state_t*)&m_motor_2;
|
2020-03-16 14:42:44 -07:00
|
|
|
|
m_isr_motor = is_second_motor ? 2 : 1;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
|
volatile TIM_TypeDef *tim = is_second_motor ? TIM8 : TIM1;
|
|
|
|
|
#endif
|
|
|
|
|
#else
|
2022-04-06 07:33:44 -07:00
|
|
|
|
motor_all_state_t *motor_other = (motor_all_state_t*)&m_motor_1;
|
|
|
|
|
motor_all_state_t *motor_now = (motor_all_state_t*)&m_motor_1;;
|
2020-03-16 14:42:44 -07:00
|
|
|
|
m_isr_motor = 1;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
|
volatile TIM_TypeDef *tim = TIM1;
|
|
|
|
|
#endif
|
|
|
|
|
#endif
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
mc_configuration *conf_now = motor_now->m_conf;
|
|
|
|
|
mc_configuration *conf_other = motor_other->m_conf;
|
2015-12-23 15:43:31 -08:00
|
|
|
|
|
2023-09-23 05:45:00 -07:00
|
|
|
|
bool skip_interpolation = motor_other->m_cc_was_hfi;
|
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
// Update modulation for V7 and collect current samples. This is used by the HFI.
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (motor_other->m_duty_next_set) {
|
|
|
|
|
motor_other->m_duty_next_set = false;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
skip_interpolation = true;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-03-23 07:04:30 -07:00
|
|
|
|
float curr0;
|
|
|
|
|
float curr1;
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (is_second_motor) {
|
2023-02-07 12:40:26 -08:00
|
|
|
|
curr0 = (GET_CURRENT1() - conf_other->foc_offsets_current[0]) * FAC_CURRENT;
|
|
|
|
|
curr1 = (GET_CURRENT2() - conf_other->foc_offsets_current[1]) * FAC_CURRENT;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIMER_UPDATE_DUTY_M1(motor_other->m_duty1_next, motor_other->m_duty2_next, motor_other->m_duty3_next);
|
|
|
|
|
} else {
|
2023-02-07 12:40:26 -08:00
|
|
|
|
curr0 = (GET_CURRENT1_M2() - conf_other->foc_offsets_current[0]) * FAC_CURRENT;
|
|
|
|
|
curr1 = (GET_CURRENT2_M2() - conf_other->foc_offsets_current[1]) * FAC_CURRENT;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIMER_UPDATE_DUTY_M2(motor_other->m_duty1_next, motor_other->m_duty2_next, motor_other->m_duty3_next);
|
|
|
|
|
}
|
|
|
|
|
#else
|
2023-02-07 12:40:26 -08:00
|
|
|
|
float curr0 = (GET_CURRENT1() - conf_other->foc_offsets_current[0]) * FAC_CURRENT;
|
|
|
|
|
float curr1 = (GET_CURRENT2() - conf_other->foc_offsets_current[1]) * FAC_CURRENT;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
|
|
|
|
|
TIMER_UPDATE_DUTY_M1(motor_other->m_duty1_next, motor_other->m_duty2_next, motor_other->m_duty3_next);
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_PARALLEL
|
2022-03-23 07:04:30 -07:00
|
|
|
|
TIMER_UPDATE_DUTY_M2(motor_other->m_duty1_next, motor_other->m_duty2_next, motor_other->m_duty3_next);
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
2022-03-23 07:04:30 -07:00
|
|
|
|
|
|
|
|
|
motor_other->m_i_alpha_sample_next = curr0;
|
|
|
|
|
motor_other->m_i_beta_sample_next = ONE_BY_SQRT3 * curr0 + TWO_BY_SQRT3 * curr1;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
|
|
|
|
|
2023-09-23 05:45:00 -07:00
|
|
|
|
bool do_return = false;
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifndef HW_HAS_DUAL_MOTORS
|
2017-09-04 12:12:43 -07:00
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (conf_now->foc_control_sample_mode != FOC_CONTROL_SAMPLE_MODE_V0_V7 && is_v7) {
|
|
|
|
|
do_return = true;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
}
|
2017-09-04 12:12:43 -07:00
|
|
|
|
#else
|
2020-01-28 10:46:19 -08:00
|
|
|
|
if (is_v7) {
|
2023-09-23 05:45:00 -07:00
|
|
|
|
do_return = true;
|
2017-09-04 12:12:43 -07:00
|
|
|
|
}
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
2020-01-28 10:46:19 -08:00
|
|
|
|
#endif
|
2017-09-04 12:12:43 -07:00
|
|
|
|
|
2023-09-23 05:45:00 -07:00
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
|
|
|
|
float dt;
|
|
|
|
|
if (conf_now->foc_control_sample_mode == FOC_CONTROL_SAMPLE_MODE_V0_V7) {
|
|
|
|
|
dt = 1.0 / conf_now->foc_f_zv;
|
|
|
|
|
} else {
|
|
|
|
|
dt = 1.0 / (conf_now->foc_f_zv / 2.0);
|
|
|
|
|
}
|
|
|
|
|
#else
|
|
|
|
|
float dt = 1.0 / (conf_now->foc_f_zv / 2.0);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
if (conf_other->foc_control_sample_mode == FOC_CONTROL_SAMPLE_MODE_V0_V7_INTERPOL && !skip_interpolation) {
|
|
|
|
|
float interpolated_phase = motor_other->m_motor_state.phase + motor_other->m_speed_est_fast * dt * 0.5;
|
2023-09-25 04:43:43 -07:00
|
|
|
|
utils_norm_angle_rad(&interpolated_phase);
|
2023-09-23 05:45:00 -07:00
|
|
|
|
|
|
|
|
|
float s, c;
|
|
|
|
|
utils_fast_sincos_better(interpolated_phase, &s, &c);
|
|
|
|
|
|
|
|
|
|
volatile motor_state_t *state_m = &(motor_other->m_motor_state);
|
|
|
|
|
state_m->phase_sin = s;
|
|
|
|
|
state_m->phase_cos = c;
|
|
|
|
|
state_m->mod_alpha_raw = c * state_m->mod_d - s * state_m->mod_q;
|
|
|
|
|
state_m->mod_beta_raw = c * state_m->mod_q + s * state_m->mod_d;
|
|
|
|
|
|
|
|
|
|
uint32_t duty1, duty2, duty3, top;
|
|
|
|
|
top = TIM1->ARR;
|
|
|
|
|
foc_svm(state_m->mod_alpha_raw, state_m->mod_beta_raw,
|
|
|
|
|
top, &duty1, &duty2, &duty3, (uint32_t*)&state_m->svm_sector);
|
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
if (is_second_motor) {
|
|
|
|
|
TIMER_UPDATE_DUTY_M1(duty1, duty2, duty3);
|
|
|
|
|
#ifdef HW_HAS_DUAL_PARALLEL
|
|
|
|
|
TIMER_UPDATE_DUTY_M2(duty1, duty2, duty3);
|
|
|
|
|
#endif
|
|
|
|
|
} else {
|
|
|
|
|
#ifndef HW_HAS_DUAL_PARALLEL
|
|
|
|
|
TIMER_UPDATE_DUTY_M2(duty1, duty2, duty3);
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
#else
|
|
|
|
|
TIMER_UPDATE_DUTY_M1(duty1, duty2, duty3);
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (do_return) {
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Reset the watchdog
|
2019-01-24 07:19:44 -08:00
|
|
|
|
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
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2021-03-24 05:09:05 -07:00
|
|
|
|
float curr0 = 0;
|
|
|
|
|
float curr1 = 0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
if (is_second_motor) {
|
|
|
|
|
curr0 = GET_CURRENT1_M2();
|
|
|
|
|
curr1 = GET_CURRENT2_M2();
|
|
|
|
|
} else {
|
|
|
|
|
curr0 = GET_CURRENT1();
|
|
|
|
|
curr1 = GET_CURRENT2();
|
|
|
|
|
}
|
|
|
|
|
#else
|
2021-02-28 11:36:02 -08:00
|
|
|
|
float curr0 = GET_CURRENT1();
|
|
|
|
|
float curr1 = GET_CURRENT2();
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_PARALLEL
|
|
|
|
|
curr0 += GET_CURRENT1_M2();
|
|
|
|
|
curr1 += GET_CURRENT2_M2();
|
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
2016-06-27 08:29:09 -07:00
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2021-02-28 11:36:02 -08:00
|
|
|
|
float curr2 = is_second_motor ? GET_CURRENT3_M2() : GET_CURRENT3();
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#else
|
2021-02-28 11:36:02 -08:00
|
|
|
|
float curr2 = GET_CURRENT3();
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_PARALLEL
|
|
|
|
|
curr2 += GET_CURRENT3_M2();
|
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
2016-06-27 08:29:09 -07:00
|
|
|
|
#endif
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-03-24 05:09:05 -07:00
|
|
|
|
motor_now->m_currents_adc[0] = curr0;
|
|
|
|
|
motor_now->m_currents_adc[1] = curr1;
|
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
|
motor_now->m_currents_adc[2] = curr2;
|
|
|
|
|
#else
|
|
|
|
|
motor_now->m_currents_adc[2] = 0.0;
|
|
|
|
|
#endif
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
curr0 -= conf_now->foc_offsets_current[0];
|
|
|
|
|
curr1 -= conf_now->foc_offsets_current[1];
|
2016-06-27 08:29:09 -07:00
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
2021-02-28 11:36:02 -08:00
|
|
|
|
curr2 -= conf_now->foc_offsets_current[2];
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_curr_unbalance = curr0 + curr1 + curr2;
|
2016-06-27 08:29:09 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
ADC_curr_norm_value[0 + norm_curr_ofs] = curr0;
|
|
|
|
|
ADC_curr_norm_value[1 + norm_curr_ofs] = curr1;
|
2016-06-27 08:29:09 -07:00
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
2020-03-16 10:32:39 -07:00
|
|
|
|
ADC_curr_norm_value[2 + norm_curr_ofs] = curr2;
|
2016-06-27 08:29:09 -07:00
|
|
|
|
#else
|
2020-03-16 10:32:39 -07:00
|
|
|
|
ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
|
2016-06-27 08:29:09 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
|
// Use the best current samples depending on the modulation state.
|
2017-09-04 12:12:43 -07:00
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (conf_now->foc_current_sample_mode == FOC_CURRENT_SAMPLE_MODE_HIGH_CURRENT) {
|
2017-09-04 12:12:43 -07:00
|
|
|
|
// High current sampling mode. Choose the lower currents to derive the highest one
|
|
|
|
|
// in order to be able to measure higher currents.
|
2020-03-16 10:32:39 -07:00
|
|
|
|
const float i0_abs = fabsf(ADC_curr_norm_value[0 + norm_curr_ofs]);
|
|
|
|
|
const float i1_abs = fabsf(ADC_curr_norm_value[1 + norm_curr_ofs]);
|
|
|
|
|
const float i2_abs = fabsf(ADC_curr_norm_value[2 + norm_curr_ofs]);
|
2017-09-04 12:12:43 -07:00
|
|
|
|
|
|
|
|
|
if (i0_abs > i1_abs && i0_abs > i2_abs) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
ADC_curr_norm_value[0 + norm_curr_ofs] = -(ADC_curr_norm_value[1 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
|
2017-09-04 12:12:43 -07:00
|
|
|
|
} else if (i1_abs > i0_abs && i1_abs > i2_abs) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
ADC_curr_norm_value[1 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
|
2017-09-04 12:12:43 -07:00
|
|
|
|
} else if (i2_abs > i0_abs && i2_abs > i1_abs) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
|
2017-09-04 12:12:43 -07:00
|
|
|
|
}
|
2023-09-23 05:45:00 -07:00
|
|
|
|
} else if (conf_now->foc_current_sample_mode == FOC_CURRENT_SAMPLE_MODE_LONGEST_ZERO) {
|
2017-09-04 12:12:43 -07:00
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
2020-01-28 10:46:19 -08:00
|
|
|
|
if (is_v7) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (tim->CCR1 > 500 && tim->CCR2 > 500) {
|
2020-01-28 10:46:19 -08:00
|
|
|
|
// Use the same 2 shunts on low modulation, as that will avoid jumps in the current reading.
|
|
|
|
|
// This is especially important when using HFI.
|
2020-03-16 10:32:39 -07:00
|
|
|
|
ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
} else {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (tim->CCR1 < tim->CCR2 && tim->CCR1 < tim->CCR3) {
|
|
|
|
|
ADC_curr_norm_value[0 + norm_curr_ofs] = -(ADC_curr_norm_value[1 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
|
|
|
|
|
} else if (tim->CCR2 < tim->CCR1 && tim->CCR2 < tim->CCR3) {
|
|
|
|
|
ADC_curr_norm_value[1 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
|
|
|
|
|
} else if (tim->CCR3 < tim->CCR1 && tim->CCR3 < tim->CCR2) {
|
|
|
|
|
ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
}
|
2017-09-04 12:12:43 -07:00
|
|
|
|
}
|
|
|
|
|
} else {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (tim->CCR1 < (tim->ARR - 500) && tim->CCR2 < (tim->ARR - 500)) {
|
2020-01-28 10:46:19 -08:00
|
|
|
|
// Use the same 2 shunts on low modulation, as that will avoid jumps in the current reading.
|
|
|
|
|
// This is especially important when using HFI.
|
2020-03-16 10:32:39 -07:00
|
|
|
|
ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
} else {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (tim->CCR1 > tim->CCR2 && tim->CCR1 > tim->CCR3) {
|
|
|
|
|
ADC_curr_norm_value[0 + norm_curr_ofs] = -(ADC_curr_norm_value[1 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
|
|
|
|
|
} else if (tim->CCR2 > tim->CCR1 && tim->CCR2 > tim->CCR3) {
|
|
|
|
|
ADC_curr_norm_value[1 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
|
|
|
|
|
} else if (tim->CCR3 > tim->CCR1 && tim->CCR3 > tim->CCR2) {
|
|
|
|
|
ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
}
|
2017-09-04 12:12:43 -07:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#else
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (tim->CCR1 < (tim->ARR - 500) && tim->CCR2 < (tim->ARR - 500)) {
|
|
|
|
|
// Use the same 2 shunts on low modulation, as that will avoid jumps in the current reading.
|
|
|
|
|
// This is especially important when using HFI.
|
|
|
|
|
ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
|
|
|
|
|
} else {
|
|
|
|
|
if (tim->CCR1 > tim->CCR2 && tim->CCR1 > tim->CCR3) {
|
|
|
|
|
ADC_curr_norm_value[0 + norm_curr_ofs] = -(ADC_curr_norm_value[1 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
|
|
|
|
|
} else if (tim->CCR2 > tim->CCR1 && tim->CCR2 > tim->CCR3) {
|
|
|
|
|
ADC_curr_norm_value[1 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
|
|
|
|
|
} else if (tim->CCR3 > tim->CCR1 && tim->CCR3 > tim->CCR2) {
|
|
|
|
|
ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
|
|
|
|
|
}
|
2017-09-04 12:12:43 -07:00
|
|
|
|
}
|
|
|
|
|
#endif
|
2016-06-27 08:29:09 -07:00
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
float ia = ADC_curr_norm_value[0 + norm_curr_ofs] * FAC_CURRENT;
|
|
|
|
|
float ib = ADC_curr_norm_value[1 + norm_curr_ofs] * FAC_CURRENT;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
float ic = ADC_curr_norm_value[2 + norm_curr_ofs] * FAC_CURRENT;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
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.
|
2020-11-18 05:26:55 -08:00
|
|
|
|
dt *= (float)FOC_CONTROL_LOOP_FREQ_DIVIDER;
|
2019-02-19 09:55:18 -08:00
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
UTILS_LP_FAST(motor_now->m_motor_state.v_bus, GET_INPUT_VOLTAGE(), 0.1);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-06-06 14:21:53 -07:00
|
|
|
|
volatile float enc_ang = 0;
|
|
|
|
|
volatile bool encoder_is_being_used = false;
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
if (virtual_motor_is_connected()) {
|
|
|
|
|
if (conf_now->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER ) {
|
2019-04-06 06:36:00 -07:00
|
|
|
|
enc_ang = virtual_motor_get_angle_deg();
|
2020-06-06 14:21:53 -07:00
|
|
|
|
encoder_is_being_used = true;
|
|
|
|
|
}
|
2021-02-28 11:36:02 -08:00
|
|
|
|
} else {
|
2022-01-09 08:10:40 -08:00
|
|
|
|
if (encoder_is_configured()) {
|
|
|
|
|
enc_ang = encoder_read_deg();
|
2020-06-06 14:21:53 -07:00
|
|
|
|
encoder_is_being_used = true;
|
2019-04-06 06:36:00 -07:00
|
|
|
|
}
|
2020-06-06 14:21:53 -07:00
|
|
|
|
}
|
2019-04-06 06:36:00 -07:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
if (encoder_is_being_used) {
|
2016-04-27 06:32:32 -07:00
|
|
|
|
float phase_tmp = enc_ang;
|
2020-03-21 03:40:50 -07:00
|
|
|
|
if (conf_now->foc_encoder_inverted) {
|
2016-04-27 06:32:32 -07:00
|
|
|
|
phase_tmp = 360.0 - phase_tmp;
|
|
|
|
|
}
|
2020-03-21 03:40:50 -07:00
|
|
|
|
phase_tmp *= conf_now->foc_encoder_ratio;
|
|
|
|
|
phase_tmp -= conf_now->foc_encoder_offset;
|
2016-04-27 06:32:32 -07:00
|
|
|
|
utils_norm_angle((float*)&phase_tmp);
|
2021-10-13 09:13:51 -07:00
|
|
|
|
motor_now->m_phase_now_encoder = DEG2RAD_f(phase_tmp);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
if (motor_now->m_state == MC_STATE_RUNNING) {
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (conf_now->foc_current_sample_mode == FOC_CURRENT_SAMPLE_MODE_ALL_SENSORS) {
|
|
|
|
|
// Full Clarke Transform
|
|
|
|
|
motor_now->m_motor_state.i_alpha = (2.0 / 3.0) * ia - (1.0 / 3.0) * ib - (1.0 / 3.0) * ic;
|
|
|
|
|
motor_now->m_motor_state.i_beta = ONE_BY_SQRT3 * ib - ONE_BY_SQRT3 * ic;
|
|
|
|
|
} else {
|
|
|
|
|
// Clarke transform assuming balanced currents
|
|
|
|
|
motor_now->m_motor_state.i_alpha = ia;
|
|
|
|
|
motor_now->m_motor_state.i_beta = ONE_BY_SQRT3 * ia + TWO_BY_SQRT3 * ib;
|
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-03-23 14:26:05 -07:00
|
|
|
|
motor_now->m_i_alpha_sample_with_offset = motor_now->m_motor_state.i_alpha;
|
|
|
|
|
motor_now->m_i_beta_sample_with_offset = motor_now->m_motor_state.i_beta;
|
|
|
|
|
|
|
|
|
|
if (motor_now->m_i_alpha_beta_has_offset) {
|
|
|
|
|
motor_now->m_motor_state.i_alpha = 0.5 * (motor_now->m_motor_state.i_alpha + motor_now->m_i_alpha_sample_next);
|
|
|
|
|
motor_now->m_motor_state.i_beta = 0.5 * (motor_now->m_motor_state.i_beta + motor_now->m_i_beta_sample_next);
|
|
|
|
|
motor_now->m_i_alpha_beta_has_offset = false;
|
|
|
|
|
}
|
|
|
|
|
|
2021-11-25 15:43:41 -08:00
|
|
|
|
const float duty_now = motor_now->m_motor_state.duty_now;
|
|
|
|
|
const float duty_abs = fabsf(duty_now);
|
|
|
|
|
const float vq_now = motor_now->m_motor_state.vq;
|
2021-11-27 01:58:52 -08:00
|
|
|
|
const float speed_fast_now = motor_now->m_pll_speed;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
float id_set_tmp = motor_now->m_id_set;
|
|
|
|
|
float iq_set_tmp = motor_now->m_iq_set;
|
|
|
|
|
motor_now->m_motor_state.max_duty = conf_now->l_max_duty;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-11-27 07:38:32 -08:00
|
|
|
|
if (motor_now->m_control_mode == CONTROL_MODE_CURRENT_BRAKE) {
|
|
|
|
|
utils_truncate_number_abs(&iq_set_tmp, -conf_now->lo_current_min);
|
|
|
|
|
}
|
|
|
|
|
|
2021-11-27 14:32:08 -08:00
|
|
|
|
UTILS_LP_FAST(motor_now->m_duty_abs_filtered, duty_abs, 0.01);
|
2021-08-16 11:33:26 -07:00
|
|
|
|
utils_truncate_number_abs((float*)&motor_now->m_duty_abs_filtered, 1.0);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-11-25 15:43:41 -08:00
|
|
|
|
UTILS_LP_FAST(motor_now->m_duty_filtered, duty_now, 0.01);
|
2021-08-20 12:36:44 -07:00
|
|
|
|
utils_truncate_number_abs((float*)&motor_now->m_duty_filtered, 1.0);
|
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
float duty_set = motor_now->m_duty_cycle_set;
|
|
|
|
|
bool control_duty = motor_now->m_control_mode == CONTROL_MODE_DUTY ||
|
|
|
|
|
motor_now->m_control_mode == CONTROL_MODE_OPENLOOP_DUTY ||
|
|
|
|
|
motor_now->m_control_mode == CONTROL_MODE_OPENLOOP_DUTY_PHASE;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-11-25 15:43:41 -08:00
|
|
|
|
// Short all phases (duty=0) the moment the direction or modulation changes sign. That will avoid
|
|
|
|
|
// active braking or changing direction. Keep all phases shorted (duty == 0) until the
|
|
|
|
|
// braking current reaches the set or maximum value, then go back to current control
|
2021-11-27 09:51:02 -08:00
|
|
|
|
// mode. Stay in duty=0 for at least 10 cycles to avoid jumping in and out of that mode rapidly
|
|
|
|
|
// around the threshold.
|
|
|
|
|
if (motor_now->m_control_mode == CONTROL_MODE_CURRENT_BRAKE) {
|
|
|
|
|
if ((SIGN(speed_fast_now) != SIGN(motor_now->m_br_speed_before) ||
|
|
|
|
|
SIGN(vq_now) != SIGN(motor_now->m_br_vq_before) ||
|
|
|
|
|
fabsf(motor_now->m_duty_filtered) < 0.001 || motor_now->m_br_no_duty_samples < 10) &&
|
|
|
|
|
motor_now->m_motor_state.i_abs_filter < fabsf(iq_set_tmp)) {
|
|
|
|
|
control_duty = true;
|
|
|
|
|
duty_set = 0.0;
|
|
|
|
|
motor_now->m_br_no_duty_samples = 0;
|
|
|
|
|
} else if (motor_now->m_br_no_duty_samples < 10) {
|
|
|
|
|
control_duty = true;
|
|
|
|
|
duty_set = 0.0;
|
|
|
|
|
motor_now->m_br_no_duty_samples++;
|
|
|
|
|
}
|
|
|
|
|
} else {
|
2023-10-16 03:00:41 -07:00
|
|
|
|
motor_now->m_br_no_duty_samples = 100;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
2021-11-27 09:51:02 -08:00
|
|
|
|
|
|
|
|
|
motor_now->m_br_speed_before = speed_fast_now;
|
|
|
|
|
motor_now->m_br_vq_before = vq_now;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2016-02-24 12:17:39 -08:00
|
|
|
|
// Brake when set ERPM is below min ERPM
|
2020-03-21 03:40:50 -07:00
|
|
|
|
if (motor_now->m_control_mode == CONTROL_MODE_SPEED &&
|
|
|
|
|
fabsf(motor_now->m_speed_pid_set_rpm) < conf_now->s_pid_min_erpm) {
|
2016-02-24 12:17:39 -08:00
|
|
|
|
control_duty = true;
|
|
|
|
|
duty_set = 0.0;
|
|
|
|
|
}
|
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
// Reset integrator when leaving duty cycle mode, as the windup protection is not too fast. Making
|
|
|
|
|
// better windup protection is probably better, but not easy.
|
2020-03-21 03:40:50 -07:00
|
|
|
|
if (!control_duty && motor_now->m_was_control_duty) {
|
|
|
|
|
motor_now->m_motor_state.vq_int = motor_now->m_motor_state.vq;
|
|
|
|
|
if (conf_now->foc_cc_decoupling == FOC_CC_DECOUPLING_BEMF ||
|
|
|
|
|
conf_now->foc_cc_decoupling == FOC_CC_DECOUPLING_CROSS_BEMF) {
|
2023-07-01 05:51:08 -07:00
|
|
|
|
motor_now->m_motor_state.vq_int -= motor_now->m_pll_speed * conf_now->foc_motor_flux_linkage;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
}
|
|
|
|
|
}
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_was_control_duty = control_duty;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2023-10-16 04:37:15 -07:00
|
|
|
|
float current_max_for_duty = conf_now->lo_current_max;
|
|
|
|
|
if (motor_now->m_control_mode == CONTROL_MODE_CURRENT_BRAKE) {
|
|
|
|
|
current_max_for_duty = fabsf(conf_now->lo_current_min);
|
|
|
|
|
}
|
|
|
|
|
|
2021-11-27 14:32:08 -08:00
|
|
|
|
if (!control_duty) {
|
2023-10-16 04:37:15 -07:00
|
|
|
|
motor_now->m_duty_i_term = motor_now->m_motor_state.iq / current_max_for_duty;
|
2022-12-10 14:48:15 -08:00
|
|
|
|
motor_now->duty_was_pi = false;
|
2021-11-27 14:32:08 -08:00
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
if (control_duty) {
|
|
|
|
|
// Duty cycle control
|
2022-12-11 02:33:47 -08:00
|
|
|
|
if (fabsf(duty_set) < (duty_abs - 0.01) &&
|
2022-12-10 14:34:11 -08:00
|
|
|
|
(!motor_now->duty_was_pi || SIGN(motor_now->duty_pi_duty_last) == SIGN(duty_now))) {
|
2022-12-11 01:37:34 -08:00
|
|
|
|
// Truncating the duty cycle here would be dangerous, so run a PI controller.
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-12-10 14:34:11 -08:00
|
|
|
|
motor_now->duty_pi_duty_last = duty_now;
|
|
|
|
|
motor_now->duty_was_pi = true;
|
|
|
|
|
|
2023-04-12 23:50:57 -07:00
|
|
|
|
// Reset the integrator in duty mode to not increase the duty if the load suddenly changes. In braking
|
|
|
|
|
// mode this would cause a discontinuity, so there we want to keep the value of the integrator.
|
|
|
|
|
if (motor_now->m_control_mode == CONTROL_MODE_DUTY) {
|
|
|
|
|
if (duty_now > 0.0) {
|
|
|
|
|
if (motor_now->m_duty_i_term > 0.0) {
|
|
|
|
|
motor_now->m_duty_i_term = 0.0;
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
if (motor_now->m_duty_i_term < 0.0) {
|
|
|
|
|
motor_now->m_duty_i_term = 0.0;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Compute error
|
2020-03-21 03:40:50 -07:00
|
|
|
|
float error = duty_set - motor_now->m_motor_state.duty_now;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Compute parameters
|
2022-12-10 14:34:11 -08:00
|
|
|
|
float scale = 1.0 / motor_now->m_motor_state.v_bus;
|
2020-03-21 03:40:50 -07:00
|
|
|
|
float p_term = error * conf_now->foc_duty_dowmramp_kp * scale;
|
|
|
|
|
motor_now->m_duty_i_term += error * (conf_now->foc_duty_dowmramp_ki * dt) * scale;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// I-term wind-up protection
|
2020-03-21 03:40:50 -07:00
|
|
|
|
utils_truncate_number((float*)&motor_now->m_duty_i_term, -1.0, 1.0);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
|
|
|
|
// Calculate output
|
2020-03-21 03:40:50 -07:00
|
|
|
|
float output = p_term + motor_now->m_duty_i_term;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
utils_truncate_number(&output, -1.0, 1.0);
|
2023-10-16 04:37:15 -07:00
|
|
|
|
iq_set_tmp = output * current_max_for_duty;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
} 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.
|
2023-10-16 04:37:15 -07:00
|
|
|
|
motor_now->m_duty_i_term = motor_now->m_motor_state.iq / current_max_for_duty;
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_motor_state.max_duty = duty_set;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
if (duty_set > 0.0) {
|
2023-10-16 04:37:15 -07:00
|
|
|
|
iq_set_tmp = current_max_for_duty;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
} else {
|
2023-10-16 04:37:15 -07:00
|
|
|
|
iq_set_tmp = -current_max_for_duty;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
2022-12-10 14:34:11 -08:00
|
|
|
|
motor_now->duty_was_pi = false;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
2020-03-21 03:40:50 -07:00
|
|
|
|
} else if (motor_now->m_control_mode == CONTROL_MODE_CURRENT_BRAKE) {
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Braking
|
2021-11-27 01:58:52 -08:00
|
|
|
|
iq_set_tmp = -SIGN(speed_fast_now) * fabsf(iq_set_tmp);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
// Set motor phase
|
|
|
|
|
{
|
2020-03-21 03:40:50 -07:00
|
|
|
|
if (!motor_now->m_phase_override) {
|
2022-04-06 06:12:24 -07:00
|
|
|
|
foc_observer_update(motor_now->m_motor_state.v_alpha, motor_now->m_motor_state.v_beta,
|
|
|
|
|
motor_now->m_motor_state.i_alpha, motor_now->m_motor_state.i_beta,
|
2022-05-21 13:54:12 -07:00
|
|
|
|
dt, &(motor_now->m_observer_state), &motor_now->m_phase_now_observer, motor_now);
|
2021-12-19 14:55:03 -08:00
|
|
|
|
|
|
|
|
|
// Compensate from the phase lag caused by the switching frequency. This is important for motors
|
|
|
|
|
// that run on high ERPM compared to the switching frequency.
|
2022-01-12 03:27:45 -08:00
|
|
|
|
motor_now->m_phase_now_observer += motor_now->m_pll_speed * dt * (0.5 + conf_now->foc_observer_offset);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
utils_norm_angle_rad((float*)&motor_now->m_phase_now_observer);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
2016-04-27 06:32:32 -07:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
switch (conf_now->foc_sensor_mode) {
|
|
|
|
|
case FOC_SENSOR_MODE_ENCODER:
|
2022-01-09 08:10:40 -08:00
|
|
|
|
if (encoder_index_found() || virtual_motor_is_connected()) {
|
2022-04-06 06:12:24 -07:00
|
|
|
|
motor_now->m_motor_state.phase = foc_correct_encoder(
|
2021-02-28 11:36:02 -08:00
|
|
|
|
motor_now->m_phase_now_observer,
|
|
|
|
|
motor_now->m_phase_now_encoder,
|
|
|
|
|
motor_now->m_speed_est_fast,
|
|
|
|
|
conf_now->foc_sl_erpm,
|
|
|
|
|
motor_now);
|
|
|
|
|
} else {
|
|
|
|
|
// Rotate the motor in open loop if the index isn't found.
|
|
|
|
|
motor_now->m_motor_state.phase = motor_now->m_phase_now_encoder_no_index;
|
|
|
|
|
}
|
|
|
|
|
|
2021-12-23 12:25:31 -08:00
|
|
|
|
if (!motor_now->m_phase_override && motor_now->m_control_mode != CONTROL_MODE_OPENLOOP_PHASE) {
|
2021-02-28 11:36:02 -08:00
|
|
|
|
id_set_tmp = 0.0;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
case FOC_SENSOR_MODE_HALL:
|
2023-08-08 10:56:48 -07:00
|
|
|
|
motor_now->m_motor_state.phase = foc_correct_hall(motor_now->m_phase_now_observer, dt, motor_now,
|
2022-04-06 06:12:24 -07:00
|
|
|
|
utils_read_hall(motor_now != &m_motor_1, conf_now->m_hall_extra_samples));
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-12-23 12:25:31 -08:00
|
|
|
|
if (!motor_now->m_phase_override && motor_now->m_control_mode != CONTROL_MODE_OPENLOOP_PHASE) {
|
2021-02-28 11:36:02 -08:00
|
|
|
|
id_set_tmp = 0.0;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
case FOC_SENSOR_MODE_SENSORLESS:
|
|
|
|
|
if (motor_now->m_phase_observer_override) {
|
|
|
|
|
motor_now->m_motor_state.phase = motor_now->m_phase_now_observer_override;
|
2022-05-21 13:54:12 -07:00
|
|
|
|
motor_now->m_observer_state.x1 = motor_now->m_observer_x1_override;
|
|
|
|
|
motor_now->m_observer_state.x2 = motor_now->m_observer_x2_override;
|
2022-08-21 09:13:48 -07:00
|
|
|
|
iq_set_tmp += conf_now->foc_sl_openloop_boost_q * SIGN(iq_set_tmp);
|
|
|
|
|
if (conf_now->foc_sl_openloop_max_q > conf_now->cc_min_current) {
|
|
|
|
|
utils_truncate_number_abs(&iq_set_tmp, conf_now->foc_sl_openloop_max_q);
|
|
|
|
|
}
|
2021-02-28 11:36:02 -08:00
|
|
|
|
} else {
|
|
|
|
|
motor_now->m_motor_state.phase = motor_now->m_phase_now_observer;
|
|
|
|
|
}
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2021-12-23 12:25:31 -08:00
|
|
|
|
if (!motor_now->m_phase_override && motor_now->m_control_mode != CONTROL_MODE_OPENLOOP_PHASE) {
|
2021-02-28 11:36:02 -08:00
|
|
|
|
id_set_tmp = 0.0;
|
|
|
|
|
}
|
|
|
|
|
break;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2021-03-13 06:41:57 -08:00
|
|
|
|
case FOC_SENSOR_MODE_HFI_START:
|
|
|
|
|
motor_now->m_motor_state.phase = motor_now->m_phase_now_observer;
|
|
|
|
|
|
|
|
|
|
if (motor_now->m_phase_observer_override) {
|
|
|
|
|
motor_now->m_hfi.est_done_cnt = 0;
|
|
|
|
|
motor_now->m_hfi.flip_cnt = 0;
|
|
|
|
|
|
|
|
|
|
motor_now->m_min_rpm_hyst_timer = 0.0;
|
|
|
|
|
motor_now->m_min_rpm_timer = 0.0;
|
|
|
|
|
motor_now->m_phase_observer_override = false;
|
|
|
|
|
}
|
|
|
|
|
|
2021-12-23 12:25:31 -08:00
|
|
|
|
if (!motor_now->m_phase_override && motor_now->m_control_mode != CONTROL_MODE_OPENLOOP_PHASE) {
|
2021-03-13 06:41:57 -08:00
|
|
|
|
id_set_tmp = 0.0;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
case FOC_SENSOR_MODE_HFI:
|
2022-03-23 07:04:30 -07:00
|
|
|
|
case FOC_SENSOR_MODE_HFI_V2:
|
2022-03-23 14:26:05 -07:00
|
|
|
|
case FOC_SENSOR_MODE_HFI_V3:
|
2022-04-05 15:35:11 -07:00
|
|
|
|
case FOC_SENSOR_MODE_HFI_V4:
|
|
|
|
|
case FOC_SENSOR_MODE_HFI_V5:
|
2021-10-27 11:07:58 -07:00
|
|
|
|
if (fabsf(RADPS2RPM_f(motor_now->m_speed_est_fast)) > conf_now->foc_sl_erpm_hfi) {
|
2021-02-28 11:36:02 -08:00
|
|
|
|
motor_now->m_hfi.observer_zero_time = 0;
|
|
|
|
|
} else {
|
|
|
|
|
motor_now->m_hfi.observer_zero_time += dt;
|
|
|
|
|
}
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
if (motor_now->m_hfi.observer_zero_time < conf_now->foc_hfi_obs_ovr_sec) {
|
|
|
|
|
motor_now->m_hfi.angle = motor_now->m_phase_now_observer;
|
2022-04-25 03:53:04 -07:00
|
|
|
|
motor_now->m_hfi.double_integrator = -motor_now->m_speed_est_fast;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
}
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-04-06 06:12:24 -07:00
|
|
|
|
motor_now->m_motor_state.phase = foc_correct_encoder(
|
2021-02-28 11:36:02 -08:00
|
|
|
|
motor_now->m_phase_now_observer,
|
|
|
|
|
motor_now->m_hfi.angle,
|
|
|
|
|
motor_now->m_speed_est_fast,
|
|
|
|
|
conf_now->foc_sl_erpm_hfi,
|
|
|
|
|
motor_now);
|
|
|
|
|
|
2021-12-23 12:25:31 -08:00
|
|
|
|
if (!motor_now->m_phase_override && motor_now->m_control_mode != CONTROL_MODE_OPENLOOP_PHASE) {
|
2021-02-28 11:36:02 -08:00
|
|
|
|
id_set_tmp = 0.0;
|
|
|
|
|
}
|
|
|
|
|
break;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
if (motor_now->m_control_mode == CONTROL_MODE_HANDBRAKE) {
|
|
|
|
|
// Force the phase to 0 in handbrake mode so that the current simply locks the rotor.
|
|
|
|
|
motor_now->m_motor_state.phase = 0.0;
|
|
|
|
|
} else if (motor_now->m_control_mode == CONTROL_MODE_OPENLOOP ||
|
|
|
|
|
motor_now->m_control_mode == CONTROL_MODE_OPENLOOP_DUTY) {
|
|
|
|
|
motor_now->m_openloop_angle += dt * motor_now->m_openloop_speed;
|
|
|
|
|
utils_norm_angle_rad((float*)&motor_now->m_openloop_angle);
|
|
|
|
|
motor_now->m_motor_state.phase = motor_now->m_openloop_angle;
|
|
|
|
|
} else if (motor_now->m_control_mode == CONTROL_MODE_OPENLOOP_PHASE ||
|
|
|
|
|
motor_now->m_control_mode == CONTROL_MODE_OPENLOOP_DUTY_PHASE) {
|
|
|
|
|
motor_now->m_motor_state.phase = motor_now->m_openloop_phase;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (motor_now->m_phase_override) {
|
|
|
|
|
motor_now->m_motor_state.phase = motor_now->m_phase_now_override;
|
|
|
|
|
}
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
utils_fast_sincos_better(motor_now->m_motor_state.phase,
|
|
|
|
|
(float*)&motor_now->m_motor_state.phase_sin,
|
|
|
|
|
(float*)&motor_now->m_motor_state.phase_cos);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2020-06-17 04:55:35 -07:00
|
|
|
|
// Apply MTPA. See: https://github.com/vedderb/bldc/pull/179
|
2020-07-21 11:08:31 -07:00
|
|
|
|
const float ld_lq_diff = conf_now->foc_motor_ld_lq_diff;
|
2021-10-24 09:10:00 -07:00
|
|
|
|
if (conf_now->foc_mtpa_mode != MTPA_MODE_OFF && ld_lq_diff != 0.0) {
|
2020-07-21 11:08:31 -07:00
|
|
|
|
const float lambda = conf_now->foc_motor_flux_linkage;
|
2020-06-17 04:55:35 -07:00
|
|
|
|
|
2021-10-24 09:10:00 -07:00
|
|
|
|
float iq_ref = iq_set_tmp;
|
|
|
|
|
if (conf_now->foc_mtpa_mode == MTPA_MODE_IQ_MEASURED) {
|
2021-11-21 09:17:40 -08:00
|
|
|
|
iq_ref = utils_min_abs(iq_set_tmp, motor_now->m_motor_state.iq_filter);
|
2021-10-24 09:10:00 -07:00
|
|
|
|
}
|
|
|
|
|
|
2022-03-07 08:33:51 -08:00
|
|
|
|
id_set_tmp = (lambda - sqrtf(SQ(lambda) + 8.0 * SQ(ld_lq_diff * iq_ref))) / (4.0 * ld_lq_diff);
|
2020-06-17 04:55:35 -07:00
|
|
|
|
iq_set_tmp = SIGN(iq_set_tmp) * sqrtf(SQ(iq_set_tmp) - SQ(id_set_tmp));
|
|
|
|
|
}
|
2020-04-30 14:34:29 -07:00
|
|
|
|
|
2021-11-07 14:00:48 -08:00
|
|
|
|
const float mod_q = motor_now->m_motor_state.mod_q_filter;
|
2021-04-15 01:04:28 -07:00
|
|
|
|
|
2021-04-11 02:31:25 -07:00
|
|
|
|
// Running FW from the 1 khz timer seems fast enough.
|
|
|
|
|
// run_fw(motor_now, dt);
|
2021-04-10 02:37:35 -07:00
|
|
|
|
id_set_tmp -= motor_now->m_i_fw_set;
|
2021-04-15 01:04:28 -07:00
|
|
|
|
iq_set_tmp -= SIGN(mod_q) * motor_now->m_i_fw_set * conf_now->foc_fw_q_current_factor;
|
2021-04-10 02:37:35 -07:00
|
|
|
|
|
2015-12-23 15:43:31 -08:00
|
|
|
|
// Apply current limits
|
2023-06-16 11:37:18 -07:00
|
|
|
|
// TODO: Consider D axis current for the input current as well. Currently this is done using
|
|
|
|
|
// l_in_current_map_start in update_override_limits.
|
2015-12-23 15:43:31 -08:00
|
|
|
|
if (mod_q > 0.001) {
|
2020-03-21 03:40:50 -07:00
|
|
|
|
utils_truncate_number(&iq_set_tmp, conf_now->lo_in_current_min / mod_q, conf_now->lo_in_current_max / mod_q);
|
2015-12-23 15:43:31 -08:00
|
|
|
|
} else if (mod_q < -0.001) {
|
2020-03-21 03:40:50 -07:00
|
|
|
|
utils_truncate_number(&iq_set_tmp, conf_now->lo_in_current_max / mod_q, conf_now->lo_in_current_min / mod_q);
|
2015-12-23 15:43:31 -08:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
|
if (mod_q > 0.0) {
|
2020-03-21 03:40:50 -07:00
|
|
|
|
utils_truncate_number(&iq_set_tmp, conf_now->lo_current_min, conf_now->lo_current_max);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
} else {
|
2020-03-21 03:40:50 -07:00
|
|
|
|
utils_truncate_number(&iq_set_tmp, -conf_now->lo_current_max, -conf_now->lo_current_min);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
}
|
|
|
|
|
|
2021-04-14 13:29:50 -07:00
|
|
|
|
float current_max_abs = fabsf(utils_max_abs(conf_now->lo_current_max, conf_now->lo_current_min));
|
|
|
|
|
utils_truncate_number_abs(&id_set_tmp, current_max_abs);
|
|
|
|
|
utils_truncate_number_abs(&iq_set_tmp, sqrtf(SQ(current_max_abs) - SQ(id_set_tmp)));
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_motor_state.id_target = id_set_tmp;
|
|
|
|
|
motor_now->m_motor_state.iq_target = iq_set_tmp;
|
2015-12-23 15:43:31 -08:00
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
control_current(motor_now, dt);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
} else {
|
2021-02-28 11:36:02 -08:00
|
|
|
|
// Motor is not running
|
|
|
|
|
|
2020-01-12 12:25:21 -08:00
|
|
|
|
// The current is 0 when the motor is undriven
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_motor_state.i_alpha = 0.0;
|
|
|
|
|
motor_now->m_motor_state.i_beta = 0.0;
|
|
|
|
|
motor_now->m_motor_state.id = 0.0;
|
|
|
|
|
motor_now->m_motor_state.iq = 0.0;
|
|
|
|
|
motor_now->m_motor_state.id_filter = 0.0;
|
|
|
|
|
motor_now->m_motor_state.iq_filter = 0.0;
|
2022-12-10 14:34:11 -08:00
|
|
|
|
motor_now->m_duty_i_term = 0.0;
|
2020-06-03 01:52:55 -07:00
|
|
|
|
#ifdef HW_HAS_INPUT_CURRENT_SENSOR
|
|
|
|
|
GET_INPUT_CURRENT_OFFSET(); // TODO: should this be done here?
|
|
|
|
|
#endif
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_motor_state.i_bus = 0.0;
|
|
|
|
|
motor_now->m_motor_state.i_abs = 0.0;
|
|
|
|
|
motor_now->m_motor_state.i_abs_filter = 0.0;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Track back emf
|
2021-02-28 11:36:02 -08:00
|
|
|
|
update_valpha_vbeta(motor_now, 0.0, 0.0);
|
2017-09-04 12:12:43 -07:00
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Run observer
|
2022-04-06 06:12:24 -07:00
|
|
|
|
foc_observer_update(motor_now->m_motor_state.v_alpha, motor_now->m_motor_state.v_beta,
|
|
|
|
|
motor_now->m_motor_state.i_alpha, motor_now->m_motor_state.i_beta,
|
2022-05-21 13:54:12 -07:00
|
|
|
|
dt, &(motor_now->m_observer_state), 0, motor_now);
|
|
|
|
|
motor_now->m_phase_now_observer = utils_fast_atan2(motor_now->m_x2_prev + motor_now->m_observer_state.x2,
|
|
|
|
|
motor_now->m_x1_prev + motor_now->m_observer_state.x1);
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2022-01-12 03:27:45 -08:00
|
|
|
|
// The observer phase offset has to be added here as well, with 0.5 switching cycles offset
|
|
|
|
|
// compared to when running. Otherwise going from undriven to driven causes a current
|
|
|
|
|
// spike.
|
|
|
|
|
motor_now->m_phase_now_observer += motor_now->m_pll_speed * dt * conf_now->foc_observer_offset;
|
2022-01-12 03:32:38 -08:00
|
|
|
|
utils_norm_angle_rad((float*)&motor_now->m_phase_now_observer);
|
2022-01-12 03:27:45 -08:00
|
|
|
|
|
2022-05-21 13:54:12 -07:00
|
|
|
|
motor_now->m_x1_prev = motor_now->m_observer_state.x1;
|
|
|
|
|
motor_now->m_x2_prev = motor_now->m_observer_state.x2;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
// Set motor phase
|
|
|
|
|
{
|
|
|
|
|
switch (conf_now->foc_sensor_mode) {
|
|
|
|
|
case FOC_SENSOR_MODE_ENCODER:
|
2022-04-06 06:12:24 -07:00
|
|
|
|
motor_now->m_motor_state.phase = foc_correct_encoder(
|
2021-02-28 11:36:02 -08:00
|
|
|
|
motor_now->m_phase_now_observer,
|
|
|
|
|
motor_now->m_phase_now_encoder,
|
|
|
|
|
motor_now->m_speed_est_fast,
|
|
|
|
|
conf_now->foc_sl_erpm,
|
|
|
|
|
motor_now);
|
|
|
|
|
break;
|
|
|
|
|
case FOC_SENSOR_MODE_HALL:
|
2023-08-08 10:56:48 -07:00
|
|
|
|
motor_now->m_motor_state.phase = foc_correct_hall(motor_now->m_phase_now_observer, dt, motor_now,
|
2022-04-06 06:12:24 -07:00
|
|
|
|
utils_read_hall(motor_now != &m_motor_1, conf_now->m_hall_extra_samples));
|
2021-02-28 11:36:02 -08:00
|
|
|
|
break;
|
|
|
|
|
case FOC_SENSOR_MODE_SENSORLESS:
|
|
|
|
|
motor_now->m_motor_state.phase = motor_now->m_phase_now_observer;
|
|
|
|
|
break;
|
2021-03-13 02:42:23 -08:00
|
|
|
|
case FOC_SENSOR_MODE_HFI:
|
2022-03-23 07:04:30 -07:00
|
|
|
|
case FOC_SENSOR_MODE_HFI_V2:
|
2022-03-23 14:26:05 -07:00
|
|
|
|
case FOC_SENSOR_MODE_HFI_V3:
|
2022-04-05 15:35:11 -07:00
|
|
|
|
case FOC_SENSOR_MODE_HFI_V4:
|
|
|
|
|
case FOC_SENSOR_MODE_HFI_V5:
|
2021-03-13 02:42:23 -08:00
|
|
|
|
case FOC_SENSOR_MODE_HFI_START:{
|
2021-02-28 11:36:02 -08:00
|
|
|
|
motor_now->m_motor_state.phase = motor_now->m_phase_now_observer;
|
2021-10-13 09:13:51 -07:00
|
|
|
|
if (fabsf(RADPS2RPM_f(motor_now->m_pll_speed)) < (conf_now->foc_sl_erpm_hfi * 1.1)) {
|
2021-02-28 11:36:02 -08:00
|
|
|
|
motor_now->m_hfi.est_done_cnt = 0;
|
2021-03-13 06:41:57 -08:00
|
|
|
|
motor_now->m_hfi.flip_cnt = 0;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
}
|
|
|
|
|
} break;
|
2021-10-13 08:26:19 -07:00
|
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
|
|
|
|
utils_fast_sincos_better(motor_now->m_motor_state.phase,
|
|
|
|
|
(float*)&motor_now->m_motor_state.phase_sin,
|
|
|
|
|
(float*)&motor_now->m_motor_state.phase_cos);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
|
// HFI Restore
|
2022-06-08 05:00:43 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
if (is_second_motor) {
|
|
|
|
|
CURRENT_FILTER_ON_M2();
|
|
|
|
|
} else {
|
|
|
|
|
CURRENT_FILTER_ON();
|
|
|
|
|
}
|
|
|
|
|
#else
|
2020-01-20 00:39:33 -08:00
|
|
|
|
CURRENT_FILTER_ON();
|
2022-06-08 05:00:43 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_hfi.ind = 0;
|
|
|
|
|
motor_now->m_hfi.ready = false;
|
2022-04-05 15:35:11 -07:00
|
|
|
|
motor_now->m_hfi.double_integrator = 0.0;
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_hfi.is_samp_n = false;
|
|
|
|
|
motor_now->m_hfi.prev_sample = 0.0;
|
|
|
|
|
motor_now->m_hfi.angle = motor_now->m_motor_state.phase;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
float s = motor_now->m_motor_state.phase_sin;
|
|
|
|
|
float c = motor_now->m_motor_state.phase_cos;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
|
|
|
|
// Park transform
|
2020-03-21 03:40:50 -07:00
|
|
|
|
float vd_tmp = c * motor_now->m_motor_state.v_alpha + s * motor_now->m_motor_state.v_beta;
|
|
|
|
|
float vq_tmp = c * motor_now->m_motor_state.v_beta - s * motor_now->m_motor_state.v_alpha;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
UTILS_NAN_ZERO(motor_now->m_motor_state.vd);
|
|
|
|
|
UTILS_NAN_ZERO(motor_now->m_motor_state.vq);
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
UTILS_LP_FAST(motor_now->m_motor_state.vd, vd_tmp, 0.2);
|
|
|
|
|
UTILS_LP_FAST(motor_now->m_motor_state.vq, vq_tmp, 0.2);
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
|
|
|
|
// Set the current controller integrator to the BEMF voltage to avoid
|
|
|
|
|
// a current spike when the motor is driven again. Notice that we have
|
|
|
|
|
// to take decoupling into account.
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_motor_state.vd_int = motor_now->m_motor_state.vd;
|
|
|
|
|
motor_now->m_motor_state.vq_int = motor_now->m_motor_state.vq;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
if (conf_now->foc_cc_decoupling == FOC_CC_DECOUPLING_BEMF ||
|
|
|
|
|
conf_now->foc_cc_decoupling == FOC_CC_DECOUPLING_CROSS_BEMF) {
|
2023-07-01 05:51:08 -07:00
|
|
|
|
motor_now->m_motor_state.vq_int -= motor_now->m_pll_speed * conf_now->foc_motor_flux_linkage;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Update corresponding modulation
|
2021-11-07 04:37:09 -08:00
|
|
|
|
/* voltage_normalize = 1/(2/3*V_bus) */
|
|
|
|
|
const float voltage_normalize = 1.5 / motor_now->m_motor_state.v_bus;
|
2021-11-07 04:35:08 -08:00
|
|
|
|
|
2021-11-07 04:37:09 -08:00
|
|
|
|
motor_now->m_motor_state.mod_d = motor_now->m_motor_state.vd * voltage_normalize;
|
|
|
|
|
motor_now->m_motor_state.mod_q = motor_now->m_motor_state.vq * voltage_normalize;
|
2021-11-07 14:00:48 -08:00
|
|
|
|
UTILS_NAN_ZERO(motor_now->m_motor_state.mod_q_filter);
|
|
|
|
|
UTILS_LP_FAST(motor_now->m_motor_state.mod_q_filter, motor_now->m_motor_state.mod_q, 0.2);
|
2021-11-15 12:29:05 -08:00
|
|
|
|
utils_truncate_number_abs((float*)&motor_now->m_motor_state.mod_q_filter, 1.0);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Calculate duty cycle
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_motor_state.duty_now = SIGN(motor_now->m_motor_state.vq) *
|
2022-03-07 08:33:51 -08:00
|
|
|
|
NORM2_f(motor_now->m_motor_state.mod_d, motor_now->m_motor_state.mod_q) * TWO_BY_SQRT3;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
float phase_for_speed_est = 0.0;
|
|
|
|
|
switch (conf_now->foc_speed_soure) {
|
2023-07-01 05:51:08 -07:00
|
|
|
|
case FOC_SPEED_SRC_CORRECTED:
|
2022-03-23 07:04:30 -07:00
|
|
|
|
phase_for_speed_est = motor_now->m_motor_state.phase;
|
|
|
|
|
break;
|
2023-07-01 05:51:08 -07:00
|
|
|
|
case FOC_SPEED_SRC_OBSERVER:
|
2022-03-23 07:04:30 -07:00
|
|
|
|
phase_for_speed_est = motor_now->m_phase_now_observer;
|
|
|
|
|
break;
|
|
|
|
|
};
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
// Run PLL for speed estimation
|
2022-04-06 06:12:24 -07:00
|
|
|
|
foc_pll_run(phase_for_speed_est, dt, &motor_now->m_pll_phase, &motor_now->m_pll_speed, conf_now);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
// Low latency speed estimation, for e.g. HFI and speed control.
|
2020-01-28 10:46:19 -08:00
|
|
|
|
{
|
2022-03-23 07:04:30 -07:00
|
|
|
|
float diff = utils_angle_difference_rad(phase_for_speed_est, motor_now->m_phase_before_speed_est);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
utils_truncate_number(&diff, -M_PI / 3.0, M_PI / 3.0);
|
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
UTILS_LP_FAST(motor_now->m_speed_est_fast, diff / dt, 0.01);
|
|
|
|
|
UTILS_NAN_ZERO(motor_now->m_speed_est_fast);
|
2020-04-16 00:12:08 -07:00
|
|
|
|
|
|
|
|
|
UTILS_LP_FAST(motor_now->m_speed_est_faster, diff / dt, 0.2);
|
|
|
|
|
UTILS_NAN_ZERO(motor_now->m_speed_est_faster);
|
|
|
|
|
|
2023-08-08 10:56:48 -07:00
|
|
|
|
float diff_corr = utils_angle_difference_rad(motor_now->m_motor_state.phase, motor_now->m_phase_before_speed_est_corrected);
|
|
|
|
|
utils_truncate_number(&diff_corr, -M_PI / 3.0, M_PI / 3.0);
|
|
|
|
|
|
|
|
|
|
UTILS_LP_FAST(motor_now->m_speed_est_fast_corrected, diff_corr / dt, 0.01);
|
|
|
|
|
UTILS_NAN_ZERO(motor_now->m_speed_est_fast_corrected);
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
// pll wind-up protection
|
|
|
|
|
utils_truncate_number_abs((float*)&motor_now->m_pll_speed, fabsf(motor_now->m_speed_est_fast) * 3.0);
|
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
motor_now->m_phase_before_speed_est = phase_for_speed_est;
|
2023-08-08 10:56:48 -07:00
|
|
|
|
motor_now->m_phase_before_speed_est_corrected = motor_now->m_motor_state.phase;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
}
|
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
|
// Update tachometer (resolution = 60 deg as for BLDC)
|
2020-03-21 03:40:50 -07:00
|
|
|
|
float ph_tmp = motor_now->m_motor_state.phase;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
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);
|
2020-03-21 03:40:50 -07:00
|
|
|
|
int diff = step - motor_now->m_tacho_step_last;
|
|
|
|
|
motor_now->m_tacho_step_last = step;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
|
|
|
|
if (diff > 3) {
|
|
|
|
|
diff -= 6;
|
|
|
|
|
} else if (diff < -2) {
|
|
|
|
|
diff += 6;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_tachometer += diff;
|
|
|
|
|
motor_now->m_tachometer_abs += abs(diff);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
2016-02-24 12:17:39 -08:00
|
|
|
|
// Track position control angle
|
2016-04-27 06:32:32 -07:00
|
|
|
|
float angle_now = 0.0;
|
2022-01-09 08:10:40 -08:00
|
|
|
|
if (encoder_is_configured()) {
|
2020-03-21 03:40:50 -07:00
|
|
|
|
if (conf_now->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501_MULTITURN) {
|
2022-01-09 08:10:40 -08:00
|
|
|
|
angle_now = encoder_read_deg_multiturn();
|
2020-01-31 15:37:25 -08:00
|
|
|
|
} else {
|
|
|
|
|
angle_now = enc_ang;
|
|
|
|
|
}
|
2016-04-27 06:32:32 -07:00
|
|
|
|
} else {
|
2021-10-13 09:13:51 -07:00
|
|
|
|
angle_now = RAD2DEG_f(motor_now->m_motor_state.phase);
|
2016-04-27 06:32:32 -07:00
|
|
|
|
}
|
2016-02-24 12:17:39 -08:00
|
|
|
|
|
2020-11-18 05:26:55 -08:00
|
|
|
|
utils_norm_angle(&angle_now);
|
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
if (conf_now->p_pid_ang_div > 0.98 && conf_now->p_pid_ang_div < 1.02) {
|
|
|
|
|
motor_now->m_pos_pid_now = angle_now;
|
2016-02-24 12:17:39 -08:00
|
|
|
|
} else {
|
2020-11-18 05:26:55 -08:00
|
|
|
|
if (angle_now < 90.0 && motor_now->m_pid_div_angle_last > 270.0) {
|
|
|
|
|
motor_now->m_pid_div_angle_accumulator += 360.0 / conf_now->p_pid_ang_div;
|
|
|
|
|
utils_norm_angle((float*)&motor_now->m_pid_div_angle_accumulator);
|
|
|
|
|
} else if (angle_now > 270.0 && motor_now->m_pid_div_angle_last < 90.0) {
|
|
|
|
|
motor_now->m_pid_div_angle_accumulator -= 360.0 / conf_now->p_pid_ang_div;
|
|
|
|
|
utils_norm_angle((float*)&motor_now->m_pid_div_angle_accumulator);
|
|
|
|
|
}
|
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
motor_now->m_pid_div_angle_last = angle_now;
|
2020-11-18 05:26:55 -08:00
|
|
|
|
|
|
|
|
|
motor_now->m_pos_pid_now = motor_now->m_pid_div_angle_accumulator + angle_now / conf_now->p_pid_ang_div;
|
2020-03-21 03:40:50 -07:00
|
|
|
|
utils_norm_angle((float*)&motor_now->m_pos_pid_now);
|
2016-02-24 12:17:39 -08:00
|
|
|
|
}
|
|
|
|
|
|
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
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
mc_interface_mc_timer_isr(is_second_motor);
|
|
|
|
|
#else
|
|
|
|
|
mc_interface_mc_timer_isr(false);
|
|
|
|
|
#endif
|
|
|
|
|
|
2020-03-16 14:42:44 -07:00
|
|
|
|
m_isr_motor = 0;
|
2019-03-10 06:57:42 -07:00
|
|
|
|
m_last_adc_isr_duration = timer_seconds_elapsed_since(t_start);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Private functions
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
static void timer_update(motor_all_state_t *motor, float dt) {
|
2022-04-06 06:12:24 -07:00
|
|
|
|
foc_run_fw(motor, dt);
|
2021-04-11 02:31:25 -07:00
|
|
|
|
|
2022-04-07 05:59:56 -07:00
|
|
|
|
const mc_configuration *conf_now = motor->m_conf;
|
|
|
|
|
|
2022-04-06 09:40:34 -07:00
|
|
|
|
// Calculate temperature-compensated parameters here
|
|
|
|
|
if (mc_interface_temp_motor_filtered() > -30.0) {
|
2022-04-07 05:59:56 -07:00
|
|
|
|
float comp_fact = 1.0 + 0.00386 * (mc_interface_temp_motor_filtered() - conf_now->foc_temp_comp_base_temp);
|
|
|
|
|
motor->m_res_temp_comp = conf_now->foc_motor_r * comp_fact;
|
|
|
|
|
motor->m_current_ki_temp_comp = conf_now->foc_current_ki * comp_fact;
|
2022-04-06 09:40:34 -07:00
|
|
|
|
} else {
|
2022-04-07 05:59:56 -07:00
|
|
|
|
motor->m_res_temp_comp = conf_now->foc_motor_r;
|
|
|
|
|
motor->m_current_ki_temp_comp = conf_now->foc_current_ki;
|
2022-04-06 09:40:34 -07:00
|
|
|
|
}
|
|
|
|
|
|
2021-04-10 02:37:35 -07:00
|
|
|
|
// Check if it is time to stop the modulation. Notice that modulation is kept on as long as there is
|
|
|
|
|
// field weakening current.
|
2021-05-15 06:24:39 -07:00
|
|
|
|
utils_sys_lock_cnt();
|
|
|
|
|
utils_step_towards((float*)&motor->m_current_off_delay, 0.0, dt);
|
2021-04-10 02:37:35 -07:00
|
|
|
|
if (!motor->m_phase_override && motor->m_state == MC_STATE_RUNNING &&
|
|
|
|
|
(motor->m_control_mode == CONTROL_MODE_CURRENT ||
|
|
|
|
|
motor->m_control_mode == CONTROL_MODE_CURRENT_BRAKE ||
|
|
|
|
|
motor->m_control_mode == CONTROL_MODE_HANDBRAKE ||
|
|
|
|
|
motor->m_control_mode == CONTROL_MODE_OPENLOOP ||
|
|
|
|
|
motor->m_control_mode == CONTROL_MODE_OPENLOOP_PHASE)) {
|
2022-01-19 09:52:45 -08:00
|
|
|
|
|
|
|
|
|
// This is required to allow releasing the motor when cc_min_current is 0
|
2022-04-07 05:59:56 -07:00
|
|
|
|
float min_current = conf_now->cc_min_current;
|
2022-04-06 07:33:44 -07:00
|
|
|
|
if (min_current < 0.001 && get_motor_now()->m_motor_released) {
|
2022-01-19 09:52:45 -08:00
|
|
|
|
min_current = 0.001;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (fabsf(motor->m_iq_set) < min_current &&
|
|
|
|
|
fabsf(motor->m_id_set) < min_current &&
|
|
|
|
|
motor->m_i_fw_set < min_current &&
|
2021-04-14 13:29:50 -07:00
|
|
|
|
motor->m_current_off_delay < dt) {
|
2021-04-10 02:37:35 -07:00
|
|
|
|
motor->m_control_mode = CONTROL_MODE_NONE;
|
|
|
|
|
motor->m_state = MC_STATE_OFF;
|
|
|
|
|
stop_pwm_hw(motor);
|
|
|
|
|
}
|
|
|
|
|
}
|
2021-05-15 06:24:39 -07:00
|
|
|
|
utils_sys_unlock_cnt();
|
2020-10-09 12:08:48 -07:00
|
|
|
|
|
|
|
|
|
// Use this to study the openloop timers under experiment plot
|
|
|
|
|
#if 0
|
|
|
|
|
{
|
|
|
|
|
static bool plot_started = false;
|
|
|
|
|
static int plot_div = 0;
|
|
|
|
|
static float plot_int = 0.0;
|
|
|
|
|
static int get_fw_version_cnt = 0;
|
|
|
|
|
|
|
|
|
|
if (commands_get_fw_version_sent_cnt() != get_fw_version_cnt) {
|
|
|
|
|
get_fw_version_cnt = commands_get_fw_version_sent_cnt();
|
|
|
|
|
plot_started = false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
plot_div++;
|
|
|
|
|
if (plot_div >= 10) {
|
|
|
|
|
plot_div = 0;
|
|
|
|
|
if (!plot_started) {
|
|
|
|
|
plot_started = true;
|
|
|
|
|
commands_init_plot("Time", "Val");
|
|
|
|
|
commands_plot_add_graph("m_min_rpm_timer");
|
|
|
|
|
commands_plot_add_graph("m_min_rpm_hyst_timer");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
commands_plot_set_graph(0);
|
|
|
|
|
commands_send_plot_points(plot_int, motor->m_min_rpm_timer);
|
|
|
|
|
commands_plot_set_graph(1);
|
|
|
|
|
commands_send_plot_points(plot_int, motor->m_min_rpm_hyst_timer);
|
|
|
|
|
plot_int++;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
// Use this to study the observer state in a XY-plot
|
|
|
|
|
#if 0
|
|
|
|
|
{
|
|
|
|
|
static bool plot_started = false;
|
|
|
|
|
static int plot_div = 0;
|
|
|
|
|
static int get_fw_version_cnt = 0;
|
|
|
|
|
|
|
|
|
|
if (commands_get_fw_version_sent_cnt() != get_fw_version_cnt) {
|
|
|
|
|
get_fw_version_cnt = commands_get_fw_version_sent_cnt();
|
|
|
|
|
plot_started = false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
plot_div++;
|
|
|
|
|
if (plot_div >= 10) {
|
|
|
|
|
plot_div = 0;
|
|
|
|
|
if (!plot_started) {
|
|
|
|
|
plot_started = true;
|
|
|
|
|
commands_init_plot("X1", "X2");
|
|
|
|
|
commands_plot_add_graph("Observer");
|
|
|
|
|
commands_plot_add_graph("Observer Mag");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
commands_plot_set_graph(0);
|
|
|
|
|
commands_send_plot_points(m_motor_1.m_observer_x1, m_motor_1.m_observer_x2);
|
2022-03-07 08:33:51 -08:00
|
|
|
|
float mag = NORM2_f(m_motor_1.m_observer_x1, m_motor_1.m_observer_x2);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
commands_plot_set_graph(1);
|
|
|
|
|
commands_send_plot_points(0.0, mag);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
2022-04-07 05:59:56 -07:00
|
|
|
|
float t_lock = conf_now->foc_sl_openloop_time_lock;
|
|
|
|
|
float t_ramp = conf_now->foc_sl_openloop_time_ramp;
|
|
|
|
|
float t_const = conf_now->foc_sl_openloop_time;
|
2021-04-10 02:37:35 -07:00
|
|
|
|
|
2022-04-11 11:35:24 -07:00
|
|
|
|
float openloop_current = fabsf(motor->m_motor_state.iq_filter);
|
|
|
|
|
openloop_current += conf_now->foc_sl_openloop_boost_q;
|
|
|
|
|
if (conf_now->foc_sl_openloop_max_q > 0.0) {
|
2022-08-19 12:36:29 -07:00
|
|
|
|
utils_truncate_number(&openloop_current, 0.0, conf_now->foc_sl_openloop_max_q);
|
2022-04-11 11:35:24 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float openloop_rpm_max = utils_map(openloop_current,
|
2022-04-07 05:59:56 -07:00
|
|
|
|
0.0, conf_now->l_current_max,
|
|
|
|
|
conf_now->foc_openloop_rpm_low * conf_now->foc_openloop_rpm,
|
|
|
|
|
conf_now->foc_openloop_rpm);
|
2020-10-09 12:08:48 -07:00
|
|
|
|
|
2022-04-07 05:59:56 -07:00
|
|
|
|
utils_truncate_number_abs(&openloop_rpm_max, conf_now->foc_openloop_rpm);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-10-09 12:08:48 -07:00
|
|
|
|
float openloop_rpm = openloop_rpm_max;
|
2022-04-07 05:59:56 -07:00
|
|
|
|
if (conf_now->foc_sensor_mode != FOC_SENSOR_MODE_ENCODER) {
|
2020-10-09 12:08:48 -07:00
|
|
|
|
float time_fwd = t_lock + t_ramp + t_const - motor->m_min_rpm_timer;
|
|
|
|
|
if (time_fwd < t_lock) {
|
|
|
|
|
openloop_rpm = 0.0;
|
|
|
|
|
} else if (time_fwd < (t_lock + t_ramp)) {
|
|
|
|
|
openloop_rpm = utils_map(time_fwd, t_lock,
|
|
|
|
|
t_lock + t_ramp, 0.0, openloop_rpm);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
utils_truncate_number_abs(&openloop_rpm, openloop_rpm_max);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
float add_min_speed = 0.0;
|
|
|
|
|
if (motor->m_motor_state.duty_now > 0.0) {
|
2021-10-13 09:13:51 -07:00
|
|
|
|
add_min_speed = RPM2RADPS_f(openloop_rpm) * dt;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
} else {
|
2021-10-13 09:13:51 -07:00
|
|
|
|
add_min_speed = -RPM2RADPS_f(openloop_rpm) * dt;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
// Open loop encoder angle for when the index is not found
|
|
|
|
|
motor->m_phase_now_encoder_no_index += add_min_speed;
|
|
|
|
|
utils_norm_angle_rad((float*)&motor->m_phase_now_encoder_no_index);
|
2016-05-19 10:24:01 -07:00
|
|
|
|
|
2021-10-13 09:13:51 -07:00
|
|
|
|
if (fabsf(motor->m_pll_speed) < RPM2RADPS_f(openloop_rpm_max) &&
|
2022-04-07 05:59:56 -07:00
|
|
|
|
motor->m_min_rpm_hyst_timer < conf_now->foc_sl_openloop_hyst) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_min_rpm_hyst_timer += dt;
|
|
|
|
|
} else if (motor->m_min_rpm_hyst_timer > 0.0) {
|
|
|
|
|
motor->m_min_rpm_hyst_timer -= dt;
|
|
|
|
|
}
|
2017-09-04 12:12:43 -07:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
// Don't use this in brake mode.
|
2020-10-09 12:08:48 -07:00
|
|
|
|
if (motor->m_control_mode == CONTROL_MODE_CURRENT_BRAKE ||
|
|
|
|
|
(motor->m_state == MC_STATE_RUNNING && fabsf(motor->m_motor_state.duty_now) < 0.001)) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_min_rpm_hyst_timer = 0.0;
|
2020-10-09 12:08:48 -07:00
|
|
|
|
motor->m_min_rpm_timer = 0.0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_phase_observer_override = false;
|
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
bool started_now = false;
|
2022-04-07 05:59:56 -07:00
|
|
|
|
if (motor->m_min_rpm_hyst_timer >= conf_now->foc_sl_openloop_hyst &&
|
2020-10-09 12:08:48 -07:00
|
|
|
|
motor->m_min_rpm_timer <= 0.0001) {
|
|
|
|
|
motor->m_min_rpm_timer = t_lock + t_ramp + t_const;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
started_now = true;
|
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-10-09 12:08:48 -07:00
|
|
|
|
if (motor->m_state != MC_STATE_RUNNING) {
|
|
|
|
|
motor->m_min_rpm_timer = 0.0;
|
|
|
|
|
}
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (motor->m_min_rpm_timer > 0.0) {
|
|
|
|
|
motor->m_phase_now_observer_override += add_min_speed;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
// When the motor gets stuck it tends to be 90 degrees off, so start the open loop
|
2021-03-13 02:42:23 -08:00
|
|
|
|
// sequence by correcting with 60 degrees.
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (started_now) {
|
|
|
|
|
if (motor->m_motor_state.duty_now > 0.0) {
|
2021-03-13 02:42:23 -08:00
|
|
|
|
motor->m_phase_now_observer_override += M_PI / 3.0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
} else {
|
2021-03-13 02:42:23 -08:00
|
|
|
|
motor->m_phase_now_observer_override -= M_PI / 3.0;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
utils_norm_angle_rad((float*)&motor->m_phase_now_observer_override);
|
|
|
|
|
motor->m_phase_observer_override = true;
|
|
|
|
|
motor->m_min_rpm_timer -= dt;
|
|
|
|
|
motor->m_min_rpm_hyst_timer = 0.0;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
|
|
|
|
// Set observer state to help it start tracking when leaving open loop.
|
|
|
|
|
float s, c;
|
|
|
|
|
utils_fast_sincos_better(motor->m_phase_now_observer_override + SIGN(motor->m_motor_state.duty_now) * M_PI / 4.0, &s, &c);
|
2022-04-07 05:59:56 -07:00
|
|
|
|
motor->m_observer_x1_override = c * conf_now->foc_motor_flux_linkage;
|
|
|
|
|
motor->m_observer_x2_override = s * conf_now->foc_motor_flux_linkage;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
} else {
|
|
|
|
|
motor->m_phase_now_observer_override = motor->m_phase_now_observer;
|
|
|
|
|
motor->m_phase_observer_override = false;
|
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
// Samples
|
|
|
|
|
if (motor->m_state == MC_STATE_RUNNING) {
|
|
|
|
|
const volatile float vd_tmp = motor->m_motor_state.vd;
|
|
|
|
|
const volatile float vq_tmp = motor->m_motor_state.vq;
|
|
|
|
|
const volatile float id_tmp = motor->m_motor_state.id;
|
|
|
|
|
const volatile float iq_tmp = motor->m_motor_state.iq;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-03-07 08:33:51 -08:00
|
|
|
|
motor->m_samples.avg_current_tot += NORM2_f(id_tmp, iq_tmp);
|
|
|
|
|
motor->m_samples.avg_voltage_tot += NORM2_f(vd_tmp, vq_tmp);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_samples.sample_num++;
|
|
|
|
|
}
|
2017-09-04 12:12:43 -07:00
|
|
|
|
|
2020-03-17 01:38:09 -07:00
|
|
|
|
// Observer gain scaling, based on bus voltage and duty cycle
|
2020-03-20 09:35:25 -07:00
|
|
|
|
float gamma_tmp = utils_map(fabsf(motor->m_motor_state.duty_now),
|
2020-03-28 11:21:45 -07:00
|
|
|
|
0.0, 40.0 / motor->m_motor_state.v_bus,
|
2022-04-07 05:59:56 -07:00
|
|
|
|
0, conf_now->foc_observer_gain);
|
|
|
|
|
if (gamma_tmp < (conf_now->foc_observer_gain_slow * conf_now->foc_observer_gain)) {
|
|
|
|
|
gamma_tmp = conf_now->foc_observer_gain_slow * conf_now->foc_observer_gain;
|
2020-03-20 09:35:25 -07:00
|
|
|
|
}
|
2020-03-29 08:00:43 -07:00
|
|
|
|
|
2020-04-08 02:05:58 -07:00
|
|
|
|
// 4.0 scaling is kind of arbitrary, but it should make configs from old VESC Tools more likely to work.
|
|
|
|
|
motor->m_gamma_now = gamma_tmp * 4.0;
|
2021-12-29 09:21:42 -08:00
|
|
|
|
|
|
|
|
|
// Run resistance observer
|
|
|
|
|
// See "An adaptive flux observer for the permanent magnet synchronous motor"
|
|
|
|
|
// https://doi.org/10.1002/acs.2587
|
|
|
|
|
{
|
|
|
|
|
float res_est_gain = 0.00002;
|
|
|
|
|
float i_abs_sq = SQ(motor->m_motor_state.i_abs);
|
2022-12-18 15:14:57 -08:00
|
|
|
|
motor->m_res_est = motor->m_r_est_state - 0.5 * res_est_gain * conf_now->foc_motor_l * i_abs_sq;
|
|
|
|
|
float res_dot = -res_est_gain * (motor->m_res_est * i_abs_sq + motor->m_speed_est_fast *
|
2022-05-21 13:54:12 -07:00
|
|
|
|
(motor->m_motor_state.i_beta * motor->m_observer_state.x1 - motor->m_motor_state.i_alpha * motor->m_observer_state.x2) -
|
2021-12-29 09:21:42 -08:00
|
|
|
|
(motor->m_motor_state.i_alpha * motor->m_motor_state.v_alpha + motor->m_motor_state.i_beta * motor->m_motor_state.v_beta));
|
|
|
|
|
motor->m_r_est_state += res_dot * dt;
|
|
|
|
|
|
2022-04-07 05:59:56 -07:00
|
|
|
|
utils_truncate_number((float*)&motor->m_r_est_state, conf_now->foc_motor_r * 0.25, conf_now->foc_motor_r * 3.0);
|
2021-12-29 09:21:42 -08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static void terminal_tmp(int argc, const char **argv) {
|
|
|
|
|
(void)argc;
|
|
|
|
|
(void)argv;
|
|
|
|
|
|
|
|
|
|
int top = 1;
|
|
|
|
|
if (argc == 2) {
|
|
|
|
|
float seconds = -1.0;
|
|
|
|
|
sscanf(argv[1], "%f", &seconds);
|
|
|
|
|
|
|
|
|
|
if (seconds > 0.0) {
|
|
|
|
|
top = seconds * 2;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (top > 1) {
|
|
|
|
|
commands_init_plot("Time", "Temperature");
|
|
|
|
|
commands_plot_add_graph("Temp Measured");
|
|
|
|
|
commands_plot_add_graph("Temp Estimated");
|
2022-05-21 13:54:12 -07:00
|
|
|
|
commands_plot_add_graph("lambda_est");
|
2021-12-29 09:21:42 -08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
for (int i = 0;i < top;i++) {
|
2022-12-18 15:14:57 -08:00
|
|
|
|
float res_est = m_motor_1.m_res_est;
|
2021-12-29 09:21:42 -08:00
|
|
|
|
float t_base = m_motor_1.m_conf->foc_temp_comp_base_temp;
|
|
|
|
|
float res_base = m_motor_1.m_conf->foc_motor_r;
|
|
|
|
|
float t_est = (res_est / res_base - 1) / 0.00386 + t_base;
|
|
|
|
|
float t_meas = mc_interface_temp_motor_filtered();
|
|
|
|
|
|
|
|
|
|
if (top > 1) {
|
|
|
|
|
commands_plot_set_graph(0);
|
|
|
|
|
commands_send_plot_points((float)i / 2.0, t_meas);
|
|
|
|
|
commands_plot_set_graph(1);
|
|
|
|
|
commands_send_plot_points((float)i / 2.0, t_est);
|
2022-05-21 13:54:12 -07:00
|
|
|
|
commands_plot_set_graph(2);
|
|
|
|
|
commands_send_plot_points((float)i / 2.0, m_motor_1.m_observer_state.lambda_est);
|
2021-12-29 09:21:42 -08:00
|
|
|
|
commands_printf("Sample %d of %d", i, top);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
commands_printf("R: %.2f, EST: %.2f",
|
|
|
|
|
(double)(res_base * 1000.0), (double)(res_est * 1000.0));
|
|
|
|
|
commands_printf("T: %.2f, T_EST: %.2f\n",
|
|
|
|
|
(double)t_meas, (double)t_est);
|
|
|
|
|
|
|
|
|
|
chThdSleepMilliseconds(500);
|
|
|
|
|
}
|
2020-03-16 10:32:39 -07:00
|
|
|
|
}
|
2017-09-04 12:12:43 -07:00
|
|
|
|
|
2020-06-03 01:52:55 -07:00
|
|
|
|
// TODO: This won't work for dual motors
|
|
|
|
|
static void input_current_offset_measurement(void) {
|
2020-02-16 06:07:08 -08:00
|
|
|
|
#ifdef HW_HAS_INPUT_CURRENT_SENSOR
|
|
|
|
|
static uint16_t delay_current_offset_measurement = 0;
|
|
|
|
|
|
2020-06-03 01:52:55 -07:00
|
|
|
|
if (delay_current_offset_measurement < 1000) {
|
2020-02-16 06:07:08 -08:00
|
|
|
|
delay_current_offset_measurement++;
|
2020-06-03 01:52:55 -07:00
|
|
|
|
} else {
|
|
|
|
|
if (delay_current_offset_measurement == 1000) {
|
2020-02-16 06:07:08 -08:00
|
|
|
|
delay_current_offset_measurement++;
|
|
|
|
|
MEASURE_INPUT_CURRENT_OFFSET();
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
static THD_FUNCTION(timer_thread, arg) {
|
|
|
|
|
(void)arg;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
chRegSetThreadName("foc timer");
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
for(;;) {
|
|
|
|
|
const float dt = 0.001;
|
|
|
|
|
|
|
|
|
|
if (timer_thd_stop) {
|
|
|
|
|
timer_thd_stop = false;
|
|
|
|
|
return;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
timer_update((motor_all_state_t*)&m_motor_1, dt);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-04-06 07:33:44 -07:00
|
|
|
|
timer_update((motor_all_state_t*)&m_motor_2, dt);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2020-02-16 06:07:08 -08:00
|
|
|
|
input_current_offset_measurement();
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
static void hfi_update(volatile motor_all_state_t *motor, float dt) {
|
2022-04-06 10:21:22 -07:00
|
|
|
|
(void)dt;
|
2021-10-13 09:13:51 -07:00
|
|
|
|
float rpm_abs = fabsf(RADPS2RPM_f(motor->m_speed_est_fast));
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (rpm_abs > motor->m_conf->foc_sl_erpm_hfi) {
|
|
|
|
|
motor->m_hfi.angle = motor->m_phase_now_observer;
|
2022-04-25 03:53:04 -07:00
|
|
|
|
motor->m_hfi.double_integrator = -motor->m_speed_est_fast;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
}
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (motor->m_hfi.ready) {
|
2022-04-06 10:21:22 -07:00
|
|
|
|
if ((motor->m_conf->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V4 ||
|
|
|
|
|
motor->m_conf->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V5) &&
|
2022-04-05 15:35:11 -07:00
|
|
|
|
motor->m_hfi.est_done_cnt >= motor->m_conf->foc_hfi_start_samples) {
|
|
|
|
|
// Nothing done here, the update is done in the interrupt.
|
2022-04-06 10:21:22 -07:00
|
|
|
|
} else if ((motor->m_conf->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V2 ||
|
|
|
|
|
motor->m_conf->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V3) &&
|
2022-03-23 07:04:30 -07:00
|
|
|
|
motor->m_hfi.est_done_cnt >= motor->m_conf->foc_hfi_start_samples) {
|
2022-04-06 10:21:22 -07:00
|
|
|
|
// Nothing done here, the update is done in the interrupt.
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-04-06 10:21:22 -07:00
|
|
|
|
// Enable to set the observer position to the HFI angle for plotting the error in the position plot RT page in VESC Tool. Just
|
|
|
|
|
// remember that enabling this will make the transition to using the observer bad.
|
2022-03-23 07:04:30 -07:00
|
|
|
|
#if 0
|
2022-04-06 10:21:22 -07:00
|
|
|
|
float s, c;
|
|
|
|
|
utils_fast_sincos_better(motor->m_hfi.angle, &s, &c);
|
|
|
|
|
motor->m_observer_x1 = c * motor->m_conf->foc_motor_flux_linkage;
|
|
|
|
|
motor->m_observer_x2 = s * motor->m_conf->foc_motor_flux_linkage;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
#endif
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-04-06 10:21:22 -07:00
|
|
|
|
// Enable to plot the sample together with encoder position
|
2022-03-23 07:04:30 -07:00
|
|
|
|
#if 0
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-04-06 10:21:22 -07:00
|
|
|
|
commands_plot_set_graph(0);
|
|
|
|
|
commands_send_plot_points(motor->m_hfi_plot_sample, ind_a);
|
|
|
|
|
commands_plot_set_graph(1);
|
|
|
|
|
commands_send_plot_points(motor->m_hfi_plot_sample, RAD2DEG_f(motor->m_phase_now_encoder) / 4e6);
|
|
|
|
|
motor->m_hfi_plot_sample++;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
} else {
|
2022-03-23 07:04:30 -07:00
|
|
|
|
float real_bin1, imag_bin1, real_bin2, imag_bin2;
|
|
|
|
|
motor->m_hfi.fft_bin1_func((float*)motor->m_hfi.buffer, &real_bin1, &imag_bin1);
|
|
|
|
|
motor->m_hfi.fft_bin2_func((float*)motor->m_hfi.buffer, &real_bin2, &imag_bin2);
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
float mag_bin_1 = NORM2_f(imag_bin1, real_bin1);
|
|
|
|
|
float angle_bin_1 = -utils_fast_atan2(imag_bin1, real_bin1);
|
|
|
|
|
|
|
|
|
|
angle_bin_1 += M_PI / 1.7; // Why 1.7??
|
|
|
|
|
utils_norm_angle_rad(&angle_bin_1);
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
float mag_bin_2 = NORM2_f(imag_bin2, real_bin2);
|
|
|
|
|
float angle_bin_2 = -utils_fast_atan2(imag_bin2, real_bin2) / 2.0;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
// Assuming this thread is much faster than it takes to fill the HFI buffer completely,
|
|
|
|
|
// we should lag 1/2 HFI buffer behind in phase. Compensate for that here.
|
|
|
|
|
float dt_sw;
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (motor->m_conf->foc_control_sample_mode == FOC_CONTROL_SAMPLE_MODE_V0_V7) {
|
2022-03-23 07:04:30 -07:00
|
|
|
|
dt_sw = 1.0 / motor->m_conf->foc_f_zv;
|
|
|
|
|
} else {
|
|
|
|
|
dt_sw = 1.0 / (motor->m_conf->foc_f_zv / 2.0);
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
2023-07-01 05:51:08 -07:00
|
|
|
|
angle_bin_2 += motor->m_pll_speed * ((float)motor->m_hfi.samples / 2.0) * dt_sw;
|
2021-03-13 02:42:23 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (fabsf(utils_angle_difference_rad(angle_bin_2 + M_PI, motor->m_hfi.angle)) <
|
|
|
|
|
fabsf(utils_angle_difference_rad(angle_bin_2, motor->m_hfi.angle))) {
|
2020-01-20 00:39:33 -08:00
|
|
|
|
angle_bin_2 += M_PI;
|
|
|
|
|
}
|
2022-03-23 07:04:30 -07:00
|
|
|
|
|
|
|
|
|
if (motor->m_hfi.est_done_cnt < motor->m_conf->foc_hfi_start_samples) {
|
|
|
|
|
motor->m_hfi.est_done_cnt++;
|
|
|
|
|
|
|
|
|
|
if (fabsf(utils_angle_difference_rad(angle_bin_2, angle_bin_1)) > (M_PI / 2.0)) {
|
|
|
|
|
motor->m_hfi.flip_cnt++;
|
|
|
|
|
}
|
2021-03-13 02:42:23 -08:00
|
|
|
|
}
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (motor->m_hfi.est_done_cnt >= motor->m_conf->foc_hfi_start_samples) {
|
|
|
|
|
if (motor->m_hfi.flip_cnt >= (motor->m_conf->foc_hfi_start_samples / 2)) {
|
|
|
|
|
angle_bin_2 += M_PI;
|
|
|
|
|
}
|
|
|
|
|
motor->m_hfi.flip_cnt = 0;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (motor->m_conf->foc_sensor_mode == FOC_SENSOR_MODE_HFI_START) {
|
|
|
|
|
float s, c;
|
|
|
|
|
utils_fast_sincos_better(angle_bin_2, &s, &c);
|
2022-05-21 13:54:12 -07:00
|
|
|
|
motor->m_observer_state.x1 = c * motor->m_conf->foc_motor_flux_linkage;
|
|
|
|
|
motor->m_observer_state.x2 = s * motor->m_conf->foc_motor_flux_linkage;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
motor->m_hfi.angle = angle_bin_2;
|
|
|
|
|
utils_norm_angle_rad((float*)&motor->m_hfi.angle);
|
|
|
|
|
|
|
|
|
|
// As angle_bin_1 is based on saturation, it is only accurate when the motor current is low. It
|
|
|
|
|
// might be possible to compensate for that, which would allow HFI on non-salient motors.
|
|
|
|
|
// m_hfi.angle = angle_bin_1;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (motor->m_hfi_plot_en == 1) {
|
|
|
|
|
static float hfi_plot_div = 0;
|
|
|
|
|
hfi_plot_div++;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (hfi_plot_div >= 8) {
|
|
|
|
|
hfi_plot_div = 0;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
float real_bin0, imag_bin0;
|
|
|
|
|
motor->m_hfi.fft_bin0_func((float*)motor->m_hfi.buffer, &real_bin0, &imag_bin0);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
commands_plot_set_graph(0);
|
|
|
|
|
commands_send_plot_points(motor->m_hfi_plot_sample, motor->m_hfi.angle);
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
commands_plot_set_graph(1);
|
|
|
|
|
commands_send_plot_points(motor->m_hfi_plot_sample, angle_bin_1);
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
commands_plot_set_graph(2);
|
|
|
|
|
commands_send_plot_points(motor->m_hfi_plot_sample, 2.0 * mag_bin_2 * 1e6);
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
commands_plot_set_graph(3);
|
|
|
|
|
commands_send_plot_points(motor->m_hfi_plot_sample, 2.0 * mag_bin_1 * 1e6);
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
commands_plot_set_graph(4);
|
|
|
|
|
commands_send_plot_points(motor->m_hfi_plot_sample, real_bin0 * 1e6);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2020-03-29 08:00:43 -07:00
|
|
|
|
// commands_plot_set_graph(0);
|
2023-07-01 05:51:08 -07:00
|
|
|
|
// commands_send_plot_points(motor->m_hfi_plot_sample, motor->m_pll_speed);
|
2020-03-29 08:00:43 -07:00
|
|
|
|
//
|
|
|
|
|
// commands_plot_set_graph(1);
|
|
|
|
|
// commands_send_plot_points(motor->m_hfi_plot_sample, motor->m_speed_est_fast);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
motor->m_hfi_plot_sample++;
|
|
|
|
|
}
|
|
|
|
|
} else if (motor->m_hfi_plot_en == 2) {
|
|
|
|
|
static float hfi_plot_div = 0;
|
|
|
|
|
hfi_plot_div++;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (hfi_plot_div >= 8) {
|
|
|
|
|
hfi_plot_div = 0;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (motor->m_hfi_plot_sample >= motor->m_hfi.samples) {
|
|
|
|
|
motor->m_hfi_plot_sample = 0;
|
|
|
|
|
}
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
commands_plot_set_graph(0);
|
|
|
|
|
commands_send_plot_points(motor->m_hfi_plot_sample, motor->m_hfi.buffer_current[(int)motor->m_hfi_plot_sample]);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
commands_plot_set_graph(1);
|
|
|
|
|
commands_send_plot_points(motor->m_hfi_plot_sample, motor->m_hfi.buffer[(int)motor->m_hfi_plot_sample] * 1e6);
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
motor->m_hfi_plot_sample++;
|
|
|
|
|
}
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
2020-03-16 10:32:39 -07:00
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
motor->m_hfi.angle = motor->m_phase_now_observer;
|
2022-04-25 03:53:04 -07:00
|
|
|
|
motor->m_hfi.double_integrator = -motor->m_speed_est_fast;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static THD_FUNCTION(hfi_thread, arg) {
|
|
|
|
|
(void)arg;
|
|
|
|
|
|
|
|
|
|
chRegSetThreadName("foc hfi");
|
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
uint32_t t_last = timer_time_now();
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
for(;;) {
|
|
|
|
|
if (hfi_thd_stop) {
|
|
|
|
|
hfi_thd_stop = false;
|
|
|
|
|
return;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
float dt = timer_seconds_elapsed_since(t_last);
|
|
|
|
|
t_last = timer_time_now();
|
|
|
|
|
|
|
|
|
|
hfi_update(&m_motor_1, dt);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-03-23 07:04:30 -07:00
|
|
|
|
hfi_update(&m_motor_2, dt);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
|
chThdSleepMicroseconds(500);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-07-12 05:31:01 -07:00
|
|
|
|
static THD_FUNCTION(pid_thread, arg) {
|
|
|
|
|
(void)arg;
|
|
|
|
|
|
|
|
|
|
chRegSetThreadName("foc pid");
|
|
|
|
|
|
|
|
|
|
uint32_t last_time = timer_time_now();
|
|
|
|
|
|
|
|
|
|
for(;;) {
|
|
|
|
|
if (pid_thd_stop) {
|
|
|
|
|
pid_thd_stop = false;
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
switch (m_motor_1.m_conf->sp_pid_loop_rate) {
|
|
|
|
|
case PID_RATE_25_HZ: chThdSleepMicroseconds(1000000 / 25); break;
|
|
|
|
|
case PID_RATE_50_HZ: chThdSleepMicroseconds(1000000 / 50); break;
|
|
|
|
|
case PID_RATE_100_HZ: chThdSleepMicroseconds(1000000 / 100); break;
|
|
|
|
|
case PID_RATE_250_HZ: chThdSleepMicroseconds(1000000 / 250); break;
|
|
|
|
|
case PID_RATE_500_HZ: chThdSleepMicroseconds(1000000 / 500); break;
|
|
|
|
|
case PID_RATE_1000_HZ: chThdSleepMicroseconds(1000000 / 1000); break;
|
|
|
|
|
case PID_RATE_2500_HZ: chThdSleepMicroseconds(1000000 / 2500); break;
|
|
|
|
|
case PID_RATE_5000_HZ: chThdSleepMicroseconds(1000000 / 5000); break;
|
|
|
|
|
case PID_RATE_10000_HZ: chThdSleepMicroseconds(1000000 / 10000); break;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float dt = timer_seconds_elapsed_since(last_time);
|
|
|
|
|
last_time = timer_time_now();
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
foc_run_pid_control_pos(encoder_index_found(), dt, (motor_all_state_t*)&m_motor_1);
|
|
|
|
|
foc_run_pid_control_speed(dt, (motor_all_state_t*)&m_motor_1);
|
2021-07-12 05:31:01 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
2022-04-06 07:33:44 -07:00
|
|
|
|
foc_run_pid_control_pos(encoder_index_found(), dt, (motor_all_state_t*)&m_motor_2);
|
|
|
|
|
foc_run_pid_control_speed(dt, (motor_all_state_t*)&m_motor_2);
|
2021-07-12 05:31:01 -07:00
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
|
/**
|
|
|
|
|
* 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
|
2015-12-23 15:43:31 -08:00
|
|
|
|
* i_abs
|
|
|
|
|
* i_abs_filter
|
2015-12-08 12:01:23 -08:00
|
|
|
|
* v_alpha
|
|
|
|
|
* v_beta
|
|
|
|
|
* mod_d
|
|
|
|
|
* mod_q
|
|
|
|
|
* id
|
|
|
|
|
* iq
|
2015-12-23 15:43:31 -08:00
|
|
|
|
* 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
|
2016-06-27 08:29:09 -07:00
|
|
|
|
* svm_sector
|
2015-12-08 12:01:23 -08:00
|
|
|
|
*
|
|
|
|
|
* @param dt
|
|
|
|
|
* The time step in seconds.
|
|
|
|
|
*/
|
2022-04-06 07:33:44 -07:00
|
|
|
|
static void control_current(motor_all_state_t *motor, float dt) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
volatile motor_state_t *state_m = &motor->m_motor_state;
|
2020-03-21 03:40:50 -07:00
|
|
|
|
volatile mc_configuration *conf_now = motor->m_conf;
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
float s = state_m->phase_sin;
|
|
|
|
|
float c = state_m->phase_cos;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-10-13 09:13:51 -07:00
|
|
|
|
float abs_rpm = fabsf(RADPS2RPM_f(motor->m_speed_est_fast));
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2021-03-13 02:42:23 -08:00
|
|
|
|
bool do_hfi = (conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI ||
|
2022-03-23 07:04:30 -07:00
|
|
|
|
conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V2 ||
|
2022-03-23 14:26:05 -07:00
|
|
|
|
conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V3 ||
|
2022-04-05 15:35:11 -07:00
|
|
|
|
conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V4 ||
|
|
|
|
|
conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V5 ||
|
2021-11-08 13:48:15 -08:00
|
|
|
|
(conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_START &&
|
|
|
|
|
motor->m_control_mode != CONTROL_MODE_CURRENT_BRAKE &&
|
|
|
|
|
fabsf(state_m->iq_target) > conf_now->cc_min_current)) &&
|
2022-03-23 07:04:30 -07:00
|
|
|
|
!motor->m_phase_override &&
|
|
|
|
|
abs_rpm < (conf_now->foc_sl_erpm_hfi * (motor->m_cc_was_hfi ? 1.8 : 1.5));
|
|
|
|
|
|
|
|
|
|
bool hfi_est_done = motor->m_hfi.est_done_cnt >= conf_now->foc_hfi_start_samples;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
|
|
|
|
// Only allow Q axis current after the HFI ambiguity is resolved. This causes
|
|
|
|
|
// a short delay when starting.
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (do_hfi && !hfi_est_done) {
|
2020-01-20 00:39:33 -08:00
|
|
|
|
state_m->iq_target = 0;
|
2021-03-13 02:42:23 -08:00
|
|
|
|
} else if (conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_START) {
|
|
|
|
|
do_hfi = false;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
|
|
|
|
|
2021-03-13 02:42:23 -08:00
|
|
|
|
motor->m_cc_was_hfi = do_hfi;
|
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
|
float max_duty = fabsf(state_m->max_duty);
|
2020-03-21 03:40:50 -07:00
|
|
|
|
utils_truncate_number(&max_duty, 0.0, conf_now->l_max_duty);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
2022-11-08 09:13:27 -08:00
|
|
|
|
// Park transform: transforms the currents from stator to the rotor reference frame
|
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;
|
2021-11-21 09:17:40 -08:00
|
|
|
|
|
|
|
|
|
// Low passed currents are used for less time critical parts, not for the feedback
|
|
|
|
|
UTILS_LP_FAST(state_m->id_filter, state_m->id, conf_now->foc_current_filter_const);
|
2020-03-21 03:40:50 -07:00
|
|
|
|
UTILS_LP_FAST(state_m->iq_filter, state_m->iq, conf_now->foc_current_filter_const);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-12-06 12:33:08 -08:00
|
|
|
|
float d_gain_scale = 1.0;
|
2021-03-27 03:18:47 -07:00
|
|
|
|
if (conf_now->foc_d_gain_scale_start < 0.99) {
|
|
|
|
|
float max_mod_norm = fabsf(state_m->duty_now / max_duty);
|
|
|
|
|
if (max_duty < 0.01) {
|
|
|
|
|
max_mod_norm = 1.0;
|
|
|
|
|
}
|
|
|
|
|
if (max_mod_norm > conf_now->foc_d_gain_scale_start) {
|
|
|
|
|
d_gain_scale = utils_map(max_mod_norm, conf_now->foc_d_gain_scale_start, 1.0,
|
|
|
|
|
1.0, conf_now->foc_d_gain_scale_max_mod);
|
|
|
|
|
if (d_gain_scale < conf_now->foc_d_gain_scale_max_mod) {
|
|
|
|
|
d_gain_scale = conf_now->foc_d_gain_scale_max_mod;
|
|
|
|
|
}
|
2020-12-06 12:33:08 -08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
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;
|
|
|
|
|
|
2020-03-21 03:40:50 -07:00
|
|
|
|
float ki = conf_now->foc_current_ki;
|
2022-04-06 09:40:34 -07:00
|
|
|
|
if (conf_now->foc_temp_comp) {
|
|
|
|
|
ki = motor->m_current_ki_temp_comp;
|
2018-03-24 14:32:58 -07:00
|
|
|
|
}
|
|
|
|
|
|
2020-12-06 12:33:08 -08:00
|
|
|
|
state_m->vd_int += Ierr_d * (ki * d_gain_scale * dt);
|
2018-03-24 14:32:58 -07:00
|
|
|
|
state_m->vq_int += Ierr_q * (ki * dt);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-05-23 02:32:55 -07:00
|
|
|
|
// Feedback (PI controller). No D action needed because the plant is a first order system (tf = 1/(Ls+R))
|
|
|
|
|
state_m->vd = state_m->vd_int + Ierr_d * conf_now->foc_current_kp * d_gain_scale;
|
|
|
|
|
state_m->vq = state_m->vq_int + Ierr_q * conf_now->foc_current_kp;
|
|
|
|
|
|
2021-11-21 09:17:40 -08:00
|
|
|
|
// Decoupling. Using feedforward this compensates for the fact that the equations of a PMSM
|
|
|
|
|
// are not really decoupled (the d axis current has impact on q axis voltage and visa-versa):
|
2022-11-08 09:13:27 -08:00
|
|
|
|
// Resistance Inductance Cross terms Back-EMF (see www.mathworks.com/help/physmod/sps/ref/pmsm.html)
|
|
|
|
|
// vd = Rs*id + Ld*did/dt − ωe*iq*Lq
|
|
|
|
|
// vq = Rs*iq + Lq*diq/dt + ωe*id*Ld + ωe*ψm
|
2020-01-12 12:25:21 -08:00
|
|
|
|
float dec_vd = 0.0;
|
|
|
|
|
float dec_vq = 0.0;
|
2020-03-06 13:04:59 -08:00
|
|
|
|
float dec_bemf = 0.0;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2022-04-05 15:35:11 -07:00
|
|
|
|
if (motor->m_control_mode < CONTROL_MODE_HANDBRAKE && conf_now->foc_cc_decoupling != FOC_CC_DECOUPLING_DISABLED) {
|
2020-03-21 03:40:50 -07:00
|
|
|
|
switch (conf_now->foc_cc_decoupling) {
|
2020-03-28 11:21:45 -07:00
|
|
|
|
case FOC_CC_DECOUPLING_CROSS:
|
2022-04-07 05:59:56 -07:00
|
|
|
|
dec_vd = state_m->iq_filter * motor->m_speed_est_fast * motor->p_lq; // m_speed_est_fast is ωe in [rad/s]
|
|
|
|
|
dec_vq = state_m->id_filter * motor->m_speed_est_fast * motor->p_ld;
|
2020-03-28 11:21:45 -07:00
|
|
|
|
break;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2020-03-28 11:21:45 -07:00
|
|
|
|
case FOC_CC_DECOUPLING_BEMF:
|
2021-05-07 00:47:45 -07:00
|
|
|
|
dec_bemf = motor->m_speed_est_fast * conf_now->foc_motor_flux_linkage;
|
2020-03-28 11:21:45 -07:00
|
|
|
|
break;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2020-03-28 11:21:45 -07:00
|
|
|
|
case FOC_CC_DECOUPLING_CROSS_BEMF:
|
2022-04-07 05:59:56 -07:00
|
|
|
|
dec_vd = state_m->iq_filter * motor->m_speed_est_fast * motor->p_lq;
|
|
|
|
|
dec_vq = state_m->id_filter * motor->m_speed_est_fast * motor->p_ld;
|
2021-05-07 00:47:45 -07:00
|
|
|
|
dec_bemf = motor->m_speed_est_fast * conf_now->foc_motor_flux_linkage;
|
2020-03-28 11:21:45 -07:00
|
|
|
|
break;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2020-03-28 11:21:45 -07:00
|
|
|
|
default:
|
|
|
|
|
break;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-11-04 23:32:08 -07:00
|
|
|
|
state_m->vd -= dec_vd; //Negative sign as in the PMSM equations
|
2020-03-06 13:04:59 -08:00
|
|
|
|
state_m->vq += dec_vq + dec_bemf;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2021-11-21 09:17:40 -08:00
|
|
|
|
// Calculate the max length of the voltage space vector without overmodulation.
|
|
|
|
|
// Is simply 1/sqrt(3) * v_bus. See https://microchipdeveloper.com/mct5001:start. Adds margin with max_duty.
|
2021-11-07 04:37:09 -08:00
|
|
|
|
float max_v_mag = ONE_BY_SQRT3 * max_duty * state_m->v_bus;
|
2020-01-12 12:25:21 -08:00
|
|
|
|
|
2021-04-10 02:37:35 -07:00
|
|
|
|
// Saturation and anti-windup. Notice that the d-axis has priority as it controls field
|
|
|
|
|
// weakening and the efficiency.
|
|
|
|
|
float vd_presat = state_m->vd;
|
2021-07-14 10:59:34 -07:00
|
|
|
|
utils_truncate_number_abs((float*)&state_m->vd, max_v_mag);
|
2021-04-10 16:02:15 -07:00
|
|
|
|
state_m->vd_int += (state_m->vd - vd_presat);
|
2021-04-10 02:37:35 -07:00
|
|
|
|
|
|
|
|
|
float max_vq = sqrtf(SQ(max_v_mag) - SQ(state_m->vd));
|
|
|
|
|
float vq_presat = state_m->vq;
|
2021-07-14 10:59:34 -07:00
|
|
|
|
utils_truncate_number_abs((float*)&state_m->vq, max_vq);
|
2021-04-10 16:02:15 -07:00
|
|
|
|
state_m->vq_int += (state_m->vq - vq_presat);
|
2021-04-10 02:37:35 -07:00
|
|
|
|
|
2020-01-12 12:25:21 -08:00
|
|
|
|
utils_saturate_vector_2d((float*)&state_m->vd, (float*)&state_m->vq, max_v_mag);
|
2021-04-10 02:37:35 -07:00
|
|
|
|
|
2021-11-07 04:35:08 -08:00
|
|
|
|
// mod_d and mod_q are normalized such that 1 corresponds to the max possible voltage:
|
2021-11-07 04:37:09 -08:00
|
|
|
|
// voltage_normalize = 1/(2/3*V_bus)
|
2021-11-07 04:35:08 -08:00
|
|
|
|
// This includes overmodulation and therefore cannot be made in any direction.
|
|
|
|
|
// Note that this scaling is different from max_v_mag, which is without over modulation.
|
2021-11-07 04:37:09 -08:00
|
|
|
|
const float voltage_normalize = 1.5 / state_m->v_bus;
|
|
|
|
|
state_m->mod_d = state_m->vd * voltage_normalize;
|
|
|
|
|
state_m->mod_q = state_m->vq * voltage_normalize;
|
2021-11-07 14:00:48 -08:00
|
|
|
|
UTILS_NAN_ZERO(state_m->mod_q_filter);
|
|
|
|
|
UTILS_LP_FAST(state_m->mod_q_filter, state_m->mod_q, 0.2);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
2015-12-23 15:43:31 -08:00
|
|
|
|
// TODO: Have a look at this?
|
2020-02-16 06:07:08 -08:00
|
|
|
|
#ifdef HW_HAS_INPUT_CURRENT_SENSOR
|
|
|
|
|
state_m->i_bus = GET_INPUT_CURRENT();
|
|
|
|
|
#else
|
2021-10-16 03:54:33 -07:00
|
|
|
|
state_m->i_bus = state_m->mod_alpha_measured * state_m->i_alpha + state_m->mod_beta_measured * state_m->i_beta;
|
|
|
|
|
// TODO: Also calculate motor power based on v_alpha, v_beta, i_alpha and i_beta. This is much more accurate
|
|
|
|
|
// with phase filters than using the modulation and bus current.
|
2020-02-16 06:07:08 -08:00
|
|
|
|
#endif
|
2022-03-07 08:33:51 -08:00
|
|
|
|
state_m->i_abs = NORM2_f(state_m->id, state_m->iq);
|
|
|
|
|
state_m->i_abs_filter = NORM2_f(state_m->id_filter, state_m->iq_filter);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-11-08 09:13:27 -08:00
|
|
|
|
// Inverse Park transform: transforms the (normalized) voltages from the rotor reference frame to the stator frame
|
2022-02-06 14:12:05 -08:00
|
|
|
|
state_m->mod_alpha_raw = c * state_m->mod_d - s * state_m->mod_q;
|
|
|
|
|
state_m->mod_beta_raw = c * state_m->mod_q + s * state_m->mod_d;
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2022-02-06 14:12:05 -08:00
|
|
|
|
update_valpha_vbeta(motor, state_m->mod_alpha_raw, state_m->mod_beta_raw);
|
2022-04-06 06:12:24 -07:00
|
|
|
|
|
2022-11-08 09:13:27 -08:00
|
|
|
|
// Dead time compensated values for vd and vq. Note that these are not used to control the switching times.
|
2020-03-16 10:32:39 -07:00
|
|
|
|
state_m->vd = c * motor->m_motor_state.v_alpha + s * motor->m_motor_state.v_beta;
|
|
|
|
|
state_m->vq = c * motor->m_motor_state.v_beta - s * motor->m_motor_state.v_alpha;
|
2016-01-30 06:57:51 -08:00
|
|
|
|
|
2023-12-26 13:37:41 -08:00
|
|
|
|
mc_audio_state *audio = &motor->m_audio;
|
|
|
|
|
switch (audio->mode) {
|
|
|
|
|
case MC_AUDIO_TABLE: {
|
|
|
|
|
float output = 0.0;
|
|
|
|
|
|
|
|
|
|
for (int i = 0;i < MC_AUDIO_CHANNELS; i++) {
|
|
|
|
|
float volts = audio->table_voltage[i];
|
|
|
|
|
audio->table_pos[i] += (float)audio->table_len[i] * audio->table_freq[i] * dt;
|
|
|
|
|
if (audio->table_pos[i] >= audio->table_len[i]) {
|
|
|
|
|
audio->table_pos[i] -= (float)audio->table_len[i];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (volts > 0.01) {
|
|
|
|
|
int index = floorf(audio->table_pos[i]);
|
|
|
|
|
output += audio->table[i][index] * volts;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Inject voltage along q-axis as that gives the most volume
|
|
|
|
|
output *= voltage_normalize;
|
|
|
|
|
state_m->mod_alpha_raw += -s * output;
|
|
|
|
|
state_m->mod_beta_raw += c * output;
|
|
|
|
|
utils_saturate_vector_2d((float*)&state_m->mod_alpha_raw, (float*)&state_m->mod_beta_raw, SQRT3_BY_2 * 0.95);
|
|
|
|
|
} break;
|
|
|
|
|
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
|
// HFI
|
|
|
|
|
if (do_hfi) {
|
2022-06-08 05:00:43 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
if (motor == &m_motor_2) {
|
|
|
|
|
CURRENT_FILTER_OFF_M2();
|
|
|
|
|
} else {
|
|
|
|
|
CURRENT_FILTER_OFF();
|
|
|
|
|
}
|
|
|
|
|
#else
|
2020-01-20 00:39:33 -08:00
|
|
|
|
CURRENT_FILTER_OFF();
|
2022-06-08 05:00:43 -07:00
|
|
|
|
#endif
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-04-07 03:51:19 -07:00
|
|
|
|
float mod_alpha_v7 = state_m->mod_alpha_raw;
|
|
|
|
|
float mod_beta_v7 = state_m->mod_beta_raw;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
2022-04-07 03:51:19 -07:00
|
|
|
|
float mod_alpha_v0 = state_m->mod_alpha_raw;
|
|
|
|
|
float mod_beta_v0 = state_m->mod_beta_raw;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2020-01-28 10:46:19 -08:00
|
|
|
|
float hfi_voltage;
|
2020-03-21 03:40:50 -07:00
|
|
|
|
if (motor->m_hfi.est_done_cnt < conf_now->foc_hfi_start_samples) {
|
|
|
|
|
hfi_voltage = conf_now->foc_hfi_voltage_start;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
} else {
|
2021-03-27 03:18:47 -07:00
|
|
|
|
hfi_voltage = utils_map(fabsf(state_m->iq), -0.01, conf_now->l_current_max,
|
2022-04-05 15:35:11 -07:00
|
|
|
|
conf_now->foc_hfi_voltage_run, conf_now->foc_hfi_voltage_max);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
}
|
|
|
|
|
|
2021-10-24 04:25:12 -07:00
|
|
|
|
utils_truncate_number_abs(&hfi_voltage, state_m->v_bus * (1.0 - fabsf(state_m->duty_now)) * SQRT3_BY_2 * (2.0 / 3.0) * 0.95);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-04-05 15:35:11 -07:00
|
|
|
|
if ((conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V4 || conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V5) && hfi_est_done) {
|
|
|
|
|
if (motor->m_hfi.is_samp_n) {
|
|
|
|
|
float sample_now = c * motor->m_i_beta_sample_with_offset - s * motor->m_i_alpha_sample_with_offset;
|
2022-04-07 04:10:17 -07:00
|
|
|
|
float di = (motor->m_hfi.prev_sample - sample_now);
|
2022-04-05 15:35:11 -07:00
|
|
|
|
|
|
|
|
|
if (!motor->m_using_encoder) {
|
2022-04-25 03:53:04 -07:00
|
|
|
|
motor->m_hfi.double_integrator = -motor->m_speed_est_fast;
|
2022-04-05 15:35:11 -07:00
|
|
|
|
motor->m_hfi.angle = motor->m_phase_now_observer;
|
|
|
|
|
} else {
|
2022-04-06 16:46:34 -07:00
|
|
|
|
float hfi_dt = dt * 2.0;
|
2022-04-06 10:41:46 -07:00
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (conf_now->foc_control_sample_mode != FOC_CONTROL_SAMPLE_MODE_V0_V7 && conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V4) {
|
2022-04-06 16:46:34 -07:00
|
|
|
|
hfi_dt = dt;
|
2022-04-06 10:41:46 -07:00
|
|
|
|
}
|
|
|
|
|
#endif
|
2022-04-07 03:51:19 -07:00
|
|
|
|
foc_hfi_adjust_angle(
|
2022-04-07 05:59:56 -07:00
|
|
|
|
(di * conf_now->foc_f_zv) / (hfi_voltage * motor->p_inv_ld_lq),
|
2022-04-07 03:51:19 -07:00
|
|
|
|
motor, hfi_dt
|
|
|
|
|
);
|
2022-04-05 15:35:11 -07:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (conf_now->foc_control_sample_mode == FOC_CONTROL_SAMPLE_MODE_V0_V7 || conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V5) {
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v7 -= hfi_voltage * c * voltage_normalize;
|
|
|
|
|
mod_beta_v7 -= hfi_voltage * s * voltage_normalize;
|
2022-04-05 15:35:11 -07:00
|
|
|
|
} else {
|
|
|
|
|
motor->m_hfi.prev_sample = c * motor->m_i_beta_sample_next - s * motor->m_i_alpha_sample_next;
|
|
|
|
|
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v0 -= hfi_voltage * c * voltage_normalize;
|
|
|
|
|
mod_beta_v0 -= hfi_voltage * s * voltage_normalize;
|
2022-04-05 15:35:11 -07:00
|
|
|
|
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v7 += hfi_voltage * c * voltage_normalize;
|
|
|
|
|
mod_beta_v7 += hfi_voltage * s * voltage_normalize;
|
2022-04-05 15:35:11 -07:00
|
|
|
|
|
|
|
|
|
motor->m_hfi.is_samp_n = !motor->m_hfi.is_samp_n;
|
|
|
|
|
motor->m_i_alpha_beta_has_offset = true;
|
|
|
|
|
}
|
|
|
|
|
#else
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v7 -= hfi_voltage * c * voltage_normalize;
|
|
|
|
|
mod_beta_v7 -= hfi_voltage * s * voltage_normalize;
|
2022-04-05 15:35:11 -07:00
|
|
|
|
#endif
|
|
|
|
|
} else {
|
|
|
|
|
motor->m_hfi.prev_sample = c * state_m->i_beta - s * state_m->i_alpha;
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v7 += hfi_voltage * c * voltage_normalize;
|
|
|
|
|
mod_beta_v7 += hfi_voltage * s * voltage_normalize;
|
2022-04-05 15:35:11 -07:00
|
|
|
|
}
|
|
|
|
|
} else if ((conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V2 || conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V3) && hfi_est_done) {
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (motor->m_hfi.is_samp_n) {
|
|
|
|
|
if (fabsf(state_m->iq_target) > conf_now->foc_hfi_hyst) {
|
|
|
|
|
motor->m_hfi.sign_last_sample = SIGN(state_m->iq_target);
|
|
|
|
|
}
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-03-23 14:26:05 -07:00
|
|
|
|
float sample_now = motor->m_hfi.cos_last * motor->m_i_alpha_sample_with_offset +
|
|
|
|
|
motor->m_hfi.sin_last * motor->m_i_beta_sample_with_offset;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
float di = (sample_now - motor->m_hfi.prev_sample);
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-04-06 10:21:22 -07:00
|
|
|
|
if (!motor->m_using_encoder) {
|
2022-04-25 03:53:04 -07:00
|
|
|
|
motor->m_hfi.double_integrator = -motor->m_speed_est_fast;
|
2022-04-06 10:21:22 -07:00
|
|
|
|
motor->m_hfi.angle = motor->m_phase_now_observer;
|
|
|
|
|
} else {
|
2022-04-07 05:59:56 -07:00
|
|
|
|
float hfi_dt = dt * 2.0;
|
2022-04-06 10:41:46 -07:00
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (conf_now->foc_control_sample_mode != FOC_CONTROL_SAMPLE_MODE_V0_V7 && conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V2) {
|
2022-04-07 05:59:56 -07:00
|
|
|
|
hfi_dt = dt;
|
2022-04-06 10:21:22 -07:00
|
|
|
|
}
|
2022-04-07 05:59:56 -07:00
|
|
|
|
#endif
|
|
|
|
|
foc_hfi_adjust_angle(
|
|
|
|
|
motor->m_hfi.sign_last_sample * ((conf_now->foc_f_zv * di) /
|
|
|
|
|
hfi_voltage - motor->p_v2_v3_inv_avg_half) / motor->p_inv_ld_lq,
|
|
|
|
|
motor, hfi_dt
|
|
|
|
|
);
|
2022-03-23 07:04:30 -07:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-07 03:51:19 -07:00
|
|
|
|
// Use precomputed rotation matrix
|
|
|
|
|
if (motor->m_hfi.sign_last_sample > 0) {
|
|
|
|
|
// +45 Degrees
|
|
|
|
|
motor->m_hfi.sin_last = ONE_BY_SQRT2 * (c + s);
|
|
|
|
|
motor->m_hfi.cos_last = ONE_BY_SQRT2 * (c - s);
|
|
|
|
|
} else {
|
|
|
|
|
// -45 Degrees
|
|
|
|
|
motor->m_hfi.sin_last = ONE_BY_SQRT2 * (-c + s);
|
|
|
|
|
motor->m_hfi.cos_last = ONE_BY_SQRT2 * (c + s);
|
|
|
|
|
}
|
2022-03-23 07:04:30 -07:00
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (conf_now->foc_control_sample_mode == FOC_CONTROL_SAMPLE_MODE_V0_V7 || conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V3) {
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v7 += hfi_voltage * motor->m_hfi.cos_last * voltage_normalize;
|
|
|
|
|
mod_beta_v7 += hfi_voltage * motor->m_hfi.sin_last * voltage_normalize;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
} else {
|
|
|
|
|
motor->m_hfi.prev_sample = motor->m_hfi.cos_last * motor->m_i_alpha_sample_next +
|
|
|
|
|
motor->m_hfi.sin_last * motor->m_i_beta_sample_next;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v0 += hfi_voltage * motor->m_hfi.cos_last * voltage_normalize;
|
|
|
|
|
mod_beta_v0 += hfi_voltage * motor->m_hfi.sin_last * voltage_normalize;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v7 -= hfi_voltage * motor->m_hfi.cos_last * voltage_normalize;
|
|
|
|
|
mod_beta_v7 -= hfi_voltage * motor->m_hfi.sin_last * voltage_normalize;
|
2022-03-23 14:26:05 -07:00
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
motor->m_hfi.is_samp_n = !motor->m_hfi.is_samp_n;
|
2022-03-23 14:26:05 -07:00
|
|
|
|
motor->m_i_alpha_beta_has_offset = true;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
}
|
|
|
|
|
#else
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v7 += hfi_voltage * motor->m_hfi.cos_last * voltage_normalize;
|
|
|
|
|
mod_beta_v7 += hfi_voltage * motor->m_hfi.sin_last * voltage_normalize;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
#endif
|
|
|
|
|
} else {
|
|
|
|
|
motor->m_hfi.prev_sample = motor->m_hfi.cos_last * state_m->i_alpha + motor->m_hfi.sin_last * state_m->i_beta;
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v7 -= hfi_voltage * motor->m_hfi.cos_last * voltage_normalize;
|
|
|
|
|
mod_beta_v7 -= hfi_voltage * motor->m_hfi.sin_last * voltage_normalize;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
}
|
2020-01-20 00:39:33 -08:00
|
|
|
|
} else {
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (motor->m_hfi.is_samp_n) {
|
|
|
|
|
float sample_now = (utils_tab_sin_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * state_m->i_alpha -
|
|
|
|
|
utils_tab_cos_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * state_m->i_beta);
|
|
|
|
|
float di = (sample_now - motor->m_hfi.prev_sample);
|
|
|
|
|
|
|
|
|
|
motor->m_hfi.buffer_current[motor->m_hfi.ind] = di;
|
|
|
|
|
|
|
|
|
|
if (di > 0.01) {
|
|
|
|
|
motor->m_hfi.buffer[motor->m_hfi.ind] = hfi_voltage / (conf_now->foc_f_zv * di);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
motor->m_hfi.ind++;
|
|
|
|
|
if (motor->m_hfi.ind == motor->m_hfi.samples) {
|
|
|
|
|
motor->m_hfi.ind = 0;
|
|
|
|
|
motor->m_hfi.ready = true;
|
|
|
|
|
}
|
2020-01-28 10:46:19 -08:00
|
|
|
|
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v7 += hfi_voltage * utils_tab_sin_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * voltage_normalize;
|
|
|
|
|
mod_beta_v7 -= hfi_voltage * utils_tab_cos_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * voltage_normalize;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
} else {
|
|
|
|
|
motor->m_hfi.prev_sample = utils_tab_sin_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * state_m->i_alpha -
|
|
|
|
|
utils_tab_cos_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * state_m->i_beta;
|
|
|
|
|
|
2022-04-07 03:51:19 -07:00
|
|
|
|
mod_alpha_v7 -= hfi_voltage * utils_tab_sin_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * voltage_normalize;
|
|
|
|
|
mod_beta_v7 += hfi_voltage * utils_tab_cos_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * voltage_normalize;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
}
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-07 03:51:19 -07:00
|
|
|
|
utils_saturate_vector_2d(&mod_alpha_v7, &mod_beta_v7, SQRT3_BY_2 * 0.95);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_hfi.is_samp_n = !motor->m_hfi.is_samp_n;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
|
2023-09-23 05:45:00 -07:00
|
|
|
|
if (conf_now->foc_control_sample_mode == FOC_CONTROL_SAMPLE_MODE_V0_V7) {
|
2022-04-07 03:51:19 -07:00
|
|
|
|
state_m->mod_alpha_raw = mod_alpha_v7;
|
|
|
|
|
state_m->mod_beta_raw = mod_beta_v7;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
} else {
|
2022-03-23 07:04:30 -07:00
|
|
|
|
#ifdef HW_HAS_PHASE_SHUNTS
|
2022-04-05 15:35:11 -07:00
|
|
|
|
if (conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V2 || conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_V4) {
|
2022-04-07 03:51:19 -07:00
|
|
|
|
utils_saturate_vector_2d(&mod_alpha_v0, &mod_beta_v0, SQRT3_BY_2 * 0.95);
|
|
|
|
|
state_m->mod_alpha_raw = mod_alpha_v0;
|
|
|
|
|
state_m->mod_beta_raw = mod_beta_v0;
|
2022-03-23 14:26:05 -07:00
|
|
|
|
}
|
2022-03-23 07:04:30 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
|
// Delay adding the HFI voltage when not sampling in both 0 vectors, as it will cancel
|
|
|
|
|
// itself with the opposite pulse from the previous HFI sample. This makes more sense
|
2020-01-28 10:46:19 -08:00
|
|
|
|
// when drawing the SVM waveform.
|
2022-04-07 03:51:19 -07:00
|
|
|
|
foc_svm(mod_alpha_v7, mod_beta_v7, TIM1->ARR,
|
2020-03-28 11:21:45 -07:00
|
|
|
|
(uint32_t*)&motor->m_duty1_next,
|
|
|
|
|
(uint32_t*)&motor->m_duty2_next,
|
|
|
|
|
(uint32_t*)&motor->m_duty3_next,
|
|
|
|
|
(uint32_t*)&state_m->svm_sector);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_duty_next_set = true;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
|
|
|
|
} else {
|
2022-06-08 05:00:43 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
if (motor == &m_motor_2) {
|
|
|
|
|
CURRENT_FILTER_ON_M2();
|
|
|
|
|
} else {
|
|
|
|
|
CURRENT_FILTER_ON();
|
|
|
|
|
}
|
|
|
|
|
#else
|
2020-01-20 00:39:33 -08:00
|
|
|
|
CURRENT_FILTER_ON();
|
2022-06-08 05:00:43 -07:00
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_hfi.ind = 0;
|
|
|
|
|
motor->m_hfi.ready = false;
|
|
|
|
|
motor->m_hfi.is_samp_n = false;
|
|
|
|
|
motor->m_hfi.prev_sample = 0.0;
|
2022-04-05 15:35:11 -07:00
|
|
|
|
motor->m_hfi.double_integrator = 0.0;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
|
|
|
|
|
2016-01-30 06:57:51 -08:00
|
|
|
|
// Set output (HW Dependent)
|
|
|
|
|
uint32_t duty1, duty2, duty3, top;
|
|
|
|
|
top = TIM1->ARR;
|
2022-04-06 06:12:24 -07:00
|
|
|
|
|
2022-02-21 00:25:46 -08:00
|
|
|
|
// Calculate the duty cycles for all the phases. This also injects a zero modulation signal to
|
2021-11-21 09:17:40 -08:00
|
|
|
|
// be able to fully utilize the bus voltage. See https://microchipdeveloper.com/mct5001:start
|
2022-04-06 06:12:24 -07:00
|
|
|
|
foc_svm(state_m->mod_alpha_raw, state_m->mod_beta_raw, top, &duty1, &duty2, &duty3, (uint32_t*)&state_m->svm_sector);
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
if (motor == &m_motor_1) {
|
|
|
|
|
TIMER_UPDATE_DUTY_M1(duty1, duty2, duty3);
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_PARALLEL
|
|
|
|
|
TIMER_UPDATE_DUTY_M2(duty1, duty2, duty3);
|
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
} else {
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#ifndef HW_HAS_DUAL_PARALLEL
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIMER_UPDATE_DUTY_M2(duty1, duty2, duty3);
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-01-12 12:25:21 -08:00
|
|
|
|
// do not allow to turn on PWM outputs if virtual motor is used
|
|
|
|
|
if(virtual_motor_is_connected() == false) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (!motor->m_output_on) {
|
|
|
|
|
start_pwm_hw(motor);
|
2019-04-06 06:36:00 -07:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
static void update_valpha_vbeta(motor_all_state_t *motor, float mod_alpha, float mod_beta) {
|
|
|
|
|
motor_state_t *state_m = &motor->m_motor_state;
|
|
|
|
|
mc_configuration *conf_now = motor->m_conf;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
float Va, Vb, Vc;
|
|
|
|
|
|
|
|
|
|
volatile float *ofs_volt = conf_now->foc_offsets_voltage_undriven;
|
|
|
|
|
if (motor->m_state == MC_STATE_RUNNING) {
|
|
|
|
|
ofs_volt = conf_now->foc_offsets_voltage;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_DUAL_MOTORS
|
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
|
if (&m_motor_1 != motor) {
|
|
|
|
|
Va = (ADC_VOLTS(ADC_IND_SENS4) - ofs_volt[0]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vb = (ADC_VOLTS(ADC_IND_SENS5) - ofs_volt[1]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vc = (ADC_VOLTS(ADC_IND_SENS6) - ofs_volt[2]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
} else {
|
|
|
|
|
Va = (ADC_VOLTS(ADC_IND_SENS1) - ofs_volt[0]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vb = (ADC_VOLTS(ADC_IND_SENS2) - ofs_volt[1]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vc = (ADC_VOLTS(ADC_IND_SENS3) - ofs_volt[2]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
}
|
|
|
|
|
#else
|
|
|
|
|
if (&m_motor_1 != motor) {
|
|
|
|
|
Va = (ADC_VOLTS(ADC_IND_SENS4) - ofs_volt[0]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vb = (ADC_VOLTS(ADC_IND_SENS6) - ofs_volt[2]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vc = (ADC_VOLTS(ADC_IND_SENS5) - ofs_volt[1]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
} else {
|
|
|
|
|
Va = (ADC_VOLTS(ADC_IND_SENS1) - ofs_volt[0]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vb = (ADC_VOLTS(ADC_IND_SENS3) - ofs_volt[2]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vc = (ADC_VOLTS(ADC_IND_SENS2) - ofs_volt[1]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#else
|
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
|
Va = (ADC_VOLTS(ADC_IND_SENS1) - ofs_volt[0]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vb = (ADC_VOLTS(ADC_IND_SENS2) - ofs_volt[1]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vc = (ADC_VOLTS(ADC_IND_SENS3) - ofs_volt[2]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
#else
|
|
|
|
|
Va = (ADC_VOLTS(ADC_IND_SENS1) - ofs_volt[0]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vb = (ADC_VOLTS(ADC_IND_SENS3) - ofs_volt[2]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
Vc = (ADC_VOLTS(ADC_IND_SENS2) - ofs_volt[1]) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
|
|
|
|
|
#endif
|
|
|
|
|
#endif
|
|
|
|
|
|
2021-10-16 03:54:33 -07:00
|
|
|
|
// Deadtime compensation
|
|
|
|
|
float s = state_m->phase_sin;
|
|
|
|
|
float c = state_m->phase_cos;
|
|
|
|
|
const float i_alpha_filter = c * state_m->id_filter - s * state_m->iq_filter;
|
|
|
|
|
const float i_beta_filter = c * state_m->iq_filter + s * state_m->id_filter;
|
|
|
|
|
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;
|
2021-11-07 04:37:09 -08:00
|
|
|
|
|
2021-11-21 09:17:40 -08:00
|
|
|
|
// mod_alpha_sign = 2/3*sign(ia) - 1/3*sign(ib) - 1/3*sign(ic)
|
|
|
|
|
// mod_beta_sign = 1/sqrt(3)*sign(ib) - 1/sqrt(3)*sign(ic)
|
2021-11-07 04:37:09 -08:00
|
|
|
|
const float mod_alpha_filter_sgn = (1.0 / 3.0) * (2.0 * SIGN(ia_filter) - SIGN(ib_filter) - SIGN(ic_filter));
|
|
|
|
|
const float mod_beta_filter_sgn = ONE_BY_SQRT3 * (SIGN(ib_filter) - SIGN(ic_filter));
|
|
|
|
|
|
2022-01-06 11:44:00 -08:00
|
|
|
|
const float mod_comp_fact = conf_now->foc_dt_us * 1e-6 * conf_now->foc_f_zv;
|
2021-10-16 03:54:33 -07:00
|
|
|
|
const float mod_alpha_comp = mod_alpha_filter_sgn * mod_comp_fact;
|
|
|
|
|
const float mod_beta_comp = mod_beta_filter_sgn * mod_comp_fact;
|
|
|
|
|
|
|
|
|
|
mod_alpha -= mod_alpha_comp;
|
|
|
|
|
mod_beta -= mod_beta_comp;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
|
|
|
|
state_m->va = Va;
|
|
|
|
|
state_m->vb = Vb;
|
|
|
|
|
state_m->vc = Vc;
|
2021-10-16 03:54:33 -07:00
|
|
|
|
state_m->mod_alpha_measured = mod_alpha;
|
|
|
|
|
state_m->mod_beta_measured = mod_beta;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
2021-11-21 09:17:40 -08:00
|
|
|
|
// v_alpha = 2/3*Va - 1/3*Vb - 1/3*Vc
|
|
|
|
|
// v_beta = 1/sqrt(3)*Vb - 1/sqrt(3)*Vc
|
2021-11-07 04:37:09 -08:00
|
|
|
|
float v_alpha = (1.0 / 3.0) * (2.0 * Va - Vb - Vc);
|
|
|
|
|
float v_beta = ONE_BY_SQRT3 * (Vb - Vc);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
|
|
|
|
// Keep the modulation updated so that the filter stays updated
|
|
|
|
|
// even when the motor is undriven.
|
|
|
|
|
if (motor->m_state != MC_STATE_RUNNING) {
|
2021-11-07 04:37:09 -08:00
|
|
|
|
/* voltage_normalize = 1/(2/3*V_bus) */
|
|
|
|
|
const float voltage_normalize = 1.5 / state_m->v_bus;
|
2021-11-07 04:35:08 -08:00
|
|
|
|
|
2021-11-07 04:37:09 -08:00
|
|
|
|
mod_alpha = v_alpha * voltage_normalize;
|
|
|
|
|
mod_beta = v_beta * voltage_normalize;
|
2021-02-28 11:36:02 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-07 05:59:56 -07:00
|
|
|
|
float abs_rpm = fabsf(RADPS2RPM_f(motor->m_speed_est_fast));
|
2021-02-28 11:36:02 -08:00
|
|
|
|
|
|
|
|
|
float filter_const = 1.0;
|
|
|
|
|
if (abs_rpm < 10000.0) {
|
|
|
|
|
filter_const = utils_map(abs_rpm, 0.0, 10000.0, 0.01, 1.0);
|
|
|
|
|
}
|
|
|
|
|
|
2022-03-07 08:33:51 -08:00
|
|
|
|
float v_mag = NORM2_f(v_alpha, v_beta);
|
2021-02-28 11:36:02 -08:00
|
|
|
|
// The 0.1 * v_mag term below compensates for the filter attenuation as the speed increases.
|
|
|
|
|
// It is chosen by trial and error, so this can be improved.
|
|
|
|
|
UTILS_LP_FAST(state_m->v_mag_filter, v_mag + 0.1 * v_mag * filter_const, filter_const);
|
|
|
|
|
UTILS_LP_FAST(state_m->mod_alpha_filter, mod_alpha, filter_const);
|
|
|
|
|
UTILS_LP_FAST(state_m->mod_beta_filter, mod_beta, filter_const);
|
|
|
|
|
UTILS_NAN_ZERO(state_m->v_mag_filter);
|
|
|
|
|
UTILS_NAN_ZERO(state_m->mod_alpha_filter);
|
|
|
|
|
UTILS_NAN_ZERO(state_m->mod_beta_filter);
|
|
|
|
|
|
|
|
|
|
mod_alpha = state_m->mod_alpha_filter;
|
|
|
|
|
mod_beta = state_m->mod_beta_filter;
|
|
|
|
|
|
|
|
|
|
if (motor->m_state == MC_STATE_RUNNING) {
|
|
|
|
|
#ifdef HW_HAS_PHASE_FILTERS
|
|
|
|
|
if (conf_now->foc_phase_filter_enable && abs_rpm < conf_now->foc_phase_filter_max_erpm) {
|
2022-04-06 12:08:48 -07:00
|
|
|
|
float mod_mag = NORM2_f(mod_alpha, mod_beta);
|
|
|
|
|
float v_mag_mod = mod_mag * (2.0 / 3.0) * state_m->v_bus;
|
|
|
|
|
|
2022-05-18 18:12:07 -07:00
|
|
|
|
if (!conf_now->foc_phase_filter_disable_fault && fabsf(v_mag_mod - state_m->v_mag_filter) > (conf_now->l_max_vin * 0.05)) {
|
2022-05-05 15:49:35 -07:00
|
|
|
|
mc_interface_set_fault_info("v_mag_mod: %.2f, v_mag_filter: %.2f", 2, v_mag_mod, state_m->v_mag_filter);
|
2022-04-06 12:08:48 -07:00
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_PHASE_FILTER, &m_motor_1 != motor, true);
|
|
|
|
|
}
|
|
|
|
|
|
2021-02-28 11:36:02 -08:00
|
|
|
|
// Compensate for the phase delay by using the direction of the modulation
|
|
|
|
|
// together with the magnitude from the phase filters
|
|
|
|
|
if (mod_mag > 0.04) {
|
|
|
|
|
state_m->v_alpha = mod_alpha / mod_mag * state_m->v_mag_filter;
|
|
|
|
|
state_m->v_beta = mod_beta / mod_mag * state_m->v_mag_filter;
|
|
|
|
|
} else {
|
|
|
|
|
state_m->v_alpha = v_alpha;
|
|
|
|
|
state_m->v_beta = v_beta;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
state_m->is_using_phase_filters = true;
|
|
|
|
|
} else {
|
|
|
|
|
#endif
|
|
|
|
|
state_m->v_alpha = mod_alpha * (2.0 / 3.0) * state_m->v_bus;
|
|
|
|
|
state_m->v_beta = mod_beta * (2.0 / 3.0) * state_m->v_bus;
|
|
|
|
|
state_m->is_using_phase_filters = false;
|
|
|
|
|
#ifdef HW_HAS_PHASE_FILTERS
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
} else {
|
|
|
|
|
state_m->v_alpha = v_alpha;
|
|
|
|
|
state_m->v_beta = v_beta;
|
|
|
|
|
state_m->is_using_phase_filters = false;
|
|
|
|
|
|
|
|
|
|
#ifdef HW_USE_LINE_TO_LINE
|
|
|
|
|
// rotate alpha-beta 30 degrees to compensate for line-to-line phase voltage sensing
|
|
|
|
|
float x_tmp = state_m->v_alpha;
|
|
|
|
|
float y_tmp = state_m->v_beta;
|
|
|
|
|
|
|
|
|
|
state_m->v_alpha = x_tmp * COS_MINUS_30_DEG - y_tmp * SIN_MINUS_30_DEG;
|
|
|
|
|
state_m->v_beta = x_tmp * SIN_MINUS_30_DEG + y_tmp * COS_MINUS_30_DEG;
|
|
|
|
|
|
|
|
|
|
// compensate voltage amplitude
|
|
|
|
|
state_m->v_alpha *= ONE_BY_SQRT3;
|
|
|
|
|
state_m->v_beta *= ONE_BY_SQRT3;
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
static void stop_pwm_hw(motor_all_state_t *motor) {
|
2020-04-03 11:12:12 -07:00
|
|
|
|
motor->m_id_set = 0.0;
|
|
|
|
|
motor->m_iq_set = 0.0;
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (motor == &m_motor_1) {
|
|
|
|
|
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);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
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);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
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);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_PARALLEL
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_ForcedAction_InActive);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Disable);
|
2020-03-29 08:00:43 -07:00
|
|
|
|
|
2020-03-28 11:21:45 -07:00
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_ForcedAction_InActive);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Disable);
|
2020-03-29 08:00:43 -07:00
|
|
|
|
|
2020-03-28 11:21:45 -07:00
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_ForcedAction_InActive);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Disable);
|
2020-03-29 08:00:43 -07:00
|
|
|
|
|
2020-03-28 11:21:45 -07:00
|
|
|
|
TIM_GenerateEvent(TIM8, TIM_EventSource_COM);
|
|
|
|
|
#endif
|
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
|
#ifdef HW_HAS_DRV8313
|
2020-03-16 10:32:39 -07:00
|
|
|
|
DISABLE_BR();
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
motor->m_output_on = false;
|
2021-03-22 04:13:19 -07:00
|
|
|
|
PHASE_FILTER_OFF();
|
2020-03-16 10:32:39 -07:00
|
|
|
|
} else {
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_ForcedAction_InActive);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_ForcedAction_InActive);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_ForcedAction_InActive);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
|
|
TIM_GenerateEvent(TIM8, TIM_EventSource_COM);
|
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_DRV8313_2
|
|
|
|
|
DISABLE_BR_2();
|
2016-11-04 07:18:34 -07:00
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
|
|
|
|
|
motor->m_output_on = false;
|
2021-03-22 04:13:19 -07:00
|
|
|
|
PHASE_FILTER_OFF_M2();
|
2020-03-16 10:32:39 -07:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
static void start_pwm_hw(motor_all_state_t *motor) {
|
2020-03-16 10:32:39 -07:00
|
|
|
|
if (motor == &m_motor_1) {
|
|
|
|
|
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);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
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);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
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);
|
2015-12-08 12:01:23 -08:00
|
|
|
|
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#ifdef HW_HAS_DUAL_PARALLEL
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_OCMode_PWM1);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Enable);
|
|
|
|
|
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_OCMode_PWM1);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Enable);
|
|
|
|
|
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_OCMode_PWM1);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Enable);
|
2021-03-22 04:13:19 -07:00
|
|
|
|
|
|
|
|
|
PHASE_FILTER_ON_M2();
|
2020-03-28 11:21:45 -07:00
|
|
|
|
#endif
|
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
|
// 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
|
2020-03-16 10:32:39 -07:00
|
|
|
|
ENABLE_BR();
|
|
|
|
|
#endif
|
|
|
|
|
motor->m_output_on = true;
|
2021-03-22 04:13:19 -07:00
|
|
|
|
PHASE_FILTER_ON();
|
2020-03-16 10:32:39 -07:00
|
|
|
|
} else {
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_OCMode_PWM1);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Enable);
|
|
|
|
|
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_OCMode_PWM1);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Enable);
|
|
|
|
|
|
|
|
|
|
TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_OCMode_PWM1);
|
|
|
|
|
TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
|
TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Enable);
|
|
|
|
|
|
|
|
|
|
#ifdef HW_HAS_DRV8313_2
|
|
|
|
|
ENABLE_BR_2();
|
2016-11-04 07:18:34 -07:00
|
|
|
|
#endif
|
2020-03-16 10:32:39 -07:00
|
|
|
|
motor->m_output_on = true;
|
2021-03-22 04:13:19 -07:00
|
|
|
|
PHASE_FILTER_ON_M2();
|
2020-03-16 10:32:39 -07:00
|
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
|
}
|
2016-01-27 14:57:23 -08:00
|
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
|
static void terminal_plot_hfi(int argc, const char **argv) {
|
|
|
|
|
if (argc == 2) {
|
|
|
|
|
int d = -1;
|
|
|
|
|
sscanf(argv[1], "%d", &d);
|
|
|
|
|
|
2022-03-23 07:04:30 -07:00
|
|
|
|
if (d == 0 || d == 1 || d == 2 || d == 3) {
|
2022-04-06 07:33:44 -07:00
|
|
|
|
get_motor_now()->m_hfi_plot_en = d;
|
|
|
|
|
if (get_motor_now()->m_hfi_plot_en == 1) {
|
|
|
|
|
get_motor_now()->m_hfi_plot_sample = 0.0;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
commands_init_plot("Sample", "Value");
|
2020-01-20 00:39:33 -08:00
|
|
|
|
commands_plot_add_graph("Phase");
|
|
|
|
|
commands_plot_add_graph("Phase bin2");
|
2020-01-28 10:46:19 -08:00
|
|
|
|
commands_plot_add_graph("Ld - Lq (uH");
|
|
|
|
|
commands_plot_add_graph("L Diff Sat (uH)");
|
|
|
|
|
commands_plot_add_graph("L Avg (uH)");
|
2022-04-06 07:33:44 -07:00
|
|
|
|
} else if (get_motor_now()->m_hfi_plot_en == 2) {
|
|
|
|
|
get_motor_now()->m_hfi_plot_sample = 0.0;
|
2020-01-28 10:46:19 -08:00
|
|
|
|
commands_init_plot("Sample Index", "Value");
|
|
|
|
|
commands_plot_add_graph("Current (A)");
|
|
|
|
|
commands_plot_add_graph("Inductance (uH)");
|
2022-04-06 07:33:44 -07:00
|
|
|
|
} else if (get_motor_now()->m_hfi_plot_en == 3) {
|
|
|
|
|
get_motor_now()->m_hfi_plot_sample = 0.0;
|
2022-03-23 07:04:30 -07:00
|
|
|
|
commands_init_plot("Sample Index", "Value");
|
|
|
|
|
commands_plot_add_graph("Inductance");;
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
|
|
|
|
|
2022-04-06 07:33:44 -07:00
|
|
|
|
commands_printf(get_motor_now()->m_hfi_plot_en ?
|
2020-03-28 11:21:45 -07:00
|
|
|
|
"HFI plot enabled" :
|
|
|
|
|
"HFI plot disabled");
|
2020-01-20 00:39:33 -08:00
|
|
|
|
} else {
|
2022-03-23 07:04:30 -07:00
|
|
|
|
commands_printf("Invalid Argument. en has to be 0, 1, 2 or 3.\n");
|
2020-01-20 00:39:33 -08:00
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
commands_printf("This command requires one argument.\n");
|
|
|
|
|
}
|
|
|
|
|
}
|