2015-12-08 12:01:23 -08:00
/*
2021-04-10 02:37:35 -07:00
Copyright 2016 - 2021 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"
# include "utils.h"
# 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"
2020-01-20 00:39:33 -08:00
# include "digital_filter.h"
2015-12-08 12:01:23 -08:00
// Private types
typedef struct {
2021-02-28 11:36:02 -08:00
float va ;
float vb ;
float vc ;
float v_mag_filter ;
float mod_alpha_filter ;
float mod_beta_filter ;
2021-10-16 03:54:33 -07:00
float mod_alpha_measured ;
float mod_beta_measured ;
2022-02-06 14:12:05 -08:00
float mod_alpha_raw ;
float mod_beta_raw ;
2015-12-08 12:01:23 -08:00
float id_target ;
float iq_target ;
float max_duty ;
float duty_now ;
float phase ;
2021-02-28 11:36:02 -08:00
float phase_cos ;
float phase_sin ;
2015-12-08 12:01:23 -08:00
float i_alpha ;
float i_beta ;
2015-12-23 15:43:31 -08:00
float i_abs ;
float i_abs_filter ;
2015-12-08 12:01:23 -08:00
float i_bus ;
float v_bus ;
float v_alpha ;
float v_beta ;
float mod_d ;
float mod_q ;
2021-11-07 14:00:48 -08:00
float mod_q_filter ;
2015-12-08 12:01:23 -08:00
float id ;
float iq ;
2015-12-23 15:43:31 -08:00
float id_filter ;
float iq_filter ;
2015-12-08 12:01:23 -08:00
float vd ;
float vq ;
2016-11-04 07:18:34 -07:00
float vd_int ;
float vq_int ;
2020-01-12 12:25:21 -08:00
float speed_rad_s ;
2016-06-27 08:29:09 -07:00
uint32_t svm_sector ;
2021-02-28 11:36:02 -08:00
bool is_using_phase_filters ;
2015-12-08 12:01:23 -08:00
} motor_state_t ;
typedef struct {
int sample_num ;
float avg_current_tot ;
float avg_voltage_tot ;
} mc_sample_t ;
2020-01-28 10:46:19 -08:00
typedef struct {
void ( * fft_bin0_func ) ( float * , float * , float * ) ;
void ( * fft_bin1_func ) ( float * , float * , float * ) ;
void ( * fft_bin2_func ) ( float * , float * , float * ) ;
int samples ;
int table_fact ;
float buffer [ 32 ] ;
float buffer_current [ 32 ] ;
bool ready ;
int ind ;
bool is_samp_n ;
float prev_sample ;
float angle ;
int est_done_cnt ;
float observer_zero_time ;
2020-03-16 10:32:39 -07:00
int flip_cnt ;
2020-01-28 10:46:19 -08:00
} hfi_state_t ;
2020-03-16 10:32:39 -07:00
typedef struct {
volatile mc_configuration * m_conf ;
mc_state m_state ;
mc_control_mode m_control_mode ;
motor_state_t m_motor_state ;
2021-03-24 05:09:05 -07:00
float m_curr_unbalance ;
float m_currents_adc [ 3 ] ;
2020-03-16 10:32:39 -07:00
bool m_phase_override ;
float m_phase_now_override ;
float m_duty_cycle_set ;
float m_id_set ;
float m_iq_set ;
2021-04-10 02:37:35 -07:00
float m_i_fw_set ;
2021-04-14 13:29:50 -07:00
float m_current_off_delay ;
2020-03-16 10:32:39 -07:00
float m_openloop_speed ;
float m_openloop_phase ;
bool m_output_on ;
float m_pos_pid_set ;
float m_speed_pid_set_rpm ;
2020-05-12 06:53:32 -07:00
float m_speed_command_rpm ;
2020-03-16 10:32:39 -07:00
float m_phase_now_observer ;
float m_phase_now_observer_override ;
2021-03-13 06:41:57 -08:00
float m_observer_x1_override ;
float m_observer_x2_override ;
2020-03-16 10:32:39 -07:00
bool m_phase_observer_override ;
float m_phase_now_encoder ;
float m_phase_now_encoder_no_index ;
float m_observer_x1 ;
float m_observer_x2 ;
float m_pll_phase ;
float m_pll_speed ;
mc_sample_t m_samples ;
int m_tachometer ;
int m_tachometer_abs ;
float m_pos_pid_now ;
float m_gamma_now ;
bool m_using_encoder ;
float m_speed_est_fast ;
2020-04-16 00:12:08 -07:00
float m_speed_est_faster ;
2020-03-16 10:32:39 -07:00
int m_duty1_next , m_duty2_next , m_duty3_next ;
bool m_duty_next_set ;
hfi_state_t m_hfi ;
int m_hfi_plot_en ;
float m_hfi_plot_sample ;
2021-11-25 15:43:41 -08:00
// For braking
2021-11-27 09:51:02 -08:00
float m_br_speed_before ;
float m_br_vq_before ;
int m_br_no_duty_samples ;
2021-11-25 15:43:41 -08:00
2021-08-16 11:33:26 -07:00
float m_duty_abs_filtered ;
2021-08-20 12:36:44 -07:00
float m_duty_filtered ;
2020-03-16 10:32:39 -07:00
bool m_was_control_duty ;
float m_duty_i_term ;
float m_openloop_angle ;
float m_x1_prev ;
float m_x2_prev ;
float m_phase_before_speed_est ;
int m_tacho_step_last ;
float m_pid_div_angle_last ;
2020-11-18 05:26:55 -08:00
float m_pid_div_angle_accumulator ;
2020-03-16 10:32:39 -07:00
float m_min_rpm_hyst_timer ;
float m_min_rpm_timer ;
bool m_cc_was_hfi ;
float m_pos_i_term ;
float m_pos_prev_error ;
float m_pos_dt_int ;
2021-07-11 11:19:26 -07:00
float m_pos_prev_proc ;
float m_pos_dt_int_proc ;
2020-03-16 10:32:39 -07:00
float m_pos_d_filter ;
2021-07-11 11:19:26 -07:00
float m_pos_d_filter_proc ;
2020-03-16 10:32:39 -07:00
float m_speed_i_term ;
float m_speed_prev_error ;
float m_speed_d_filter ;
int m_ang_hall_int_prev ;
bool m_using_hall ;
float m_ang_hall ;
2021-04-26 08:57:49 -07:00
float m_ang_hall_rate_limited ;
2020-04-16 15:38:36 -07:00
float m_hall_dt_diff_last ;
float m_hall_dt_diff_now ;
2022-01-19 09:52:45 -08:00
bool m_motor_released ;
2021-12-29 09:21:42 -08:00
// Resistance observer
float m_r_est ;
float m_r_est_state ;
2020-03-16 10:32:39 -07:00
} motor_all_state_t ;
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
void observer_update ( float v_alpha , float v_beta , float i_alpha , float i_beta ,
2020-03-28 11:21:45 -07:00
float dt , volatile float * x1 , volatile float * x2 , volatile float * phase , volatile motor_all_state_t * motor ) ;
2015-12-08 12:01:23 -08:00
static void pll_run ( float phase , float dt , volatile float * phase_var ,
2020-03-28 11:21:45 -07:00
volatile float * speed_var , volatile mc_configuration * conf ) ;
2020-03-16 10:32:39 -07:00
static void control_current ( volatile motor_all_state_t * motor , float dt ) ;
2021-02-28 11:36:02 -08:00
static void update_valpha_vbeta ( volatile motor_all_state_t * motor , float mod_alpha , float mod_beta ) ;
2021-11-13 12:51:20 -08:00
static void svm ( float alpha , float beta , uint32_t PWMFullDutyCycle ,
2020-03-28 11:21:45 -07:00
uint32_t * tAout , uint32_t * tBout , uint32_t * tCout , uint32_t * svm_sector ) ;
2021-07-12 05:31:01 -07:00
static void run_pid_control_pos ( float dt , volatile motor_all_state_t * motor ) ;
2020-03-16 10:32:39 -07:00
static void run_pid_control_speed ( float dt , volatile motor_all_state_t * motor ) ;
static void stop_pwm_hw ( volatile motor_all_state_t * motor ) ;
static void start_pwm_hw ( volatile motor_all_state_t * motor ) ;
static float correct_encoder ( float obs_angle , float enc_angle , float speed , float sl_erpm , volatile motor_all_state_t * motor ) ;
2020-04-16 15:38:36 -07:00
static float correct_hall ( float angle , float dt , volatile 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 ) ;
2021-04-11 02:31:25 -07:00
static void run_fw ( volatile motor_all_state_t * motor , float dt ) ;
2020-03-16 10:32:39 -07:00
static void timer_update ( volatile motor_all_state_t * motor , float dt ) ;
2020-02-16 06:07:08 -08:00
static void input_current_offset_measurement ( void ) ;
2020-03-16 10:32:39 -07:00
static void hfi_update ( volatile motor_all_state_t * motor ) ;
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
2020-03-16 10:32:39 -07:00
// Time Base configuration
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
// Channel 1, 2 and 3 Configuration in PWM mode
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 ) ;
// ------------- Timer2 for ADC sampling ------------- //
// Time Base configuration
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 ) ;
// TIM1 Master and TIM8 slave
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
// Enable TIM1 and TIM2
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
stop_pwm_hw ( & m_motor_1 ) ;
# ifdef HW_HAS_DUAL_MOTORS
stop_pwm_hw ( & m_motor_2 ) ;
# endif
// Main Output Enable
TIM_CtrlPWMOutputs ( TIM1 , ENABLE ) ;
TIM_CtrlPWMOutputs ( TIM8 , ENABLE ) ;
2020-03-17 01:38:09 -07:00
// Sample intervals
2020-03-16 10:32:39 -07:00
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 ) ;
}
void mcpwm_foc_init ( volatile mc_configuration * conf_m1 , volatile mc_configuration * conf_m2 ) {
utils_sys_lock_cnt ( ) ;
# ifndef HW_HAS_DUAL_MOTORS
( void ) conf_m2 ;
# endif
m_init_done = false ;
// Initialize variables
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 ;
2020-03-16 10:32:39 -07:00
update_hfi_samples ( m_motor_1 . m_conf - > foc_hfi_samples , & m_motor_1 ) ;
# 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 ;
2020-03-16 10:32:39 -07:00
update_hfi_samples ( m_motor_2 . m_conf - > foc_hfi_samples , & m_motor_2 ) ;
# 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 ) ;
TIM1 - > CNT = 0 ;
TIM2 - > CNT = 0 ;
TIM8 - > CNT = 0 ;
2020-01-20 00:39:33 -08:00
// ADC
2015-12-08 12:01:23 -08:00
ADC_CommonInitTypeDef ADC_CommonInitStructure ;
DMA_InitTypeDef DMA_InitStructure ;
ADC_InitTypeDef ADC_InitStructure ;
// Clock
RCC_AHB1PeriphClockCmd ( RCC_AHB1Periph_DMA2 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC , ENABLE ) ;
RCC_APB2PeriphClockCmd ( RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | RCC_APB2Periph_ADC3 , ENABLE ) ;
2016-11-04 07:18:34 -07:00
dmaStreamAllocate ( STM32_DMA_STREAM ( STM32_DMA_STREAM_ID ( 2 , 4 ) ) ,
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 for the ADC
DMA_InitStructure . DMA_Channel = DMA_Channel_0 ;
DMA_InitStructure . DMA_Memory0BaseAddr = ( uint32_t ) & ADC_Value ;
DMA_InitStructure . DMA_PeripheralBaseAddr = ( uint32_t ) & ADC - > CDR ;
DMA_InitStructure . DMA_DIR = DMA_DIR_PeripheralToMemory ;
DMA_InitStructure . DMA_BufferSize = HW_ADC_CHANNELS ;
DMA_InitStructure . DMA_PeripheralInc = DMA_PeripheralInc_Disable ;
DMA_InitStructure . DMA_MemoryInc = DMA_MemoryInc_Enable ;
DMA_InitStructure . DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord ;
DMA_InitStructure . DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord ;
DMA_InitStructure . DMA_Mode = DMA_Mode_Circular ;
DMA_InitStructure . DMA_Priority = DMA_Priority_High ;
DMA_InitStructure . DMA_FIFOMode = DMA_FIFOMode_Disable ;
DMA_InitStructure . DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
DMA_InitStructure . DMA_MemoryBurst = DMA_MemoryBurst_Single ;
DMA_InitStructure . DMA_PeripheralBurst = DMA_PeripheralBurst_Single ;
DMA_Init ( DMA2_Stream4 , & DMA_InitStructure ) ;
DMA_Cmd ( DMA2_Stream4 , ENABLE ) ;
2016-11-04 07:18:34 -07:00
DMA_ITConfig ( DMA2_Stream4 , DMA_IT_TC , ENABLE ) ;
2015-12-08 12:01:23 -08:00
// ADC Common Init
// Note that the ADC is running at 42MHz, which is higher than the
// specified 36MHz in the data sheet, but it works.
ADC_CommonInitStructure . ADC_Mode = ADC_TripleMode_RegSimult ;
ADC_CommonInitStructure . ADC_Prescaler = ADC_Prescaler_Div2 ;
ADC_CommonInitStructure . ADC_DMAAccessMode = ADC_DMAAccessMode_1 ;
ADC_CommonInitStructure . ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles ;
ADC_CommonInit ( & ADC_CommonInitStructure ) ;
// Channel-specific settings
ADC_InitStructure . ADC_Resolution = ADC_Resolution_12b ;
ADC_InitStructure . ADC_ScanConvMode = ENABLE ;
ADC_InitStructure . ADC_ContinuousConvMode = DISABLE ;
ADC_InitStructure . ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Falling ;
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
2020-03-16 10:32:39 -07:00
stop_pwm_hw ( & m_motor_1 ) ;
# ifdef HW_HAS_DUAL_MOTORS
stop_pwm_hw ( & m_motor_2 ) ;
# endif
2015-12-08 12:01:23 -08:00
// Sample intervals. For now they are fixed with voltage samples in the center of V7
// and current samples in the center of V0
2016-11-04 07:18:34 -07:00
TIMER_UPDATE_SAMP ( MCPWM_FOC_CURRENT_SAMP_OFFSET ) ;
2015-12-08 12:01:23 -08:00
2020-03-16 10:32:39 -07:00
// Enable CC2 interrupt, which will be fired in V0 and V7
TIM_ITConfig ( TIM2 , TIM_IT_CC2 , ENABLE ) ;
nvicEnableVector ( TIM2_IRQn , 6 ) ;
2017-09-04 12:12:43 -07:00
2015-12-08 12:01:23 -08:00
utils_sys_unlock_cnt ( ) ;
2019-02-18 10:30:19 -08:00
CURRENT_FILTER_ON ( ) ;
2015-12-08 12:01:23 -08:00
ENABLE_GATE ( ) ;
DCCAL_OFF ( ) ;
2021-02-28 11:36:02 -08:00
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 ;
}
}
}
if ( ! m_dccal_done ) {
for ( int i = 0 ; i < 3 ; i + + ) {
m_motor_1 . m_conf - > foc_offsets_voltage [ i ] = 0.0 ;
m_motor_1 . m_conf - > foc_offsets_voltage_undriven [ i ] = 0.0 ;
m_motor_1 . m_conf - > foc_offsets_current [ i ] = 2048 ;
2021-03-24 05:09:05 -07:00
2021-02-28 11:36:02 -08:00
# ifdef HW_HAS_DUAL_MOTORS
2021-05-13 02:45:30 -07:00
m_motor_2 . m_conf - > foc_offsets_voltage [ i ] = 0.0 ;
m_motor_2 . m_conf - > foc_offsets_voltage_undriven [ i ] = 0.0 ;
m_motor_2 . m_conf - > foc_offsets_current [ i ] = 2048 ;
2021-02-28 11:36:02 -08:00
# endif
2021-05-13 02:45:30 -07:00
}
2021-02-28 11:36:02 -08:00
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 ;
}
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 ) ;
2019-01-24 07:19:44 -08:00
// Check if the system has resumed from IWDG reset
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 ) ) ) ;
}
2020-03-16 14:42:44 -07:00
static volatile motor_all_state_t * 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 ;
}
2015-12-08 12:01:23 -08:00
void mcpwm_foc_set_configuration ( volatile mc_configuration * configuration ) {
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_conf = configuration ;
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 ;
stop_pwm_hw ( & m_motor_1 ) ;
m_motor_2 . m_control_mode = CONTROL_MODE_NONE ;
m_motor_2 . m_state = MC_STATE_OFF ;
stop_pwm_hw ( & m_motor_2 ) ;
2022-01-06 11:44:00 -08:00
timer_reinit ( ( int ) configuration - > foc_f_zv ) ;
2020-03-16 10:32:39 -07:00
# else
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_NONE ;
motor_now ( ) - > m_state = MC_STATE_OFF ;
stop_pwm_hw ( 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
2020-03-16 14:42:44 -07:00
if ( ( ( 1 < < motor_now ( ) - > m_conf - > foc_hfi_samples ) * 8 ) ! = motor_now ( ) - > m_hfi . samples ) {
motor_now ( ) - > m_control_mode = CONTROL_MODE_NONE ;
motor_now ( ) - > m_state = MC_STATE_OFF ;
stop_pwm_hw ( motor_now ( ) ) ;
2020-03-20 09:35:25 -07:00
update_hfi_samples ( motor_now ( ) - > m_conf - > foc_hfi_samples , 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 ) {
2020-03-16 14:42:44 -07:00
return motor_now ( ) - > m_state ;
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 ) {
2020-10-27 10:44:58 -07:00
volatile motor_all_state_t * motor = 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
* The duty cycle to use .
*/
void mcpwm_foc_set_duty ( float dutyCycle ) {
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_DUTY ;
motor_now ( ) - > m_duty_cycle_set = dutyCycle ;
2015-12-08 12:01:23 -08:00
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
2022-01-19 09:52:45 -08:00
motor_now ( ) - > m_motor_released = false ;
2020-03-16 14:42:44 -07:00
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 .
*/
void mcpwm_foc_set_duty_noramp ( float dutyCycle ) {
// TODO: Actually do this without ramping
mcpwm_foc_set_duty ( dutyCycle ) ;
}
/**
* Use PID rpm control . Note that this value has to be multiplied by half of
* the number of motor poles .
*
* @ param rpm
* The electrical RPM goal value to use .
*/
void mcpwm_foc_set_pid_speed ( float rpm ) {
2020-05-12 06:53:32 -07:00
if ( motor_now ( ) - > m_conf - > s_pid_ramp_erpms_s > 0.0 ) {
if ( motor_now ( ) - > m_control_mode ! = CONTROL_MODE_SPEED | |
motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
motor_now ( ) - > m_speed_pid_set_rpm = mcpwm_foc_get_rpm ( ) ;
}
motor_now ( ) - > m_speed_command_rpm = rpm ;
} else {
motor_now ( ) - > m_speed_pid_set_rpm = rpm ;
}
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_SPEED ;
2015-12-08 12:01:23 -08:00
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
2022-01-19 09:52:45 -08:00
motor_now ( ) - > m_motor_released = false ;
2020-03-16 14:42:44 -07:00
motor_now ( ) - > 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 .
*/
void mcpwm_foc_set_pid_pos ( float pos ) {
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_POS ;
motor_now ( ) - > m_pos_pid_set = pos ;
2015-12-08 12:01:23 -08:00
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
2022-01-19 09:52:45 -08:00
motor_now ( ) - > m_motor_released = false ;
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_state = MC_STATE_RUNNING ;
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 .
*/
void mcpwm_foc_set_current ( float current ) {
2021-04-10 02:37:35 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_CURRENT ;
motor_now ( ) - > m_iq_set = current ;
2021-12-23 12:25:31 -08:00
motor_now ( ) - > m_id_set = 0 ;
2020-03-16 14:42:44 -07:00
if ( fabsf ( current ) < motor_now ( ) - > m_conf - > cc_min_current ) {
2015-12-08 12:01:23 -08:00
return ;
}
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
2022-01-19 09:52:45 -08:00
motor_now ( ) - > m_motor_released = false ;
2020-03-16 14:42:44 -07:00
motor_now ( ) - > 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 ) {
motor_now ( ) - > m_control_mode = CONTROL_MODE_CURRENT ;
motor_now ( ) - > m_iq_set = 0.0 ;
motor_now ( ) - > m_id_set = 0.0 ;
motor_now ( ) - > m_motor_released = true ;
}
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 .
*/
void mcpwm_foc_set_brake_current ( float current ) {
2021-04-10 02:37:35 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_CURRENT_BRAKE ;
motor_now ( ) - > m_iq_set = current ;
2020-03-16 14:42:44 -07:00
if ( fabsf ( current ) < motor_now ( ) - > m_conf - > cc_min_current ) {
2015-12-08 12:01:23 -08:00
return ;
}
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
2022-01-19 09:52:45 -08:00
motor_now ( ) - > m_motor_released = false ;
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_state = MC_STATE_RUNNING ;
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 .
*/
void mcpwm_foc_set_handbrake ( float current ) {
2021-04-10 02:37:35 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_HANDBRAKE ;
motor_now ( ) - > m_iq_set = current ;
2020-03-16 14:42:44 -07:00
if ( fabsf ( current ) < motor_now ( ) - > m_conf - > cc_min_current ) {
2016-11-04 07:18:34 -07:00
return ;
}
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
2022-01-19 09:52:45 -08:00
motor_now ( ) - > m_motor_released = false ;
2020-03-16 14:42:44 -07:00
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 .
*/
void mcpwm_foc_set_openloop ( float current , float rpm ) {
2020-03-16 14:42:44 -07:00
utils_truncate_number ( & current , - motor_now ( ) - > m_conf - > l_current_max * motor_now ( ) - > m_conf - > l_current_max_scale ,
2020-03-28 11:21:45 -07:00
motor_now ( ) - > m_conf - > l_current_max * motor_now ( ) - > m_conf - > l_current_max_scale ) ;
2016-11-04 07:18:34 -07:00
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_OPENLOOP ;
motor_now ( ) - > m_iq_set = current ;
2021-10-13 09:13:51 -07:00
motor_now ( ) - > m_openloop_speed = RPM2RADPS_f ( rpm ) ;
2016-11-04 07:18:34 -07:00
2021-04-10 02:37:35 -07:00
if ( fabsf ( current ) < motor_now ( ) - > m_conf - > cc_min_current ) {
return ;
}
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
2022-01-19 09:52:45 -08:00
motor_now ( ) - > m_motor_released = false ;
2020-03-16 14:42:44 -07:00
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 ]
*/
void mcpwm_foc_set_openloop_phase ( float current , float phase ) {
2020-03-16 14:42:44 -07:00
utils_truncate_number ( & current , - motor_now ( ) - > m_conf - > l_current_max * motor_now ( ) - > m_conf - > l_current_max_scale ,
2020-03-28 11:21:45 -07:00
motor_now ( ) - > m_conf - > l_current_max * motor_now ( ) - > m_conf - > l_current_max_scale ) ;
2019-02-18 10:30:19 -08:00
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_OPENLOOP_PHASE ;
2021-12-23 12:25:31 -08:00
motor_now ( ) - > m_id_set = current ;
motor_now ( ) - > m_iq_set = 0 ;
2019-02-18 10:30:19 -08:00
2021-10-13 09:13:51 -07:00
motor_now ( ) - > m_openloop_phase = DEG2RAD_f ( phase ) ;
2020-03-16 14:42:44 -07:00
utils_norm_angle_rad ( ( float * ) & motor_now ( ) - > m_openloop_phase ) ;
2019-02-18 10:30:19 -08:00
2021-04-10 02:37:35 -07:00
if ( fabsf ( current ) < motor_now ( ) - > m_conf - > cc_min_current ) {
return ;
}
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
2022-01-19 09:52:45 -08:00
motor_now ( ) - > m_motor_released = false ;
2020-03-16 14:42:44 -07:00
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 ) {
motor_now ( ) - > m_conf - > foc_offsets_current [ 0 ] = curr0_offset ;
motor_now ( ) - > m_conf - > foc_offsets_current [ 1 ] = curr1_offset ;
motor_now ( ) - > m_conf - > foc_offsets_current [ 2 ] = curr2_offset ;
}
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
}
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 .
*/
void mcpwm_foc_set_openloop_duty ( float dutyCycle , float rpm ) {
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_OPENLOOP_DUTY ;
motor_now ( ) - > m_duty_cycle_set = dutyCycle ;
2021-10-13 09:13:51 -07:00
motor_now ( ) - > m_openloop_speed = RPM2RADPS_f ( rpm ) ;
2019-03-31 01:49:18 -07:00
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
2022-01-19 09:52:45 -08:00
motor_now ( ) - > m_motor_released = false ;
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_state = MC_STATE_RUNNING ;
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 ]
*/
void mcpwm_foc_set_openloop_duty_phase ( float dutyCycle , float phase ) {
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_control_mode = CONTROL_MODE_OPENLOOP_DUTY_PHASE ;
motor_now ( ) - > m_duty_cycle_set = dutyCycle ;
2021-10-13 09:13:51 -07:00
motor_now ( ) - > m_openloop_phase = DEG2RAD_f ( phase ) ;
2020-03-16 14:42:44 -07:00
utils_norm_angle_rad ( ( float * ) & motor_now ( ) - > m_openloop_phase ) ;
2019-03-31 01:49:18 -07:00
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_state ! = MC_STATE_RUNNING ) {
2022-01-19 09:52:45 -08:00
motor_now ( ) - > m_motor_released = false ;
2020-03-16 14:42:44 -07:00
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 ) {
2020-03-16 14:42:44 -07:00
return motor_now ( ) - > m_duty_cycle_set ;
2015-12-08 12:01:23 -08:00
}
float mcpwm_foc_get_duty_cycle_now ( void ) {
2020-03-16 14:42:44 -07:00
return 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 ) {
2020-03-16 14:42:44 -07:00
return motor_now ( ) - > m_pos_pid_set ;
2016-02-24 12:17:39 -08:00
}
float mcpwm_foc_get_pid_pos_now ( void ) {
2020-03-16 14:42:44 -07:00
return 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-01-06 11:44:00 -08:00
return 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
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_conf - > foc_sample_v0_v7 ) {
2022-01-06 11:44:00 -08:00
return motor_now ( ) - > m_conf - > foc_f_zv ;
2017-09-04 12:12:43 -07:00
} else {
2022-01-06 11:44:00 -08:00
return 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-01-06 11:44:00 -08:00
return 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
2020-03-16 14:42:44 -07:00
if ( motor_now ( ) - > m_conf - > foc_sample_v0_v7 ) {
2022-01-06 11:44:00 -08:00
return ( 1.0 / motor_now ( ) - > m_conf - > foc_f_zv ) ;
2019-04-06 06:36:00 -07:00
} else {
2022-01-06 11:44:00 -08:00
return ( 1.0 / ( motor_now ( ) - > m_conf - > foc_f_zv / 2.0 ) ) ;
2019-04-06 06:36:00 -07:00
}
# else
2022-01-06 11:44:00 -08:00
return ( 1.0 / 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 ) {
2020-03-16 14:42:44 -07:00
return 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 ) {
volatile motor_all_state_t * motor = motor_now ( ) ;
* x1 = motor - > m_observer_x1 ;
* x2 = motor - > m_observer_x2 ;
}
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 ) {
if ( motor_now ( ) - > m_current_off_delay < delay_sec ) {
motor_now ( ) - > m_current_off_delay = delay_sec ;
}
}
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 ) {
2021-10-13 09:13:51 -07:00
return RADPS2RPM_f ( motor_now ( ) - > m_motor_state . speed_rad_s ) ;
2021-10-13 08:26:19 -07:00
// return 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 ) {
2021-10-13 09:13:51 -07:00
return RADPS2RPM_f ( 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 ) {
2021-10-13 09:13:51 -07:00
return RADPS2RPM_f ( 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 ) {
2020-07-21 00:23:48 -07:00
volatile motor_all_state_t * motor = motor_now ( ) ;
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 ) {
2020-07-21 00:23:48 -07:00
volatile motor_all_state_t * motor = motor_now ( ) ;
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 ) {
2020-03-16 14:42:44 -07:00
return 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 ) {
2021-03-24 05:09:05 -07:00
return 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 ) {
2020-03-16 14:42:44 -07:00
const float vd_tmp = motor_now ( ) - > m_motor_state . vd ;
const float vq_tmp = motor_now ( ) - > m_motor_state . vq ;
2017-10-20 11:06:06 -07:00
return sqrtf ( SQ ( vd_tmp ) + SQ ( 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 ) {
2020-03-16 14:42:44 -07:00
return 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 ) {
2020-03-16 14:42:44 -07:00
return 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 ) {
2020-03-16 14:42:44 -07:00
return 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 ) {
2020-03-16 14:42:44 -07:00
return 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 ) {
2020-03-16 14:42:44 -07:00
return motor_now ( ) - > m_motor_state . iq ;
2016-11-04 07:18:34 -07:00
}
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 ) {
2020-03-16 14:42:44 -07:00
return 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 ) {
2020-03-16 14:42:44 -07:00
return 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 ) {
2020-03-16 14:42:44 -07:00
int val = motor_now ( ) - > m_tachometer ;
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 ) {
2020-03-16 14:42:44 -07:00
int val = motor_now ( ) - > m_tachometer ;
2015-12-08 12:01:23 -08:00
if ( reset ) {
2020-03-16 14:42:44 -07:00
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 ) {
2020-03-16 14:42:44 -07:00
int val = motor_now ( ) - > m_tachometer_abs ;
2015-12-08 12:01:23 -08:00
if ( reset ) {
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_tachometer_abs = 0 ;
2015-12-08 12:01:23 -08:00
}
return val ;
}
/**
* Read the motor phase .
*
* @ return
* The phase angle in degrees .
*/
float mcpwm_foc_get_phase ( void ) {
2021-10-13 09:13:51 -07:00
float angle = RAD2DEG_f ( motor_now ( ) - > m_motor_state . phase ) ;
2015-12-08 12:01:23 -08:00
utils_norm_angle ( & angle ) ;
return angle ;
}
/**
* Read the phase that the observer has calculated .
*
* @ return
* The phase angle in degrees .
*/
float mcpwm_foc_get_phase_observer ( void ) {
2021-10-13 09:13:51 -07:00
float angle = RAD2DEG_f ( motor_now ( ) - > m_phase_now_observer ) ;
2015-12-08 12:01:23 -08:00
utils_norm_angle ( & angle ) ;
return angle ;
}
/**
* Read the phase from based on the encoder .
*
* @ return
* The phase angle in degrees .
*/
float mcpwm_foc_get_phase_encoder ( void ) {
2021-10-13 09:13:51 -07:00
float angle = RAD2DEG_f ( motor_now ( ) - > m_phase_now_encoder ) ;
2015-12-08 12:01:23 -08:00
utils_norm_angle ( & angle ) ;
return angle ;
}
2015-12-19 12:24:46 -08:00
float mcpwm_foc_get_vd ( void ) {
2020-03-16 14:42:44 -07:00
return motor_now ( ) - > m_motor_state . vd ;
2015-12-19 12:24:46 -08:00
}
float mcpwm_foc_get_vq ( void ) {
2020-03-16 14:42:44 -07:00
return 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 ) {
return motor_now ( ) - > m_motor_state . mod_alpha_raw ;
}
float mcpwm_foc_get_mod_beta_raw ( void ) {
return motor_now ( ) - > m_motor_state . mod_beta_raw ;
}
float mcpwm_foc_get_mod_alpha_measured ( void ) {
return motor_now ( ) - > m_motor_state . mod_alpha_measured ;
}
float mcpwm_foc_get_mod_beta_measured ( void ) {
return motor_now ( ) - > m_motor_state . mod_beta_measured ;
}
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 .
2015-12-08 12:01:23 -08:00
*/
2015-12-27 16:50:23 -08:00
void mcpwm_foc_encoder_detect ( float current , bool print , float * offset , float * ratio , bool * inverted ) {
2016-02-02 08:19:37 -08:00
mc_interface_lock ( ) ;
2020-03-16 14:42:44 -07:00
volatile motor_all_state_t * motor = 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 ;
2015-12-08 12:01:23 -08:00
chThdSleepMilliseconds ( 1 ) ;
}
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 ;
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 ;
j + = ( 2.0 * M_PI ) / 500.0 ) {
2020-03-16 10:32:39 -07:00
motor - > m_phase_now_override = j ;
2016-04-27 06:32:32 -07:00
chThdSleepMilliseconds ( 1 ) ;
}
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 ;
2019-01-28 11:52:02 -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 ;
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 ;
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 ) ;
2019-02-18 10:30:19 -08:00
chThdSleepMilliseconds ( 4 ) ;
}
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 ) ;
2019-02-18 10:30:19 -08:00
chThdSleepMilliseconds ( 4 ) ;
}
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
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 ;
stop_pwm_hw ( 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 ( ) ;
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 .
*
2015-12-08 12:01:23 -08:00
* @ return
* The calculated motor resistance .
*/
2020-04-02 11:43:11 -07:00
float mcpwm_foc_measure_resistance ( float current , int samples , bool stop_after ) {
2015-12-08 12:01:23 -08:00
mc_interface_lock ( ) ;
2020-03-16 14:42:44 -07:00
volatile motor_all_state_t * motor = motor_now ( ) ;
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 ) {
2020-04-03 15:58:42 -07:00
utils_step_towards ( ( float * ) & motor - > m_iq_set , current , fabsf ( current ) / 500.0 ) ;
chThdSleepMilliseconds ( 1 ) ;
}
2015-12-08 12:01:23 -08:00
// Wait for the current to rise and the motor to lock.
2020-04-01 17:58:37 -07:00
chThdSleepMilliseconds ( 100 ) ;
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 ;
}
2019-02-18 10:30:19 -08:00
if ( mc_interface_get_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 ;
stop_pwm_hw ( 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 ( ) ;
return 0.0 ;
}
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 ;
stop_pwm_hw ( motor ) ;
}
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 ( ) ;
2021-10-15 13:25:07 -07:00
return voltage_avg / current_avg ;
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 .
*
* @ return
2020-01-28 10:46:19 -08:00
* The average d and q axis inductance in uH .
2015-12-08 12:01:23 -08:00
*/
2020-01-28 10:46:19 -08:00
float mcpwm_foc_measure_inductance ( float duty , int samples , float * curr , float * ld_lq_diff ) {
2020-03-16 14:42:44 -07:00
volatile motor_all_state_t * motor = motor_now ( ) ;
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 ;
2020-03-16 10:32:39 -07:00
bool sample_v0_v7_old = motor - > m_conf - > foc_sample_v0_v7 ;
foc_hfi_samples samples_old = motor - > m_conf - > foc_hfi_samples ;
2020-04-16 02:40:56 -07:00
bool sample_high_current_old = motor - > m_conf - > foc_sample_high_current ;
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 ;
stop_pwm_hw ( motor ) ;
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 ;
2020-03-16 10:32:39 -07:00
motor - > m_conf - > foc_sample_v0_v7 = false ;
motor - > m_conf - > foc_hfi_samples = HFI_SAMPLES_32 ;
2020-04-16 02:40:56 -07:00
motor - > m_conf - > foc_sample_high_current = false ;
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
2020-01-28 10:46:19 -08:00
for ( int i = 0 ; i < ( samples / 10 ) ; i + + ) {
2020-04-01 17:58:37 -07:00
if ( mc_interface_get_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 ) ;
2020-04-02 07:26:59 -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-04-02 07:26:59 -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 ;
2020-04-02 07:26:59 -07:00
motor - > m_conf - > foc_sample_v0_v7 = sample_v0_v7_old ;
motor - > m_conf - > foc_hfi_samples = samples_old ;
2020-04-16 02:40:56 -07:00
motor - > m_conf - > foc_sample_high_current = sample_high_current_old ;
2020-04-02 07:26:59 -07:00
2021-10-15 13:25:07 -07:00
mcpwm_foc_set_configuration ( motor - > m_conf ) ;
2020-04-02 07:26:59 -07:00
2020-04-01 17:58:37 -07:00
mc_interface_unlock ( ) ;
2020-04-02 07:26:59 -07:00
2020-04-01 17:58:37 -07:00
return 0.0 ;
}
2020-04-02 07:26:59 -07:00
2020-01-28 10:46:19 -08:00
timeout_reset ( ) ;
mcpwm_foc_set_duty ( 0.0 ) ;
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
ld_lq_diff_sum + = 4.0 * sqrtf ( SQ ( real_bin2 ) + SQ ( imag_bin2 ) ) ;
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 ;
2020-03-16 10:32:39 -07:00
motor - > m_conf - > foc_sample_v0_v7 = sample_v0_v7_old ;
motor - > m_conf - > foc_hfi_samples = samples_old ;
2020-04-16 02:40:56 -07:00
motor - > m_conf - > foc_sample_high_current = sample_high_current_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.
float ind_scale_factor = 0.8 ;
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
}
2021-10-31 09:47:09 -07:00
return ( l_sum / iterations ) * 1e6 * ind_scale_factor ;
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 .
*
* @ return
2020-01-28 10:46:19 -08:00
* The average d and q axis inductance in uH .
2019-02-18 10:30:19 -08:00
*/
2020-01-28 10:46:19 -08:00
float mcpwm_foc_measure_inductance_current ( float curr_goal , int samples , float * curr , float * ld_lq_diff ) {
2019-02-18 10:30:19 -08:00
float duty_last = 0.0 ;
for ( float i = 0.02 ; i < 0.5 ; i * = 1.5 ) {
float i_tmp ;
2020-01-28 10:46:19 -08:00
mcpwm_foc_measure_inductance ( i , 10 , & i_tmp , 0 ) ;
2019-02-18 10:30:19 -08:00
duty_last = i ;
if ( i_tmp > = curr_goal ) {
break ;
}
}
2020-01-28 10:46:19 -08:00
float ind = mcpwm_foc_measure_inductance ( duty_last , samples , curr , ld_lq_diff ) ;
2019-02-18 10:30:19 -08:00
return ind ;
}
2015-12-08 12:01:23 -08:00
/**
* Automatically measure the resistance and inductance of the motor with small steps .
*
* @ param res
* The measured resistance in ohm .
*
* @ param ind
* The measured inductance in microhenry .
*
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
* True if the measurement succeeded , false otherwise .
*/
2021-11-21 09:17:40 -08:00
bool mcpwm_foc_measure_res_ind ( float * res , float * ind , float * ld_lq_diff ) {
2020-03-16 14:42:44 -07:00
volatile motor_all_state_t * motor = motor_now ( ) ;
2020-03-16 10:32:39 -07:00
2022-01-06 11:44:00 -08:00
const float f_zv_old = motor - > m_conf - > foc_f_zv ;
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 ) {
2020-04-02 11:43:11 -07:00
if ( i > ( 1.0 / mcpwm_foc_measure_resistance ( i , 20 , false ) ) ) {
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
2020-04-02 11:43:11 -07:00
* res = mcpwm_foc_measure_resistance ( i_last , 200 , true ) ;
2020-03-16 10:32:39 -07:00
motor - > m_conf - > foc_motor_r = * res ;
2021-11-21 09:17:40 -08:00
* ind = mcpwm_foc_measure_inductance_current ( i_last , 200 , 0 , ld_lq_diff ) ;
2015-12-08 12:01:23 -08:00
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_current_kp = kp_old ;
motor - > m_conf - > foc_current_ki = ki_old ;
motor - > m_conf - > foc_motor_r = res_old ;
2015-12-08 12:01:23 -08:00
return true ;
}
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 .
*
* @ return
* true : Success
* false : Something went wrong
*/
bool mcpwm_foc_hall_detect ( float current , uint8_t * hall_table ) {
2020-03-16 14:42:44 -07:00
volatile motor_all_state_t * motor = motor_now ( ) ;
2020-03-16 10:32:39 -07:00
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
for ( int i = 0 ; i < 1000 ; i + + ) {
motor - > m_id_set = ( float ) i * current / 1000.0 ;
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 ) ;
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 ) ;
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 ] + + ;
}
}
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 ;
stop_pwm_hw ( motor ) ;
2016-01-27 14:57:23 -08:00
2021-10-24 09:10:00 -07:00
motor - > m_conf - > foc_mtpa_mode = mtpa_old ;
2020-07-21 11:07:46 -07:00
2016-01-27 14:57:23 -08:00
// Enable timeout
2021-06-13 04:46:27 -07:00
timeout_configure ( tout , tout_c , tout_ksw ) ;
2016-01-27 14:57:23 -08:00
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 + + ;
}
}
2016-02-02 08:19:37 -08:00
mc_interface_unlock ( ) ;
2016-01-27 14:57:23 -08:00
return fails = = 2 ;
}
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
*/
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
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
stop_pwm_hw ( & m_motor_1 ) ;
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
stop_pwm_hw ( & m_motor_2 ) ;
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
stop_pwm_hw ( & m_motor_1 ) ;
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
stop_pwm_hw ( & m_motor_2 ) ;
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
2021-02-28 11:36:02 -08:00
stop_pwm_hw ( & 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
stop_pwm_hw ( & 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 ) ;
}
stop_pwm_hw ( & 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
2021-03-22 04:13:19 -07:00
stop_pwm_hw ( & m_motor_2 ) ;
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
2021-03-22 04:13:19 -07:00
stop_pwm_hw ( & 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
2021-03-22 04:13:19 -07:00
stop_pwm_hw ( & m_motor_2 ) ;
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
}
2015-12-08 12:01:23 -08:00
void mcpwm_foc_print_state ( void ) {
2021-11-15 12:29:05 -08:00
commands_printf ( " Mod d: %.2f " , ( double ) motor_now ( ) - > m_motor_state . mod_d ) ;
commands_printf ( " Mod q: %.2f " , ( double ) motor_now ( ) - > m_motor_state . mod_q ) ;
commands_printf ( " Mod q flt: %.2f " , ( double ) motor_now ( ) - > m_motor_state . mod_q_filter ) ;
commands_printf ( " Duty: %.2f " , ( double ) motor_now ( ) - > m_motor_state . duty_now ) ;
commands_printf ( " Vd: %.2f " , ( double ) motor_now ( ) - > m_motor_state . vd ) ;
commands_printf ( " Vq: %.2f " , ( double ) motor_now ( ) - > m_motor_state . vq ) ;
commands_printf ( " Phase: %.2f " , ( double ) motor_now ( ) - > m_motor_state . phase ) ;
commands_printf ( " V_alpha: %.2f " , ( double ) motor_now ( ) - > m_motor_state . v_alpha ) ;
commands_printf ( " V_beta: %.2f " , ( double ) motor_now ( ) - > m_motor_state . v_beta ) ;
commands_printf ( " id: %.2f " , ( double ) motor_now ( ) - > m_motor_state . id ) ;
commands_printf ( " iq: %.2f " , ( double ) motor_now ( ) - > m_motor_state . iq ) ;
commands_printf ( " id_filter: %.2f " , ( double ) motor_now ( ) - > m_motor_state . id_filter ) ;
commands_printf ( " iq_filter: %.2f " , ( double ) motor_now ( ) - > m_motor_state . iq_filter ) ;
commands_printf ( " id_target: %.2f " , ( double ) motor_now ( ) - > m_motor_state . id_target ) ;
commands_printf ( " iq_target: %.2f " , ( double ) motor_now ( ) - > m_motor_state . iq_target ) ;
commands_printf ( " i_abs: %.2f " , ( double ) motor_now ( ) - > m_motor_state . i_abs ) ;
commands_printf ( " i_abs_flt: %.2f " , ( double ) motor_now ( ) - > m_motor_state . i_abs_filter ) ;
commands_printf ( " Obs_x1: %.2f " , ( double ) motor_now ( ) - > m_observer_x1 ) ;
commands_printf ( " Obs_x2: %.2f " , ( double ) motor_now ( ) - > m_observer_x2 ) ;
commands_printf ( " vd_int: %.2f " , ( double ) motor_now ( ) - > m_motor_state . vd_int ) ;
commands_printf ( " vq_int: %.2f " , ( double ) motor_now ( ) - > m_motor_state . vq_int ) ;
commands_printf ( " off_delay: %.2f " , ( double ) 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 ;
2020-03-21 03:40:50 -07:00
volatile motor_all_state_t * motor_now = is_second_motor ? & m_motor_2 : & m_motor_1 ;
2020-03-16 10:32:39 -07:00
volatile motor_all_state_t * motor_other = is_second_motor ? & m_motor_1 : & 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
volatile motor_all_state_t * motor_other = & m_motor_1 ;
2020-03-21 03:40:50 -07:00
volatile motor_all_state_t * motor_now = & 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
2020-03-21 03:40:50 -07:00
volatile mc_configuration * conf_now = motor_now - > m_conf ;
2015-12-23 15:43:31 -08:00
2020-03-16 10:32:39 -07:00
if ( motor_other - > m_duty_next_set ) {
motor_other - > m_duty_next_set = false ;
# ifdef HW_HAS_DUAL_MOTORS
if ( is_second_motor ) {
TIMER_UPDATE_DUTY_M1 ( motor_other - > m_duty1_next , motor_other - > m_duty2_next , motor_other - > m_duty3_next ) ;
} else {
TIMER_UPDATE_DUTY_M2 ( motor_other - > m_duty1_next , motor_other - > m_duty2_next , motor_other - > m_duty3_next ) ;
}
# else
2020-03-21 03:40:50 -07:00
TIMER_UPDATE_DUTY_M1 ( motor_now - > m_duty1_next , motor_now - > m_duty2_next , motor_now - > m_duty3_next ) ;
2020-03-28 11:21:45 -07:00
# ifdef HW_HAS_DUAL_PARALLEL
TIMER_UPDATE_DUTY_M2 ( motor_now - > m_duty1_next , motor_now - > m_duty2_next , motor_now - > m_duty3_next ) ;
# endif
2020-03-16 10:32:39 -07:00
# endif
2020-01-20 00:39:33 -08:00
}
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
2020-03-21 03:40:50 -07:00
if ( ! conf_now - > foc_sample_v0_v7 & & is_v7 ) {
2020-01-28 10:46:19 -08:00
return ;
}
2017-09-04 12:12:43 -07:00
# else
2020-01-28 10:46:19 -08:00
if ( is_v7 ) {
return ;
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
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
2020-03-21 03:40:50 -07:00
if ( conf_now - > foc_sample_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
}
} else {
# 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 ;
2020-03-29 08:00:43 -07:00
// float ic = -(ia + ib);
2015-12-08 12:01:23 -08:00
2017-09-04 12:12:43 -07:00
# ifdef HW_HAS_PHASE_SHUNTS
float dt ;
2020-03-21 03:40:50 -07:00
if ( conf_now - > foc_sample_v0_v7 ) {
2022-01-06 11:44:00 -08:00
dt = 1.0 / conf_now - > foc_f_zv ;
2017-09-04 12:12:43 -07:00
} else {
2022-01-06 11:44:00 -08:00
dt = 1.0 / ( conf_now - > foc_f_zv / 2.0 ) ;
2017-09-04 12:12:43 -07:00
}
2016-11-04 07:18:34 -07:00
# else
2022-01-06 11:44:00 -08:00
float dt = 1.0 / ( conf_now - > foc_f_zv / 2.0 ) ;
2016-11-04 07:18:34 -07:00
# endif
2019-02-19 09:55:18 -08:00
// This has to be done for the skip function to have any chance at working with the
// observer and control loops.
// TODO: Test this.
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 ) {
2015-12-08 12:01:23 -08:00
// Clarke transform assuming balanced currents
2020-03-21 03:40:50 -07:00
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
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 {
motor_now - > m_br_no_duty_samples = 0 ;
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 ) {
motor_now - > m_motor_state . vq_int - = motor_now - > m_motor_state . speed_rad_s * 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
2021-11-27 14:32:08 -08:00
if ( ! control_duty ) {
motor_now - > m_duty_i_term = motor_now - > m_motor_state . iq / conf_now - > lo_current_max ;
}
2015-12-08 12:01:23 -08:00
if ( control_duty ) {
// Duty cycle control
if ( fabsf ( duty_set ) < ( duty_abs - 0.05 ) | |
2020-03-21 03:40:50 -07:00
( SIGN ( motor_now - > m_motor_state . vq ) * motor_now - > m_motor_state . iq ) < conf_now - > lo_current_min ) {
2015-12-08 12:01:23 -08:00
// Truncating the duty cycle here would be dangerous, so run a PID controller.
// Compensation for supply voltage variations
2021-04-27 08:48:15 -07:00
float scale = 1.0 / motor_now - > m_motor_state . v_bus ;
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
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 ) ;
2020-03-21 03:40:50 -07:00
iq_set_tmp = output * conf_now - > lo_current_max ;
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.
2021-11-27 14:32:08 -08:00
motor_now - > m_duty_i_term = motor_now - > m_motor_state . iq / conf_now - > lo_current_max ;
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 ) {
2020-03-21 03:40:50 -07:00
iq_set_tmp = conf_now - > lo_current_max ;
2015-12-08 12:01:23 -08:00
} else {
2020-03-21 03:40:50 -07:00
iq_set_tmp = - conf_now - > lo_current_max ;
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 ) {
2021-02-28 11:36:02 -08:00
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 , dt ,
& motor_now - > m_observer_x1 , & motor_now - > m_observer_x2 , & 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 ( ) ) {
2021-02-28 11:36:02 -08:00
motor_now - > m_motor_state . phase = correct_encoder (
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 :
motor_now - > m_phase_now_observer = correct_hall ( motor_now - > m_phase_now_observer , dt , motor_now ) ;
2020-03-21 03:40:50 -07:00
motor_now - > m_motor_state . phase = motor_now - > m_phase_now_observer ;
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 ;
2021-03-13 06:41:57 -08:00
motor_now - > m_observer_x1 = motor_now - > m_observer_x1_override ;
motor_now - > m_observer_x2 = motor_now - > m_observer_x2_override ;
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 :
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 ;
}
2020-01-20 00:39:33 -08:00
2021-02-28 11:36:02 -08:00
motor_now - > m_motor_state . phase = correct_encoder (
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
}
id_set_tmp = ( lambda - sqrtf ( SQ ( lambda ) + 8.0 * SQ ( ld_lq_diff ) * SQ ( 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
2017-09-04 12:12:43 -07:00
// TODO: Consider D axis current for the input current as well.
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 ;
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
2020-03-21 03:40:50 -07:00
observer_update ( motor_now - > m_motor_state . v_alpha , motor_now - > m_motor_state . v_beta ,
2020-03-29 08:00:43 -07:00
motor_now - > m_motor_state . i_alpha , motor_now - > m_motor_state . i_beta , dt ,
& motor_now - > m_observer_x1 , & motor_now - > m_observer_x2 , 0 , motor_now ) ;
2020-03-21 03:40:50 -07:00
motor_now - > m_phase_now_observer = utils_fast_atan2 ( motor_now - > m_x2_prev + motor_now - > m_observer_x2 ,
2020-03-28 11:21:45 -07:00
motor_now - > m_x1_prev + motor_now - > m_observer_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
2020-03-21 03:40:50 -07:00
motor_now - > m_x1_prev = motor_now - > m_observer_x1 ;
motor_now - > m_x2_prev = motor_now - > m_observer_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 :
motor_now - > m_motor_state . phase = correct_encoder (
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 :
motor_now - > m_phase_now_observer = correct_hall ( motor_now - > m_phase_now_observer , dt , motor_now ) ;
motor_now - > m_motor_state . phase = motor_now - > m_phase_now_observer ;
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 :
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
CURRENT_FILTER_ON ( ) ;
2020-03-21 03:40:50 -07:00
motor_now - > m_hfi . ind = 0 ;
motor_now - > m_hfi . ready = false ;
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 ) {
motor_now - > m_motor_state . vq_int - = motor_now - > m_motor_state . speed_rad_s * 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 ) *
2021-12-29 09:21:42 -08:00
sqrtf ( SQ ( motor_now - > m_motor_state . mod_d ) + SQ ( motor_now - > m_motor_state . mod_q ) ) / SQRT3_BY_2 ;
2015-12-08 12:01:23 -08:00
// Run PLL for speed estimation
2020-03-21 03:40:50 -07:00
pll_run ( motor_now - > m_motor_state . phase , dt , & motor_now - > m_pll_phase , & motor_now - > m_pll_speed , conf_now ) ;
motor_now - > m_motor_state . speed_rad_s = motor_now - > m_pll_speed ;
2015-12-08 12:01:23 -08:00
2020-01-28 10:46:19 -08:00
// Low latency speed estimation, for e.g. HFI.
{
2020-03-21 03:40:50 -07:00
float diff = utils_angle_difference_rad ( motor_now - > m_motor_state . phase , 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 ) ;
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 ) ;
2020-03-21 03:40:50 -07:00
motor_now - > m_phase_before_speed_est = 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
2021-04-11 02:31:25 -07:00
static void run_fw ( volatile motor_all_state_t * motor , float dt ) {
2022-01-19 09:52:45 -08:00
if ( motor - > m_conf - > foc_fw_current_max < fmaxf ( motor - > m_conf - > cc_min_current , 0.001 ) ) {
2021-04-11 02:31:25 -07:00
return ;
}
2021-04-10 02:37:35 -07:00
// Field Weakening
// FW is used in the current and speed control modes. If a different mode is used
// this code also runs if field weakening was active before. This allows
// changing control mode even while in field weakening.
2021-04-11 02:31:25 -07:00
if ( motor - > m_state = = MC_STATE_RUNNING & &
2021-04-10 02:37:35 -07:00
( motor - > m_control_mode = = CONTROL_MODE_CURRENT | |
motor - > m_control_mode = = CONTROL_MODE_CURRENT_BRAKE | |
motor - > m_control_mode = = CONTROL_MODE_SPEED | |
motor - > m_i_fw_set > motor - > m_conf - > cc_min_current ) ) {
float fw_current_now = 0.0 ;
2021-08-16 11:33:26 -07:00
float duty_abs = motor - > m_duty_abs_filtered ;
2021-04-10 02:37:35 -07:00
if ( motor - > m_conf - > foc_fw_duty_start < 0.99 & &
duty_abs > motor - > m_conf - > foc_fw_duty_start * motor - > m_conf - > l_max_duty ) {
fw_current_now = utils_map ( duty_abs ,
motor - > m_conf - > foc_fw_duty_start * motor - > m_conf - > l_max_duty ,
motor - > m_conf - > l_max_duty ,
0.0 , motor - > m_conf - > foc_fw_current_max ) ;
2021-04-11 02:31:25 -07:00
2021-04-14 13:29:50 -07:00
// m_current_off_delay is used to not stop the modulation too soon after leaving FW. If axis decoupling
2021-04-11 02:31:25 -07:00
// is not working properly an oscillation can occur on the modulation when changing the current
// fast, which can make the estimated duty cycle drop below the FW threshold long enough to stop
// modulation. When that happens the body diodes in the MOSFETs can see a lot of current and unexpected
// braking happens. Therefore the modulation is left on for some time after leaving FW to give the
// oscillation a chance to decay while the MOSFETs are still driven.
2021-04-14 13:29:50 -07:00
motor - > m_current_off_delay = 1.0 ;
2021-04-10 02:37:35 -07:00
}
if ( motor - > m_conf - > foc_fw_ramp_time < dt ) {
motor - > m_i_fw_set = fw_current_now ;
} else {
utils_step_towards ( ( float * ) & motor - > m_i_fw_set , fw_current_now ,
( dt / motor - > m_conf - > foc_fw_ramp_time ) * motor - > m_conf - > foc_fw_current_max ) ;
}
}
2021-04-11 02:31:25 -07:00
}
static void timer_update ( volatile motor_all_state_t * motor , float dt ) {
run_fw ( motor , dt ) ;
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
float min_current = motor - > m_conf - > cc_min_current ;
if ( min_current < 0.001 & & motor_now ( ) - > m_motor_released ) {
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 ) ;
2021-03-13 02:42:23 -08:00
float mag = sqrtf ( SQ ( m_motor_1 . m_observer_x1 ) + SQ ( 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
2021-04-10 02:37:35 -07:00
float t_lock = motor - > m_conf - > foc_sl_openloop_time_lock ;
float t_ramp = motor - > m_conf - > foc_sl_openloop_time_ramp ;
float t_const = motor - > m_conf - > foc_sl_openloop_time ;
2020-10-09 12:08:48 -07:00
float openloop_rpm_max = utils_map ( fabsf ( motor - > m_motor_state . iq_filter ) ,
0.0 , motor - > m_conf - > l_current_max ,
motor - > m_conf - > foc_openloop_rpm_low * motor - > m_conf - > foc_openloop_rpm ,
motor - > m_conf - > foc_openloop_rpm ) ;
utils_truncate_number_abs ( & openloop_rpm_max , motor - > m_conf - > 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 ;
if ( motor - > m_conf - > foc_sensor_mode ! = FOC_SENSOR_MODE_ENCODER ) {
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 ) & &
2020-10-09 12:08:48 -07:00
motor - > m_min_rpm_hyst_timer < motor - > m_conf - > 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 ;
2020-10-09 12:08:48 -07:00
if ( motor - > m_min_rpm_hyst_timer > = motor - > m_conf - > foc_sl_openloop_hyst & &
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 ) ;
2021-03-13 06:41:57 -08:00
motor - > m_observer_x1_override = c * motor - > m_conf - > foc_motor_flux_linkage ;
motor - > m_observer_x2_override = s * motor - > m_conf - > 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
2020-03-16 10:32:39 -07:00
motor - > m_samples . avg_current_tot + = sqrtf ( SQ ( id_tmp ) + SQ ( iq_tmp ) ) ;
motor - > m_samples . avg_voltage_tot + = sqrtf ( SQ ( vd_tmp ) + SQ ( vq_tmp ) ) ;
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 ,
2021-02-28 11:36:02 -08:00
0 , motor - > m_conf - > foc_observer_gain ) ;
2020-03-20 09:35:25 -07:00
if ( gamma_tmp < ( motor - > m_conf - > foc_observer_gain_slow * motor - > m_conf - > foc_observer_gain ) ) {
gamma_tmp = motor - > m_conf - > foc_observer_gain_slow * motor - > m_conf - > foc_observer_gain ;
}
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 ) ;
motor - > m_r_est = motor - > m_r_est_state - 0.5 * res_est_gain * motor - > m_conf - > foc_motor_l * i_abs_sq ;
float res_dot = - res_est_gain * ( motor - > m_r_est * i_abs_sq + motor - > m_speed_est_fast *
( motor - > m_motor_state . i_beta * motor - > m_observer_x1 - motor - > m_motor_state . i_alpha * motor - > m_observer_x2 ) -
( 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 ;
utils_truncate_number ( ( float * ) & motor - > m_r_est_state , motor - > m_conf - > foc_motor_r * 0.25 , motor - > m_conf - > foc_motor_r * 3.0 ) ;
}
}
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 " ) ;
}
for ( int i = 0 ; i < top ; i + + ) {
float res_est = m_motor_1 . m_r_est ;
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 ) ;
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
}
2020-03-16 10:32:39 -07:00
timer_update ( & m_motor_1 , dt ) ;
# ifdef HW_HAS_DUAL_MOTORS
timer_update ( & m_motor_2 , dt ) ;
# endif
2020-02-16 06:07:08 -08:00
input_current_offset_measurement ( ) ;
2015-12-08 12:01:23 -08:00
chThdSleepMilliseconds ( 1 ) ;
}
}
2020-03-16 10:32:39 -07:00
static void hfi_update ( volatile motor_all_state_t * motor ) {
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 ;
}
2020-01-20 00:39:33 -08:00
2020-03-16 10:32:39 -07:00
if ( motor - > m_hfi . ready ) {
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
2020-03-16 10:32:39 -07:00
float mag_bin_1 = sqrtf ( SQ ( imag_bin1 ) + SQ ( real_bin1 ) ) ;
float angle_bin_1 = - utils_fast_atan2 ( imag_bin1 , real_bin1 ) ;
2020-01-20 00:39:33 -08:00
2020-03-16 10:32:39 -07:00
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
2020-03-16 10:32:39 -07:00
float mag_bin_2 = sqrtf ( SQ ( imag_bin2 ) + SQ ( real_bin2 ) ) ;
float angle_bin_2 = - utils_fast_atan2 ( imag_bin2 , real_bin2 ) / 2.0 ;
2020-01-20 00:39:33 -08:00
2020-03-16 10:32:39 -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 ;
if ( motor - > m_conf - > foc_sample_v0_v7 ) {
2022-01-06 11:44:00 -08:00
dt_sw = 1.0 / motor - > m_conf - > foc_f_zv ;
2020-03-16 10:32:39 -07:00
} else {
2022-01-06 11:44:00 -08:00
dt_sw = 1.0 / ( motor - > m_conf - > foc_f_zv / 2.0 ) ;
2020-03-16 10:32:39 -07:00
}
angle_bin_2 + = motor - > m_motor_state . speed_rad_s * ( ( float ) motor - > m_hfi . samples / 2.0 ) * dt_sw ;
2020-01-20 00:39:33 -08:00
2020-03-16 10:32:39 -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 ) ) ) {
angle_bin_2 + = M_PI ;
}
2020-01-20 00:39:33 -08:00
2020-03-16 10:32:39 -07:00
if ( motor - > m_hfi . est_done_cnt < motor - > m_conf - > foc_hfi_start_samples ) {
motor - > m_hfi . est_done_cnt + + ;
2020-01-20 00:39:33 -08:00
2020-03-16 10:32:39 -07:00
if ( fabsf ( utils_angle_difference_rad ( angle_bin_2 , angle_bin_1 ) ) > ( M_PI / 2.0 ) ) {
motor - > m_hfi . flip_cnt + + ;
2020-01-20 00:39:33 -08:00
}
2021-03-13 02:42:23 -08:00
}
if ( motor - > m_hfi . est_done_cnt > = motor - > m_conf - > foc_hfi_start_samples ) {
2020-03-16 10:32:39 -07:00
if ( motor - > m_hfi . flip_cnt > = ( motor - > m_conf - > foc_hfi_start_samples / 2 ) ) {
2020-01-20 00:39:33 -08:00
angle_bin_2 + = M_PI ;
}
2020-03-16 10:32:39 -07:00
motor - > m_hfi . flip_cnt = 0 ;
2021-03-13 02:42:23 -08:00
if ( motor - > m_conf - > foc_sensor_mode = = FOC_SENSOR_MODE_HFI_START ) {
float s , c ;
utils_norm_angle_rad ( & angle_bin_2 ) ;
utils_fast_sincos_better ( angle_bin_2 , & 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 ;
}
2020-03-16 10:32:39 -07:00
}
2020-01-20 00:39:33 -08:00
2020-03-16 10:32:39 -07:00
motor - > m_hfi . angle = angle_bin_2 ;
utils_norm_angle_rad ( ( float * ) & motor - > m_hfi . angle ) ;
2020-01-28 10:46:19 -08:00
2020-03-16 10:32:39 -07:00
// 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
2020-03-16 10:32:39 -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
2020-03-16 10:32:39 -07:00
if ( hfi_plot_div > = 8 ) {
hfi_plot_div = 0 ;
2020-01-28 10:46:19 -08:00
2020-03-16 10:32:39 -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
2020-03-16 10:32:39 -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
2020-03-16 10:32:39 -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
2020-03-16 10:32:39 -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
2020-03-16 10:32:39 -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
2020-03-16 10:32:39 -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);
// commands_send_plot_points(motor->m_hfi_plot_sample, motor->m_motor_state.speed_rad_s);
//
// 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
2020-03-16 10:32:39 -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
2020-03-16 10:32:39 -07:00
if ( hfi_plot_div > = 8 ) {
hfi_plot_div = 0 ;
2020-01-28 10:46:19 -08:00
2020-03-16 10:32:39 -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
2020-03-16 10:32:39 -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
2020-03-16 10:32:39 -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
2020-03-16 10:32:39 -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 ;
}
}
static THD_FUNCTION ( hfi_thread , arg ) {
( void ) arg ;
chRegSetThreadName ( " foc hfi " ) ;
for ( ; ; ) {
if ( hfi_thd_stop ) {
hfi_thd_stop = false ;
return ;
2020-01-20 00:39:33 -08:00
}
2020-03-16 10:32:39 -07:00
hfi_update ( & m_motor_1 ) ;
# ifdef HW_HAS_DUAL_MOTORS
hfi_update ( & m_motor_2 ) ;
# 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 ( ) ;
run_pid_control_pos ( dt , & m_motor_1 ) ;
run_pid_control_speed ( dt , & m_motor_1 ) ;
# ifdef HW_HAS_DUAL_MOTORS
run_pid_control_pos ( dt , & m_motor_2 ) ;
run_pid_control_speed ( dt , & m_motor_2 ) ;
# endif
}
}
2015-12-08 12:01:23 -08:00
// See http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
void observer_update ( float v_alpha , float v_beta , float i_alpha , float i_beta ,
2020-03-28 11:21:45 -07:00
float dt , volatile float * x1 , volatile float * x2 , volatile float * phase , volatile motor_all_state_t * motor ) {
2015-12-08 12:01:23 -08:00
2020-03-21 03:40:50 -07:00
volatile mc_configuration * conf_now = motor - > m_conf ;
2021-10-15 13:25:07 -07:00
float R = conf_now - > foc_motor_r ;
2021-10-24 04:25:12 -07:00
float L = conf_now - > foc_motor_l ;
float lambda = conf_now - > foc_motor_flux_linkage ;
2015-12-08 12:01:23 -08:00
2017-09-04 12:12:43 -07:00
// Saturation compensation
2021-10-24 04:25:12 -07:00
const float comp_fact = conf_now - > foc_sat_comp * ( motor - > m_motor_state . i_abs_filter / conf_now - > l_current_max ) ;
L - = L * comp_fact ;
lambda - = lambda * comp_fact ;
2015-12-08 12:01:23 -08:00
2017-09-04 12:12:43 -07:00
// Temperature compensation
const float t = mc_interface_temp_motor_filtered ( ) ;
2021-11-24 03:18:29 -08:00
if ( conf_now - > foc_temp_comp & & t > - 30.0 ) {
2020-03-21 03:40:50 -07:00
R + = R * 0.00386 * ( t - conf_now - > foc_temp_comp_base_temp ) ;
2017-09-04 12:12:43 -07:00
}
2015-12-08 12:01:23 -08:00
2021-10-24 04:25:12 -07:00
float ld_lq_diff = conf_now - > foc_motor_ld_lq_diff ;
float id = motor - > m_motor_state . id ;
float iq = motor - > m_motor_state . iq ;
// Adjust inductance for saliency.
if ( fabsf ( id ) > 0.1 | | fabsf ( iq ) > 0.1 ) {
L = L - ld_lq_diff / 2.0 + ld_lq_diff * SQ ( iq ) / ( SQ ( id ) + SQ ( iq ) ) ;
}
2017-09-04 12:12:43 -07:00
const float L_ia = L * i_alpha ;
const float L_ib = L * i_beta ;
const float R_ia = R * i_alpha ;
const float R_ib = R * i_beta ;
2021-10-24 04:25:12 -07:00
const float lambda_2 = SQ ( lambda ) ;
2020-03-16 10:32:39 -07:00
const float gamma_half = motor - > m_gamma_now * 0.5 ;
2020-03-21 03:40:50 -07:00
switch ( conf_now - > foc_observer_type ) {
2020-03-28 11:21:45 -07:00
case FOC_OBSERVER_ORTEGA_ORIGINAL : {
float err = lambda_2 - ( SQ ( * x1 - L_ia ) + SQ ( * x2 - L_ib ) ) ;
2021-02-28 11:36:02 -08:00
// Forcing this term to stay negative helps convergence according to
//
// http://cas.ensmp.fr/Publications/Publications/Papers/ObserverPermanentMagnet.pdf
// and
// https://arxiv.org/pdf/1905.00833.pdf
if ( err > 0.0 ) {
err = 0.0 ;
}
float x1_dot = v_alpha - R_ia + gamma_half * ( * x1 - L_ia ) * err ;
float x2_dot = v_beta - R_ib + gamma_half * ( * x2 - L_ib ) * err ;
2020-03-28 11:21:45 -07:00
* x1 + = x1_dot * dt ;
* x2 + = x2_dot * dt ;
} break ;
default :
break ;
2017-10-20 11:06:06 -07:00
}
2017-09-04 12:12:43 -07:00
2017-10-20 11:06:06 -07:00
UTILS_NAN_ZERO ( * x1 ) ;
UTILS_NAN_ZERO ( * x2 ) ;
2015-12-10 14:21:04 -08:00
2021-02-28 11:36:02 -08:00
// Prevent the magnitude from getting too low, as that makes the angle very unstable.
float mag = sqrtf ( SQ ( * x1 ) + SQ ( * x2 ) ) ;
if ( mag < ( conf_now - > foc_motor_flux_linkage * 0.5 ) ) {
* x1 * = 1.1 ;
* x2 * = 1.1 ;
}
2020-01-12 12:25:21 -08:00
if ( phase ) {
* phase = utils_fast_atan2 ( * x2 - L_ib , * x1 - L_ia ) ;
}
2015-12-08 12:01:23 -08:00
}
static void pll_run ( float phase , float dt , volatile float * phase_var ,
2020-03-28 11:21:45 -07:00
volatile float * speed_var , volatile mc_configuration * conf ) {
2017-10-20 11:06:06 -07:00
UTILS_NAN_ZERO ( * phase_var ) ;
2015-12-08 12:01:23 -08:00
float delta_theta = phase - * phase_var ;
utils_norm_angle_rad ( & delta_theta ) ;
2017-10-20 11:06:06 -07:00
UTILS_NAN_ZERO ( * speed_var ) ;
2020-03-21 03:40:50 -07:00
* phase_var + = ( * speed_var + conf - > foc_pll_kp * delta_theta ) * dt ;
2015-12-08 12:01:23 -08:00
utils_norm_angle_rad ( ( float * ) phase_var ) ;
2020-03-21 03:40:50 -07:00
* speed_var + = conf - > foc_pll_ki * delta_theta * dt ;
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
2020-01-12 12:25:21 -08:00
* speed_rad_s
2015-12-08 12:01:23 -08:00
*
* 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 .
*/
2020-03-16 10:32:39 -07:00
static void control_current ( volatile motor_all_state_t * motor , float dt ) {
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 | |
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 ) ) & &
2020-03-16 10:32:39 -07:00
! motor - > m_phase_override & &
2020-03-21 03:40:50 -07:00
abs_rpm < ( conf_now - > foc_sl_erpm_hfi * ( motor - > m_cc_was_hfi ? 1.8 : 1.5 ) ) ;
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.
2020-03-21 03:40:50 -07:00
if ( do_hfi & & motor - > m_hfi . est_done_cnt < conf_now - > foc_hfi_start_samples ) {
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
2021-11-04 23:32:08 -07: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 ;
2021-11-04 23:32:08 -07:00
state_m - > vd = state_m - > vd_int + Ierr_d * conf_now - > foc_current_kp * d_gain_scale ; //Feedback (PI controller). No D action needed because the plant is a first order system (tf = 1/(Ls+R))
2020-03-21 03:40:50 -07:00
state_m - > vq = state_m - > vq_int + Ierr_q * conf_now - > foc_current_kp ;
2018-03-24 14:32:58 -07:00
// Temperature compensation
const float t = mc_interface_temp_motor_filtered ( ) ;
2020-03-21 03:40:50 -07:00
float ki = conf_now - > foc_current_ki ;
2021-11-24 03:18:29 -08:00
if ( conf_now - > foc_temp_comp & & t > - 30.0 ) {
2020-03-21 03:40:50 -07:00
ki + = ki * 0.00386 * ( t - conf_now - > foc_temp_comp_base_temp ) ;
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
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):
// 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
2020-03-21 03:40:50 -07:00
if ( motor - > m_control_mode < CONTROL_MODE_HANDBRAKE & & conf_now - > foc_cc_decoupling ! = FOC_CC_DECOUPLING_DISABLED ) {
2021-12-19 14:55:03 -08:00
float lq = conf_now - > foc_motor_l + conf_now - > foc_motor_ld_lq_diff * 0.5 ;
float ld = conf_now - > foc_motor_l - conf_now - > foc_motor_ld_lq_diff * 0.5 ;
2021-04-11 10:38:55 -07:00
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 :
2021-11-21 09:17:40 -08:00
dec_vd = state_m - > iq_filter * motor - > m_speed_est_fast * lq ; // m_speed_est_fast is ωe in [rad/s]
2021-10-15 13:25:07 -07:00
dec_vq = state_m - > id_filter * motor - > m_speed_est_fast * 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 :
2021-10-15 13:25:07 -07:00
dec_vd = state_m - > iq_filter * motor - > m_speed_est_fast * lq ;
dec_vq = state_m - > id_filter * motor - > m_speed_est_fast * 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
state_m - > i_abs = sqrtf ( SQ ( state_m - > id ) + SQ ( state_m - > iq ) ) ;
2017-09-04 12:12:43 -07:00
state_m - > i_abs_filter = sqrtf ( SQ ( state_m - > id_filter ) + SQ ( state_m - > iq_filter ) ) ;
2015-12-08 12:01:23 -08:00
2021-11-04 23:32:08 -07: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 ) ;
2021-11-04 23:32:08 -07: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
2020-01-20 00:39:33 -08:00
// HFI
if ( do_hfi ) {
CURRENT_FILTER_OFF ( ) ;
2022-02-06 14:12:05 -08:00
float mod_alpha_tmp = state_m - > mod_alpha_raw ;
float mod_beta_tmp = state_m - > mod_beta_raw ;
2020-01-20 00:39:33 -08:00
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 ,
2020-03-28 11:21:45 -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
2020-03-16 10:32:39 -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 ) ;
2021-10-15 13:25:07 -07:00
float di = ( sample_now - motor - > m_hfi . prev_sample ) ;
2020-01-20 00:39:33 -08:00
2021-10-15 13:25:07 -07:00
motor - > m_hfi . buffer_current [ motor - > m_hfi . ind ] = di ;
2020-01-28 10:46:19 -08:00
2021-10-15 13:25:07 -07:00
if ( di > 0.01 ) {
2022-01-06 11:44:00 -08:00
motor - > m_hfi . buffer [ motor - > m_hfi . ind ] = hfi_voltage / ( conf_now - > foc_f_zv * di ) ;
2020-01-28 10:46:19 -08:00
}
2020-03-16 10:32:39 -07:00
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-20 00:39:33 -08:00
}
2021-11-07 04:37:09 -08:00
mod_alpha_tmp + = hfi_voltage * utils_tab_sin_32_1 [ motor - > m_hfi . ind * motor - > m_hfi . table_fact ] * voltage_normalize ;
mod_beta_tmp - = hfi_voltage * utils_tab_cos_32_1 [ motor - > m_hfi . ind * motor - > m_hfi . table_fact ] * voltage_normalize ;
2020-01-20 00:39:33 -08:00
} else {
2020-03-16 10:32:39 -07:00
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 ;
2020-01-28 10:46:19 -08:00
2021-11-07 04:37:09 -08:00
mod_alpha_tmp - = hfi_voltage * utils_tab_sin_32_1 [ motor - > m_hfi . ind * motor - > m_hfi . table_fact ] * voltage_normalize ;
mod_beta_tmp + = hfi_voltage * utils_tab_cos_32_1 [ motor - > m_hfi . ind * motor - > m_hfi . table_fact ] * voltage_normalize ;
2020-01-20 00:39:33 -08:00
}
utils_saturate_vector_2d ( & mod_alpha_tmp , & mod_beta_tmp , 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
2020-03-21 03:40:50 -07:00
if ( conf_now - > foc_sample_v0_v7 ) {
2022-02-06 14:12:05 -08:00
state_m - > mod_alpha_raw = mod_alpha_tmp ;
state_m - > mod_beta_raw = mod_beta_tmp ;
2020-01-20 00:39:33 -08:00
} else {
// 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.
2021-11-13 12:51:20 -08:00
svm ( mod_alpha_tmp , mod_beta_tmp , 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 {
CURRENT_FILTER_ON ( ) ;
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 ;
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-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-02-06 14:12:05 -08:00
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
}
}
2021-02-28 11:36:02 -08:00
static void update_valpha_vbeta ( volatile motor_all_state_t * motor , float mod_alpha , float mod_beta ) {
volatile motor_state_t * state_m = & motor - > m_motor_state ;
volatile mc_configuration * conf_now = motor - > m_conf ;
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
}
2021-10-27 11:07:58 -07:00
float abs_rpm = fabsf ( RADPS2RPM_f ( motor - > m_pll_speed ) ) ;
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 ) ;
}
float v_mag = sqrtf ( SQ ( v_alpha ) + SQ ( v_beta ) ) ;
// 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 ) {
// Compensate for the phase delay by using the direction of the modulation
// together with the magnitude from the phase filters
float mod_mag = sqrtf ( SQ ( mod_alpha ) + SQ ( mod_beta ) ) ;
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
}
}
2021-11-07 15:21:23 -08:00
/**
2021-11-13 12:51:20 -08:00
* @ brief svm Space vector modulation . Magnitude must not be larger than sqrt ( 3 ) / 2 , or 0.866 to avoid overmodulation .
2021-11-07 15:21:23 -08:00
* See https : //github.com/vedderb/bldc/pull/372#issuecomment-962499623 for a full description.
2021-11-13 12:51:20 -08:00
* @ param alpha voltage
2021-11-07 15:21:23 -08:00
* @ param beta Park transformed and normalized voltage
2021-11-13 12:51:20 -08:00
* @ param PWMFullDutyCycle is the peak value of the PWM counter .
* @ param tAout PWM duty cycle phase A ( 0 = off all of the time , PWMFullDutyCycle = on all of the time )
* @ param tBout PWM duty cycle phase B
* @ param tCout PWM duty cycle phase C
2021-11-07 15:21:23 -08:00
*/
2021-11-13 12:51:20 -08:00
static void svm ( float alpha , float beta , uint32_t PWMFullDutyCycle ,
2020-03-28 11:21:45 -07:00
uint32_t * tAout , uint32_t * tBout , uint32_t * tCout , uint32_t * svm_sector ) {
2015-12-08 12:01:23 -08:00
uint32_t sector ;
if ( beta > = 0.0f ) {
if ( alpha > = 0.0f ) {
//quadrant I
2016-06-27 08:29:09 -07:00
if ( ONE_BY_SQRT3 * beta > alpha ) {
2015-12-08 12:01:23 -08:00
sector = 2 ;
2016-06-27 08:29:09 -07:00
} else {
2015-12-08 12:01:23 -08:00
sector = 1 ;
2016-06-27 08:29:09 -07:00
}
2015-12-08 12:01:23 -08:00
} else {
//quadrant II
2016-06-27 08:29:09 -07:00
if ( - ONE_BY_SQRT3 * beta > alpha ) {
2015-12-08 12:01:23 -08:00
sector = 3 ;
2016-06-27 08:29:09 -07:00
} else {
2015-12-08 12:01:23 -08:00
sector = 2 ;
2016-06-27 08:29:09 -07:00
}
2015-12-08 12:01:23 -08:00
}
} else {
if ( alpha > = 0.0f ) {
//quadrant IV5
2016-06-27 08:29:09 -07:00
if ( - ONE_BY_SQRT3 * beta > alpha ) {
2015-12-08 12:01:23 -08:00
sector = 5 ;
2016-06-27 08:29:09 -07:00
} else {
2015-12-08 12:01:23 -08:00
sector = 6 ;
2016-06-27 08:29:09 -07:00
}
2015-12-08 12:01:23 -08:00
} else {
//quadrant III
2016-06-27 08:29:09 -07:00
if ( ONE_BY_SQRT3 * beta > alpha ) {
2015-12-08 12:01:23 -08:00
sector = 4 ;
2016-06-27 08:29:09 -07:00
} else {
2015-12-08 12:01:23 -08:00
sector = 5 ;
2016-06-27 08:29:09 -07:00
}
2015-12-08 12:01:23 -08:00
}
}
// PWM timings
uint32_t tA , tB , tC ;
switch ( sector ) {
// sector 1-2
case 1 : {
// Vector on-times
2021-11-13 12:51:20 -08:00
uint32_t t1 = ( alpha - ONE_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
uint32_t t2 = ( TWO_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
2015-12-08 12:01:23 -08:00
// PWM timings
2021-11-13 12:51:20 -08:00
tA = ( PWMFullDutyCycle + t1 + t2 ) / 2 ;
tB = tA - t1 ;
tC = tB - t2 ;
2015-12-08 12:01:23 -08:00
break ;
}
// sector 2-3
case 2 : {
// Vector on-times
2021-11-13 12:51:20 -08:00
uint32_t t2 = ( alpha + ONE_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
uint32_t t3 = ( - alpha + ONE_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
2015-12-08 12:01:23 -08:00
// PWM timings
2021-11-13 12:51:20 -08:00
tB = ( PWMFullDutyCycle + t2 + t3 ) / 2 ;
tA = tB - t3 ;
tC = tA - t2 ;
2015-12-08 12:01:23 -08:00
break ;
}
// sector 3-4
case 3 : {
// Vector on-times
2021-11-13 12:51:20 -08:00
uint32_t t3 = ( TWO_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
uint32_t t4 = ( - alpha - ONE_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
2015-12-08 12:01:23 -08:00
// PWM timings
2021-11-13 12:51:20 -08:00
tB = ( PWMFullDutyCycle + t3 + t4 ) / 2 ;
tC = tB - t3 ;
tA = tC - t4 ;
2015-12-08 12:01:23 -08:00
break ;
}
// sector 4-5
case 4 : {
// Vector on-times
2021-11-13 12:51:20 -08:00
uint32_t t4 = ( - alpha + ONE_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
uint32_t t5 = ( - TWO_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
2015-12-08 12:01:23 -08:00
// PWM timings
2021-11-13 12:51:20 -08:00
tC = ( PWMFullDutyCycle + t4 + t5 ) / 2 ;
tB = tC - t5 ;
tA = tB - t4 ;
2015-12-08 12:01:23 -08:00
break ;
}
// sector 5-6
case 5 : {
// Vector on-times
2021-11-13 12:51:20 -08:00
uint32_t t5 = ( - alpha - ONE_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
uint32_t t6 = ( alpha - ONE_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
2015-12-08 12:01:23 -08:00
// PWM timings
2021-11-13 12:51:20 -08:00
tC = ( PWMFullDutyCycle + t5 + t6 ) / 2 ;
tA = tC - t5 ;
tB = tA - t6 ;
2015-12-08 12:01:23 -08:00
break ;
}
// sector 6-1
case 6 : {
// Vector on-times
2021-11-13 12:51:20 -08:00
uint32_t t6 = ( - TWO_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
uint32_t t1 = ( alpha + ONE_BY_SQRT3 * beta ) * PWMFullDutyCycle ;
2015-12-08 12:01:23 -08:00
// PWM timings
2021-11-13 12:51:20 -08:00
tA = ( PWMFullDutyCycle + t6 + t1 ) / 2 ;
tC = tA - t1 ;
tB = tC - t6 ;
2015-12-08 12:01:23 -08:00
break ;
}
}
* tAout = tA ;
* tBout = tB ;
* tCout = tC ;
2016-06-27 08:29:09 -07:00
* svm_sector = sector ;
2015-12-08 12:01:23 -08:00
}
2021-07-12 05:31:01 -07:00
static void run_pid_control_pos ( float dt , volatile motor_all_state_t * motor ) {
2020-03-21 03:40:50 -07:00
volatile mc_configuration * conf_now = motor - > m_conf ;
2021-07-12 05:31:01 -07:00
float angle_now = motor - > m_pos_pid_now ;
float angle_set = motor - > m_pos_pid_set ;
2015-12-08 12:01:23 -08:00
float p_term ;
float d_term ;
2021-07-11 11:19:26 -07:00
float d_term_proc ;
2015-12-08 12:01:23 -08:00
// PID is off. Return.
2020-03-16 10:32:39 -07:00
if ( motor - > m_control_mode ! = CONTROL_MODE_POS ) {
motor - > m_pos_i_term = 0 ;
motor - > m_pos_prev_error = 0 ;
2021-11-15 12:29:05 -08:00
motor - > m_pos_prev_proc = angle_now ;
motor - > m_pos_d_filter = 0.0 ;
motor - > m_pos_d_filter_proc = 0.0 ;
2015-12-08 12:01:23 -08:00
return ;
}
2016-02-24 12:17:39 -08:00
// Compute parameters
float error = utils_angle_difference ( angle_set , angle_now ) ;
2021-07-11 11:19:26 -07:00
float error_sign = 1.0 ;
2015-12-28 17:37:09 -08:00
2022-01-09 08:10:40 -08:00
if ( encoder_is_configured ( ) ) {
2020-03-21 03:40:50 -07:00
if ( conf_now - > foc_encoder_inverted ) {
2021-07-11 11:19:26 -07:00
error_sign = - 1.0 ;
2016-04-27 06:32:32 -07:00
}
2015-12-28 17:37:09 -08:00
}
2021-07-11 11:19:26 -07:00
error * = error_sign ;
2021-04-26 14:14:05 -07:00
float kp = conf_now - > p_pid_kp ;
float ki = conf_now - > p_pid_ki ;
float kd = conf_now - > p_pid_kd ;
2021-07-11 11:19:26 -07:00
float kd_proc = conf_now - > p_pid_kd_proc ;
2021-04-26 14:14:05 -07:00
if ( conf_now - > p_pid_gain_dec_angle > 0.1 ) {
float min_error = conf_now - > p_pid_gain_dec_angle / conf_now - > p_pid_ang_div ;
float error_abs = fabs ( error ) ;
if ( error_abs < min_error ) {
float scale = error_abs / min_error ;
kp * = scale ;
ki * = scale ;
kd * = scale ;
2021-07-11 11:19:26 -07:00
kd_proc * = scale ;
2021-04-26 14:14:05 -07:00
}
}
p_term = error * kp ;
motor - > m_pos_i_term + = error * ( ki * dt ) ;
2016-02-24 12:17:39 -08:00
// Average DT for the D term when the error does not change. This likely
// happens at low speed when the position resolution is low and several
// control iterations run without position updates.
// TODO: Are there problems with this approach?
2020-03-16 10:32:39 -07:00
motor - > m_pos_dt_int + = dt ;
if ( error = = motor - > m_pos_prev_error ) {
2016-02-24 12:17:39 -08:00
d_term = 0.0 ;
} else {
2021-04-26 14:14:05 -07:00
d_term = ( error - motor - > m_pos_prev_error ) * ( kd / motor - > m_pos_dt_int ) ;
2020-03-16 10:32:39 -07:00
motor - > m_pos_dt_int = 0.0 ;
2016-02-24 12:17:39 -08:00
}
2015-12-08 12:01:23 -08:00
2018-03-24 14:32:58 -07:00
// Filter D
2020-03-21 03:40:50 -07:00
UTILS_LP_FAST ( motor - > m_pos_d_filter , d_term , conf_now - > p_pid_kd_filter ) ;
2020-03-16 10:32:39 -07:00
d_term = motor - > m_pos_d_filter ;
2018-03-24 14:32:58 -07:00
2021-07-11 11:19:26 -07:00
// Process D term
motor - > m_pos_dt_int_proc + = dt ;
if ( angle_now = = motor - > m_pos_prev_proc ) {
d_term_proc = 0.0 ;
} else {
d_term_proc = - utils_angle_difference ( angle_now , motor - > m_pos_prev_proc ) * error_sign * ( kd_proc / motor - > m_pos_dt_int_proc ) ;
motor - > m_pos_dt_int_proc = 0.0 ;
}
// Filter D process
UTILS_LP_FAST ( motor - > m_pos_d_filter_proc , d_term_proc , conf_now - > p_pid_kd_filter ) ;
d_term_proc = motor - > m_pos_d_filter_proc ;
2018-03-24 14:32:58 -07:00
2015-12-08 12:01:23 -08:00
// I-term wind-up protection
2020-02-27 08:49:35 -08:00
float p_tmp = p_term ;
utils_truncate_number_abs ( & p_tmp , 1.0 ) ;
2020-03-16 10:32:39 -07:00
utils_truncate_number_abs ( ( float * ) & motor - > m_pos_i_term , 1.0 - fabsf ( p_tmp ) ) ;
2015-12-08 12:01:23 -08:00
// Store previous error
2020-03-16 10:32:39 -07:00
motor - > m_pos_prev_error = error ;
2021-07-11 11:19:26 -07:00
motor - > m_pos_prev_proc = angle_now ;
2015-12-08 12:01:23 -08:00
// Calculate output
2021-07-11 11:19:26 -07:00
float output = p_term + motor - > m_pos_i_term + d_term + d_term_proc ;
2015-12-08 12:01:23 -08:00
utils_truncate_number ( & output , - 1.0 , 1.0 ) ;
2022-01-09 08:10:40 -08:00
if ( encoder_is_configured ( ) ) {
if ( encoder_index_found ( ) ) {
2021-03-13 02:42:23 -08:00
motor - > m_iq_set = output * conf_now - > l_current_max * conf_now - > l_current_max_scale ; ;
2016-04-27 06:32:32 -07:00
} else {
// Rotate the motor with 40 % power until the encoder index is found.
2021-03-13 02:42:23 -08:00
motor - > m_iq_set = 0.4 * conf_now - > l_current_max * conf_now - > l_current_max_scale ; ;
2016-04-27 06:32:32 -07:00
}
2015-12-27 16:50:23 -08:00
} else {
2021-03-13 02:42:23 -08:00
motor - > m_iq_set = output * conf_now - > l_current_max * conf_now - > l_current_max_scale ; ;
2015-12-27 16:50:23 -08:00
}
2015-12-08 12:01:23 -08:00
}
2020-03-16 10:32:39 -07:00
static void run_pid_control_speed ( float dt , volatile motor_all_state_t * motor ) {
2020-03-21 03:40:50 -07:00
volatile mc_configuration * conf_now = motor - > m_conf ;
2015-12-08 12:01:23 -08:00
float p_term ;
float d_term ;
// PID is off. Return.
2020-03-16 10:32:39 -07:00
if ( motor - > m_control_mode ! = CONTROL_MODE_SPEED ) {
motor - > m_speed_i_term = 0.0 ;
motor - > m_speed_prev_error = 0.0 ;
2021-11-15 12:29:05 -08:00
motor - > m_speed_d_filter = 0.0 ;
2015-12-08 12:01:23 -08:00
return ;
}
2020-05-12 06:53:32 -07:00
if ( conf_now - > s_pid_ramp_erpms_s > 0.0 ) {
utils_step_towards ( ( float * ) & motor - > m_speed_pid_set_rpm , motor - > m_speed_command_rpm , conf_now - > s_pid_ramp_erpms_s * dt ) ;
}
2017-09-04 12:12:43 -07:00
const float rpm = mcpwm_foc_get_rpm ( ) ;
2020-03-16 10:32:39 -07:00
float error = motor - > m_speed_pid_set_rpm - rpm ;
2017-09-04 12:12:43 -07:00
2016-02-24 12:17:39 -08:00
// Too low RPM set. Reset state and return.
2020-03-21 03:40:50 -07:00
if ( fabsf ( motor - > m_speed_pid_set_rpm ) < conf_now - > s_pid_min_erpm ) {
2020-03-16 10:32:39 -07:00
motor - > m_speed_i_term = 0.0 ;
motor - > m_speed_prev_error = error ;
2015-12-08 12:01:23 -08:00
return ;
}
// Compute parameters
2020-03-21 03:40:50 -07:00
p_term = error * conf_now - > s_pid_kp * ( 1.0 / 20.0 ) ;
d_term = ( error - motor - > m_speed_prev_error ) * ( conf_now - > s_pid_kd / dt ) * ( 1.0 / 20.0 ) ;
2015-12-08 12:01:23 -08:00
2018-03-24 14:32:58 -07:00
// Filter D
2020-03-21 03:40:50 -07:00
UTILS_LP_FAST ( motor - > m_speed_d_filter , d_term , conf_now - > s_pid_kd_filter ) ;
2020-03-16 10:32:39 -07:00
d_term = motor - > m_speed_d_filter ;
2018-03-24 14:32:58 -07:00
2015-12-08 12:01:23 -08:00
// Store previous error
2020-03-16 10:32:39 -07:00
motor - > m_speed_prev_error = error ;
2015-12-08 12:01:23 -08:00
// Calculate output
2021-07-14 10:59:34 -07:00
utils_truncate_number_abs ( & p_term , 1.0 ) ;
utils_truncate_number_abs ( & d_term , 1.0 ) ;
2020-03-16 10:32:39 -07:00
float output = p_term + motor - > m_speed_i_term + d_term ;
2021-04-11 07:43:19 -07:00
float pre_output = output ;
2021-07-14 10:59:34 -07:00
utils_truncate_number_abs ( & output , 1.0 ) ;
2015-12-08 12:01:23 -08:00
2021-04-11 07:43:19 -07:00
float output_saturation = output - pre_output ;
motor - > m_speed_i_term + = error * ( conf_now - > s_pid_ki * dt ) * ( 1.0 / 20.0 ) + output_saturation ;
2021-07-14 10:59:34 -07:00
if ( conf_now - > s_pid_ki < 1e-9 ) {
motor - > m_speed_i_term = 0.0 ;
}
2021-04-11 07:43:19 -07:00
2017-09-04 12:12:43 -07:00
// Optionally disable braking
2020-03-21 03:40:50 -07:00
if ( ! conf_now - > s_pid_allow_braking ) {
2019-02-18 10:30:19 -08:00
if ( rpm > 20.0 & & output < 0.0 ) {
2017-09-04 12:12:43 -07:00
output = 0.0 ;
}
2019-02-18 10:30:19 -08:00
if ( rpm < - 20.0 & & output > 0.0 ) {
2017-09-04 12:12:43 -07:00
output = 0.0 ;
}
}
2021-03-13 02:42:23 -08:00
motor - > m_iq_set = output * conf_now - > l_current_max * conf_now - > l_current_max_scale ;
2015-12-08 12:01:23 -08:00
}
2020-03-16 10:32:39 -07:00
static void stop_pwm_hw ( volatile 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
}
2020-03-16 10:32:39 -07:00
static void start_pwm_hw ( volatile motor_all_state_t * motor ) {
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-03-16 10:32:39 -07:00
static float correct_encoder ( float obs_angle , float enc_angle , float speed ,
2020-03-28 11:21:45 -07:00
float sl_erpm , volatile motor_all_state_t * motor ) {
2021-10-13 09:13:51 -07:00
float rpm_abs = fabsf ( RADPS2RPM_f ( speed ) ) ;
2016-04-27 06:32:32 -07:00
// Hysteresis 5 % of total speed
2020-01-20 00:39:33 -08:00
float hyst = sl_erpm * 0.05 ;
2020-03-16 10:32:39 -07:00
if ( motor - > m_using_encoder ) {
2020-01-20 00:39:33 -08:00
if ( rpm_abs > ( sl_erpm + hyst ) ) {
2020-03-16 10:32:39 -07:00
motor - > m_using_encoder = false ;
2016-04-27 06:32:32 -07:00
}
} else {
2020-01-20 00:39:33 -08:00
if ( rpm_abs < ( sl_erpm - hyst ) ) {
2020-03-16 10:32:39 -07:00
motor - > m_using_encoder = true ;
2016-04-27 06:32:32 -07:00
}
}
2020-03-16 10:32:39 -07:00
return motor - > m_using_encoder ? enc_angle : obs_angle ;
2016-04-27 06:32:32 -07:00
}
2020-04-16 15:38:36 -07:00
static float correct_hall ( float angle , float dt , volatile motor_all_state_t * motor ) {
2020-03-21 03:40:50 -07:00
volatile mc_configuration * conf_now = motor - > m_conf ;
2020-04-16 15:38:36 -07:00
motor - > m_hall_dt_diff_now + = dt ;
float rad_per_sec = ( M_PI / 3.0 ) / motor - > m_hall_dt_diff_last ;
2021-10-13 09:13:51 -07:00
float rpm_abs_fast = fabsf ( RADPS2RPM_f ( motor - > m_speed_est_fast ) ) ;
float rpm_abs_hall = fabsf ( RADPS2RPM_f ( rad_per_sec ) ) ;
2016-01-27 14:57:23 -08:00
2016-04-27 06:32:32 -07:00
// Hysteresis 5 % of total speed
2020-03-21 03:40:50 -07:00
float hyst = conf_now - > foc_sl_erpm * 0.1 ;
2020-03-16 10:32:39 -07:00
if ( motor - > m_using_hall ) {
2020-04-16 15:38:36 -07:00
if ( fminf ( rpm_abs_fast , rpm_abs_hall ) > ( conf_now - > foc_sl_erpm + hyst ) ) {
2020-03-16 10:32:39 -07:00
motor - > m_using_hall = false ;
2016-01-27 14:57:23 -08:00
}
} else {
2020-04-16 15:38:36 -07:00
if ( rpm_abs_fast < ( conf_now - > foc_sl_erpm - hyst ) ) {
2020-03-16 10:32:39 -07:00
motor - > m_using_hall = true ;
2016-01-27 14:57:23 -08:00
}
}
2020-10-09 12:08:48 -07:00
int ang_hall_int = conf_now - > foc_hall_table [ utils_read_hall (
motor ! = & m_motor_1 , conf_now - > m_hall_extra_samples ) ] ;
2016-01-27 14:57:23 -08:00
2020-04-16 15:38:36 -07:00
// Only override the observer if the hall sensor value is valid.
if ( ang_hall_int < 201 ) {
2021-10-13 09:13:51 -07:00
// Scale to the circle and convert to radians
float ang_hall_now = ( ( float ) ang_hall_int / 200.0 ) * 2 * M_PI ;
2016-01-27 14:57:23 -08:00
2020-04-16 15:38:36 -07:00
if ( motor - > m_ang_hall_int_prev < 0 ) {
// Previous angle not valid
motor - > m_ang_hall_int_prev = ang_hall_int ;
motor - > m_ang_hall = ang_hall_now ;
} else if ( ang_hall_int ! = motor - > m_ang_hall_int_prev ) {
int diff = ang_hall_int - motor - > m_ang_hall_int_prev ;
if ( diff > 100 ) {
diff - = 200 ;
} else if ( diff < - 100 ) {
diff + = 200 ;
}
2016-01-27 14:57:23 -08:00
2020-04-16 15:38:36 -07:00
// This is only valid if the direction did not just change. If it did, we use the
// last speed together with the sign right now.
if ( SIGN ( diff ) = = SIGN ( motor - > m_hall_dt_diff_last ) ) {
if ( diff > 0 ) {
motor - > m_hall_dt_diff_last = motor - > m_hall_dt_diff_now ;
2016-01-27 14:57:23 -08:00
} else {
2020-04-16 15:38:36 -07:00
motor - > m_hall_dt_diff_last = - motor - > m_hall_dt_diff_now ;
2016-01-27 14:57:23 -08:00
}
2020-04-16 15:38:36 -07:00
} else {
motor - > m_hall_dt_diff_last = - motor - > m_hall_dt_diff_last ;
2016-01-27 14:57:23 -08:00
}
2020-04-16 15:38:36 -07:00
motor - > m_hall_dt_diff_now = 0.0 ;
// A transition was just made. The angle is in the middle of the new and old angle.
int ang_avg = motor - > m_ang_hall_int_prev + diff / 2 ;
ang_avg % = 200 ;
2021-10-13 09:13:51 -07:00
// Scale to the circle and convert to radians
motor - > m_ang_hall = ( ( float ) ang_avg / 200.0 ) * 2 * M_PI ;
2020-04-16 15:38:36 -07:00
}
motor - > m_ang_hall_int_prev = ang_hall_int ;
2016-01-27 14:57:23 -08:00
2021-10-13 09:13:51 -07:00
if ( RADPS2RPM_f ( ( M_PI / 3.0 ) /
2020-10-21 23:37:32 -07:00
fmaxf ( fabsf ( motor - > m_hall_dt_diff_now ) ,
2021-10-13 09:13:51 -07:00
fabsf ( motor - > m_hall_dt_diff_last ) ) ) < conf_now - > foc_hall_interp_erpm ) {
2020-04-16 15:38:36 -07:00
// Don't interpolate on very low speed, just use the closest hall sensor. The reason is that we might
// get stuck at 60 degrees off if a direction change happens between two steps.
motor - > m_ang_hall = ang_hall_now ;
} else {
// Interpolate
float diff = utils_angle_difference_rad ( motor - > m_ang_hall , ang_hall_now ) ;
if ( fabsf ( diff ) < ( ( 2.0 * M_PI ) / 12.0 ) ) {
// Do interpolation
motor - > m_ang_hall + = rad_per_sec * dt ;
2016-01-27 14:57:23 -08:00
} else {
2020-04-16 15:38:36 -07:00
// We are too far away with the interpolation
motor - > m_ang_hall - = diff / 100.0 ;
2016-01-27 14:57:23 -08:00
}
2020-04-16 15:38:36 -07:00
}
2016-01-27 14:57:23 -08:00
2021-04-26 08:57:49 -07:00
// Limit hall sensor rate of change. This will reduce current spikes in the current controllers when the angle estimation
// changes fast.
float angle_step = ( fmaxf ( rpm_abs_hall , conf_now - > foc_hall_interp_erpm ) / 60.0 ) * 2.0 * M_PI * dt * 1.5 ;
float angle_diff = utils_angle_difference_rad ( motor - > m_ang_hall , motor - > m_ang_hall_rate_limited ) ;
if ( fabsf ( angle_diff ) < angle_step ) {
motor - > m_ang_hall_rate_limited = motor - > m_ang_hall ;
} else {
motor - > m_ang_hall_rate_limited + = angle_step * SIGN ( angle_diff ) ;
}
utils_norm_angle_rad ( ( float * ) & motor - > m_ang_hall_rate_limited ) ;
2020-04-16 15:38:36 -07:00
utils_norm_angle_rad ( ( float * ) & motor - > m_ang_hall ) ;
2021-04-26 08:57:49 -07:00
2020-04-16 15:38:36 -07:00
if ( motor - > m_using_hall ) {
2021-04-26 08:57:49 -07:00
angle = motor - > m_ang_hall_rate_limited ;
2016-01-27 14:57:23 -08:00
}
} else {
2020-04-16 15:38:36 -07:00
// Invalid hall reading. Don't update angle.
motor - > m_ang_hall_int_prev = - 1 ;
// Also allow open loop in order to behave like normal sensorless
// operation. Then the motor works even if the hall sensor cable
// gets disconnected (when the sensor spacing is 120 degrees).
if ( motor - > m_phase_observer_override & & motor - > m_state = = MC_STATE_RUNNING ) {
angle = motor - > m_phase_now_observer_override ;
}
2016-01-27 14:57:23 -08:00
}
return angle ;
}
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 ) ;
2020-01-28 10:46:19 -08:00
if ( d = = 0 | | d = = 1 | | d = = 2 ) {
2020-03-16 14:42:44 -07:00
motor_now ( ) - > m_hfi_plot_en = d ;
if ( motor_now ( ) - > m_hfi_plot_en = = 1 ) {
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) " ) ;
2020-03-16 14:42:44 -07:00
} else if ( motor_now ( ) - > m_hfi_plot_en = = 2 ) {
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) " ) ;
2020-01-20 00:39:33 -08:00
}
2020-03-16 14:42:44 -07:00
commands_printf ( 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 {
2020-01-28 10:46:19 -08:00
commands_printf ( " Invalid Argument. en has to be 0, 1 or 2. \n " ) ;
2020-01-20 00:39:33 -08:00
}
} else {
commands_printf ( " This command requires one argument. \n " ) ;
}
}