2014-01-09 06:20:26 -08:00
|
|
|
/*
|
2022-01-19 09:52:45 -08:00
|
|
|
Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se
|
2014-01-09 06:20:26 -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
|
2014-01-09 06:20:26 -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,
|
2014-01-09 06:20:26 -08:00
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
2016-11-04 07:18:34 -07:00
|
|
|
*/
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
#include "ch.h"
|
|
|
|
#include "hal.h"
|
|
|
|
#include "stm32f4xx_conf.h"
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <math.h>
|
|
|
|
#include <stdio.h>
|
2014-03-29 16:57:22 -07:00
|
|
|
#include <string.h>
|
2014-01-09 06:20:26 -08:00
|
|
|
#include "mcpwm.h"
|
2015-12-08 12:01:23 -08:00
|
|
|
#include "mc_interface.h"
|
2014-01-09 06:20:26 -08:00
|
|
|
#include "digital_filter.h"
|
2022-03-16 10:40:51 -07:00
|
|
|
#include "utils_math.h"
|
|
|
|
#include "utils_sys.h"
|
2014-01-09 06:20:26 -08:00
|
|
|
#include "ledpwm.h"
|
2014-10-24 14:04:10 -07:00
|
|
|
#include "terminal.h"
|
2019-01-24 07:19:44 -08:00
|
|
|
#include "timeout.h"
|
2022-01-09 08:10:40 -08:00
|
|
|
#include "encoder/encoder.h"
|
2019-03-10 06:57:42 -07:00
|
|
|
#include "timer.h"
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-04-01 10:26:46 -07:00
|
|
|
// Structs
|
|
|
|
typedef struct {
|
2014-10-24 14:04:10 -07:00
|
|
|
volatile bool updated;
|
2014-04-01 10:26:46 -07:00
|
|
|
volatile unsigned int top;
|
|
|
|
volatile unsigned int duty;
|
|
|
|
volatile unsigned int val_sample;
|
|
|
|
volatile unsigned int curr1_sample;
|
|
|
|
volatile unsigned int curr2_sample;
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
volatile unsigned int curr3_sample;
|
|
|
|
#endif
|
2014-04-01 10:26:46 -07:00
|
|
|
} mc_timer_struct;
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
// Private variables
|
2014-04-21 07:17:39 -07:00
|
|
|
static volatile int comm_step; // Range [1 6]
|
|
|
|
static volatile int detect_step; // Range [0 5]
|
2014-01-09 06:20:26 -08:00
|
|
|
static volatile int direction;
|
|
|
|
static volatile float dutycycle_set;
|
|
|
|
static volatile float dutycycle_now;
|
|
|
|
static volatile float rpm_now;
|
2014-03-30 15:51:59 -07:00
|
|
|
static volatile float speed_pid_set_rpm;
|
2015-04-26 15:02:32 -07:00
|
|
|
static volatile float pos_pid_set_pos;
|
2014-03-30 15:51:59 -07:00
|
|
|
static volatile float current_set;
|
2014-01-09 06:20:26 -08:00
|
|
|
static volatile int tachometer;
|
2014-10-16 00:42:26 -07:00
|
|
|
static volatile int tachometer_abs;
|
2014-03-29 16:57:22 -07:00
|
|
|
static volatile int tachometer_for_direction;
|
2014-01-09 06:20:26 -08:00
|
|
|
static volatile int curr0_sum;
|
|
|
|
static volatile int curr1_sum;
|
|
|
|
static volatile int curr_start_samples;
|
|
|
|
static volatile int curr0_offset;
|
|
|
|
static volatile int curr1_offset;
|
|
|
|
static volatile mc_state state;
|
2014-03-30 15:51:59 -07:00
|
|
|
static volatile mc_control_mode control_mode;
|
2014-01-09 06:20:26 -08:00
|
|
|
static volatile float last_current_sample;
|
2014-04-02 02:54:21 -07:00
|
|
|
static volatile float last_current_sample_filtered;
|
2014-03-15 14:32:00 -07:00
|
|
|
static volatile float mcpwm_detect_currents_avg[6];
|
2015-06-26 12:45:28 -07:00
|
|
|
static volatile float mcpwm_detect_avg_samples[6];
|
2014-08-05 13:24:13 -07:00
|
|
|
static volatile float switching_frequency_now;
|
2014-05-05 11:39:25 -07:00
|
|
|
static volatile int ignore_iterations;
|
2014-04-01 10:26:46 -07:00
|
|
|
static volatile mc_timer_struct timer_struct;
|
2014-04-12 12:59:33 -07:00
|
|
|
static volatile int curr_samp_volt; // Use the voltage-synchronized samples for this current sample
|
2014-06-29 05:23:39 -07:00
|
|
|
static int hall_to_phase_table[16];
|
2014-07-27 10:40:38 -07:00
|
|
|
static volatile unsigned int slow_ramping_cycles;
|
|
|
|
static volatile int has_commutated;
|
2014-09-13 01:46:46 -07:00
|
|
|
static volatile mc_rpm_dep_struct rpm_dep;
|
2014-09-12 14:37:33 -07:00
|
|
|
static volatile float cycle_integrator_sum;
|
|
|
|
static volatile float cycle_integrator_iterations;
|
2015-12-08 12:01:23 -08:00
|
|
|
static volatile mc_configuration *conf;
|
2014-04-04 14:19:11 -07:00
|
|
|
static volatile float pwm_cycles_sum;
|
2015-08-26 14:12:39 -07:00
|
|
|
static volatile int pwm_cycles;
|
2014-04-04 14:19:11 -07:00
|
|
|
static volatile float last_pwm_cycles_sum;
|
|
|
|
static volatile float last_pwm_cycles_sums[6];
|
2014-10-24 14:04:10 -07:00
|
|
|
static volatile bool dccal_done;
|
2015-06-19 14:38:46 -07:00
|
|
|
static volatile bool sensorless_now;
|
|
|
|
static volatile int hall_detect_table[8][7];
|
2019-02-18 10:30:19 -08:00
|
|
|
static volatile bool init_done = false;
|
2017-09-04 12:12:43 -07:00
|
|
|
static volatile mc_comm_mode comm_mode_next;
|
2019-02-18 10:30:19 -08:00
|
|
|
static volatile float m_pll_phase;
|
|
|
|
static volatile float m_pll_speed;
|
2019-03-10 06:57:42 -07:00
|
|
|
static volatile uint32_t rpm_timer_start;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
static volatile int curr2_sum;
|
|
|
|
static volatile int curr2_offset;
|
|
|
|
#endif
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
// KV FIR filter
|
|
|
|
#define KV_FIR_TAPS_BITS 7
|
|
|
|
#define KV_FIR_LEN (1 << KV_FIR_TAPS_BITS)
|
|
|
|
#define KV_FIR_FCUT 0.02
|
|
|
|
static volatile float kv_fir_coeffs[KV_FIR_LEN];
|
|
|
|
static volatile float kv_fir_samples[KV_FIR_LEN];
|
|
|
|
static volatile int kv_fir_index = 0;
|
|
|
|
|
2014-03-29 05:15:09 -07:00
|
|
|
// Amplitude FIR filter
|
|
|
|
#define AMP_FIR_TAPS_BITS 7
|
2014-09-12 14:37:33 -07:00
|
|
|
#define AMP_FIR_LEN (1 << AMP_FIR_TAPS_BITS)
|
2014-03-29 05:15:09 -07:00
|
|
|
#define AMP_FIR_FCUT 0.02
|
2014-09-12 14:37:33 -07:00
|
|
|
static volatile float amp_fir_coeffs[AMP_FIR_LEN];
|
|
|
|
static volatile float amp_fir_samples[AMP_FIR_LEN];
|
2014-03-29 05:15:09 -07:00
|
|
|
static volatile int amp_fir_index = 0;
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
// Current FIR filter
|
|
|
|
#define CURR_FIR_TAPS_BITS 4
|
|
|
|
#define CURR_FIR_LEN (1 << CURR_FIR_TAPS_BITS)
|
|
|
|
#define CURR_FIR_FCUT 0.15
|
|
|
|
static volatile float current_fir_coeffs[CURR_FIR_LEN];
|
|
|
|
static volatile float current_fir_samples[CURR_FIR_LEN];
|
|
|
|
static volatile int current_fir_index = 0;
|
|
|
|
|
|
|
|
static volatile float last_adc_isr_duration;
|
|
|
|
static volatile float last_inj_adc_isr_duration;
|
|
|
|
|
|
|
|
// Global variables
|
2014-03-13 07:28:56 -07:00
|
|
|
volatile float mcpwm_detect_currents[6];
|
2015-06-26 12:45:28 -07:00
|
|
|
volatile float mcpwm_detect_voltages[6];
|
2014-04-21 07:17:39 -07:00
|
|
|
volatile float mcpwm_detect_currents_diff[6];
|
2014-03-15 14:32:00 -07:00
|
|
|
volatile int mcpwm_vzero;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
// Private functions
|
2014-03-30 15:51:59 -07:00
|
|
|
static void set_duty_cycle_hl(float dutyCycle);
|
|
|
|
static void set_duty_cycle_ll(float dutyCycle);
|
2014-01-18 13:10:10 -08:00
|
|
|
static void set_duty_cycle_hw(float dutyCycle);
|
2014-05-05 11:39:25 -07:00
|
|
|
static void stop_pwm_ll(void);
|
|
|
|
static void stop_pwm_hw(void);
|
|
|
|
static void full_brake_ll(void);
|
|
|
|
static void full_brake_hw(void);
|
2015-04-26 15:02:32 -07:00
|
|
|
static void run_pid_control_speed(void);
|
2019-02-18 10:30:19 -08:00
|
|
|
static void run_pid_control_pos(float dt, float pos_now);
|
2014-01-09 06:20:26 -08:00
|
|
|
static void set_next_comm_step(int next_step);
|
|
|
|
static void update_rpm_tacho(void);
|
2015-06-19 14:38:46 -07:00
|
|
|
static void update_sensor_mode(void);
|
|
|
|
static int read_hall(void);
|
2014-04-01 10:26:46 -07:00
|
|
|
static void update_adc_sample_pos(mc_timer_struct *timer_tmp);
|
2014-10-07 13:44:44 -07:00
|
|
|
static void commutate(int steps);
|
2014-04-01 10:26:46 -07:00
|
|
|
static void set_next_timer_settings(mc_timer_struct *settings);
|
2014-10-24 14:04:10 -07:00
|
|
|
static void update_timer_attempt(void);
|
2014-08-05 13:24:13 -07:00
|
|
|
static void set_switching_frequency(float frequency);
|
2014-06-29 05:23:39 -07:00
|
|
|
static void do_dc_cal(void);
|
2019-02-18 10:30:19 -08:00
|
|
|
static void pll_run(float phase, float dt, volatile float *phase_var,
|
|
|
|
volatile float *speed_var);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
// Defines
|
2014-04-21 07:17:39 -07:00
|
|
|
#define IS_DETECTING() (state == MC_STATE_DETECTING)
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
// Threads
|
2022-01-16 12:57:12 -08:00
|
|
|
static THD_WORKING_AREA(timer_thread_wa, 512);
|
2015-10-08 14:09:39 -07:00
|
|
|
static THD_FUNCTION(timer_thread, arg);
|
2022-01-16 12:57:12 -08:00
|
|
|
static THD_WORKING_AREA(rpm_thread_wa, 512);
|
2015-10-08 14:09:39 -07:00
|
|
|
static THD_FUNCTION(rpm_thread, arg);
|
2015-12-08 12:01:23 -08:00
|
|
|
static volatile bool timer_thd_stop;
|
|
|
|
static volatile bool rpm_thd_stop;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
void mcpwm_init(volatile mc_configuration *configuration) {
|
2014-08-29 21:00:48 -07:00
|
|
|
utils_sys_lock_cnt();
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
init_done= false;
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
|
|
|
TIM_OCInitTypeDef TIM_OCInitStructure;
|
|
|
|
TIM_BDTRInitTypeDef TIM_BDTRInitStructure;
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
conf = configuration;
|
2014-09-14 14:08:22 -07:00
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
// Initialize variables
|
|
|
|
comm_step = 1;
|
2014-04-21 07:17:39 -07:00
|
|
|
detect_step = 0;
|
2014-01-09 06:20:26 -08:00
|
|
|
direction = 1;
|
2015-06-19 14:38:46 -07:00
|
|
|
rpm_now = 0.0;
|
2014-01-18 13:10:10 -08:00
|
|
|
dutycycle_set = 0.0;
|
|
|
|
dutycycle_now = 0.0;
|
2014-03-30 15:51:59 -07:00
|
|
|
speed_pid_set_rpm = 0.0;
|
2015-04-26 15:02:32 -07:00
|
|
|
pos_pid_set_pos = 0.0;
|
2014-03-30 15:51:59 -07:00
|
|
|
current_set = 0.0;
|
2014-01-09 06:20:26 -08:00
|
|
|
tachometer = 0;
|
2014-10-16 00:42:26 -07:00
|
|
|
tachometer_abs = 0;
|
2014-03-29 16:57:22 -07:00
|
|
|
tachometer_for_direction = 0;
|
2014-01-09 06:20:26 -08:00
|
|
|
state = MC_STATE_OFF;
|
2014-04-12 12:59:33 -07:00
|
|
|
control_mode = CONTROL_MODE_NONE;
|
2014-01-09 06:20:26 -08:00
|
|
|
last_current_sample = 0.0;
|
2014-04-02 02:54:21 -07:00
|
|
|
last_current_sample_filtered = 0.0;
|
2017-09-04 12:12:43 -07:00
|
|
|
switching_frequency_now = conf->m_bldc_f_sw_max;
|
2014-05-05 11:39:25 -07:00
|
|
|
ignore_iterations = 0;
|
2014-04-12 12:59:33 -07:00
|
|
|
curr_samp_volt = 0;
|
2014-07-27 10:40:38 -07:00
|
|
|
slow_ramping_cycles = 0;
|
|
|
|
has_commutated = 0;
|
2014-08-01 06:43:36 -07:00
|
|
|
memset((void*)&rpm_dep, 0, sizeof(rpm_dep));
|
2014-09-12 14:37:33 -07:00
|
|
|
cycle_integrator_sum = 0.0;
|
|
|
|
cycle_integrator_iterations = 0.0;
|
2014-04-04 14:19:11 -07:00
|
|
|
pwm_cycles_sum = 0.0;
|
2015-08-26 14:12:39 -07:00
|
|
|
pwm_cycles = 0;
|
2014-04-04 14:19:11 -07:00
|
|
|
last_pwm_cycles_sum = 0.0;
|
|
|
|
memset((float*)last_pwm_cycles_sums, 0, sizeof(last_pwm_cycles_sums));
|
2014-10-24 14:04:10 -07:00
|
|
|
dccal_done = false;
|
2016-06-27 08:29:09 -07:00
|
|
|
memset((void*)hall_detect_table, 0, sizeof(hall_detect_table[0][0]) * 8 * 7);
|
2015-06-19 14:38:46 -07:00
|
|
|
update_sensor_mode();
|
2017-09-04 12:12:43 -07:00
|
|
|
comm_mode_next = conf->comm_mode;
|
2019-02-18 10:30:19 -08:00
|
|
|
m_pll_phase = 0.0;
|
|
|
|
m_pll_speed = 0.0;
|
2019-03-10 06:57:42 -07:00
|
|
|
rpm_timer_start = 0;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
mcpwm_init_hall_table((int8_t*)conf->hall_table);
|
2014-06-30 00:50:05 -07:00
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
// Create KV FIR filter
|
2014-04-05 14:27:51 -07:00
|
|
|
filter_create_fir_lowpass((float*)kv_fir_coeffs, KV_FIR_FCUT, KV_FIR_TAPS_BITS, 1);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-03-29 05:15:09 -07:00
|
|
|
// Create amplitude FIR filter
|
2014-04-05 14:27:51 -07:00
|
|
|
filter_create_fir_lowpass((float*)amp_fir_coeffs, AMP_FIR_FCUT, AMP_FIR_TAPS_BITS, 1);
|
2014-03-29 05:15:09 -07:00
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
// Create current FIR filter
|
2014-04-05 14:27:51 -07:00
|
|
|
filter_create_fir_lowpass((float*)current_fir_coeffs, CURR_FIR_FCUT, CURR_FIR_TAPS_BITS, 1);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-08-06 13:26:53 -07:00
|
|
|
TIM_DeInit(TIM1);
|
|
|
|
TIM_DeInit(TIM8);
|
|
|
|
TIM1->CNT = 0;
|
|
|
|
TIM8->CNT = 0;
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
// TIM1 clock enable
|
|
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
|
|
|
|
|
|
|
|
// Time Base configuration
|
|
|
|
TIM_TimeBaseStructure.TIM_Prescaler = 0;
|
|
|
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
2014-08-05 13:24:13 -07:00
|
|
|
TIM_TimeBaseStructure.TIM_Period = SYSTEM_CORE_CLOCK / (int)switching_frequency_now;
|
2014-01-09 06:20:26 -08:00
|
|
|
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
|
|
|
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
|
|
|
|
|
|
|
|
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
|
|
|
|
|
|
|
|
// Channel 1, 2 and 3 Configuration in PWM mode
|
|
|
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
|
|
|
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
|
2015-12-19 12:24:46 -08:00
|
|
|
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
|
2014-03-30 11:04:35 -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
|
2014-01-09 06:20:26 -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
|
2014-01-09 06:20:26 -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);
|
|
|
|
|
2014-04-01 10:26:46 -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);
|
2014-01-09 06:20:26 -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;
|
2014-01-09 06:20:26 -08:00
|
|
|
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
|
2019-01-30 19:49:26 -08:00
|
|
|
TIM_BDTRInitStructure.TIM_DeadTime = conf_general_calculate_deadtime(HW_DEAD_TIME_NSEC, SYSTEM_CORE_CLOCK);
|
2014-01-09 06:20:26 -08:00
|
|
|
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
|
|
|
|
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
|
2014-04-12 12:59:33 -07:00
|
|
|
TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
TIM_BDTRConfig(TIM1, &TIM_BDTRInitStructure);
|
|
|
|
TIM_CCPreloadControl(TIM1, ENABLE);
|
2014-04-01 10:26:46 -07:00
|
|
|
TIM_ARRPreloadConfig(TIM1, ENABLE);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
/*
|
|
|
|
* ADC!
|
|
|
|
*/
|
|
|
|
ADC_CommonInitTypeDef ADC_CommonInitStructure;
|
|
|
|
DMA_InitTypeDef DMA_InitStructure;
|
|
|
|
ADC_InitTypeDef ADC_InitStructure;
|
|
|
|
|
|
|
|
// Clock
|
|
|
|
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC, ENABLE);
|
|
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | RCC_APB2Periph_ADC3, ENABLE);
|
|
|
|
|
|
|
|
dmaStreamAllocate(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)),
|
2019-12-19 07:55:38 -08:00
|
|
|
5,
|
2014-01-09 06:20:26 -08:00
|
|
|
(stm32_dmaisr_t)mcpwm_adc_int_handler,
|
|
|
|
(void *)0);
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
// DMA for the ADC
|
2014-01-09 06:20:26 -08:00
|
|
|
DMA_InitStructure.DMA_Channel = DMA_Channel_0;
|
|
|
|
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&ADC_Value;
|
2014-08-01 06:43:36 -07:00
|
|
|
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC->CDR;
|
2014-01-09 06:20:26 -08:00
|
|
|
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
|
2014-04-12 12:59:33 -07:00
|
|
|
DMA_InitStructure.DMA_BufferSize = HW_ADC_CHANNELS;
|
2014-01-09 06:20:26 -08:00
|
|
|
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
|
|
|
|
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
|
|
|
|
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
|
|
|
|
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
|
|
|
|
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
|
|
|
|
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
|
|
|
|
DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
|
|
|
|
DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
|
|
|
|
DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
|
|
|
|
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
|
|
|
|
DMA_Init(DMA2_Stream4, &DMA_InitStructure);
|
|
|
|
|
|
|
|
// DMA2_Stream0 enable
|
|
|
|
DMA_Cmd(DMA2_Stream4, ENABLE);
|
|
|
|
|
|
|
|
// Enable transfer complete interrupt
|
|
|
|
DMA_ITConfig(DMA2_Stream4, DMA_IT_TC, ENABLE);
|
|
|
|
|
|
|
|
// ADC Common Init
|
2014-09-12 04:03:34 -07:00
|
|
|
// Note that the ADC is running at 42MHz, which is higher than the
|
|
|
|
// specified 36MHz in the data sheet, but it works.
|
2014-01-09 06:20:26 -08:00
|
|
|
ADC_CommonInitStructure.ADC_Mode = ADC_TripleMode_RegSimult;
|
|
|
|
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2;
|
|
|
|
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1;
|
|
|
|
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
|
|
|
|
ADC_CommonInit(&ADC_CommonInitStructure);
|
|
|
|
|
|
|
|
// Channel-specific settings
|
|
|
|
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
|
|
|
|
ADC_InitStructure.ADC_ScanConvMode = ENABLE;
|
|
|
|
ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
|
|
|
|
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Falling;
|
|
|
|
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T8_CC1;
|
|
|
|
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
|
2014-04-12 12:59:33 -07:00
|
|
|
ADC_InitStructure.ADC_NbrOfConversion = HW_ADC_NBR_CONV;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
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);
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
// Enable Vrefint channel
|
|
|
|
ADC_TempSensorVrefintCmd(ENABLE);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
// Enable DMA request after last transfer (Multi-ADC mode)
|
|
|
|
ADC_MultiModeDMARequestAfterLastTransferCmd(ENABLE);
|
|
|
|
|
|
|
|
// Injected channels for current measurement at end of cycle
|
|
|
|
ADC_ExternalTrigInjectedConvConfig(ADC1, ADC_ExternalTrigInjecConv_T1_CC4);
|
|
|
|
ADC_ExternalTrigInjectedConvConfig(ADC2, ADC_ExternalTrigInjecConv_T8_CC2);
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
ADC_ExternalTrigInjectedConvConfig(ADC3, ADC_ExternalTrigInjecConv_T8_CC3);
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
ADC_ExternalTrigInjectedConvEdgeConfig(ADC1, ADC_ExternalTrigInjecConvEdge_Falling);
|
|
|
|
ADC_ExternalTrigInjectedConvEdgeConfig(ADC2, ADC_ExternalTrigInjecConvEdge_Falling);
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
ADC_ExternalTrigInjectedConvEdgeConfig(ADC3, ADC_ExternalTrigInjecConvEdge_Falling);
|
|
|
|
#endif
|
2016-11-04 07:18:34 -07:00
|
|
|
ADC_InjectedSequencerLengthConfig(ADC1, HW_ADC_INJ_CHANNELS);
|
|
|
|
ADC_InjectedSequencerLengthConfig(ADC2, HW_ADC_INJ_CHANNELS);
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
2016-11-04 07:18:34 -07:00
|
|
|
ADC_InjectedSequencerLengthConfig(ADC3, HW_ADC_INJ_CHANNELS);
|
2016-06-27 08:29:09 -07:00
|
|
|
#endif
|
2015-08-23 09:26:05 -07:00
|
|
|
|
|
|
|
hw_setup_adc_channels();
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
// Interrupt
|
|
|
|
ADC_ITConfig(ADC1, ADC_IT_JEOC, ENABLE);
|
2015-10-08 14:09:39 -07:00
|
|
|
nvicEnableVector(ADC_IRQn, 6);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
// Enable ADC1
|
|
|
|
ADC_Cmd(ADC1, ENABLE);
|
|
|
|
|
|
|
|
// Enable ADC2
|
|
|
|
ADC_Cmd(ADC2, ENABLE);
|
|
|
|
|
|
|
|
// Enable ADC3
|
|
|
|
ADC_Cmd(ADC3, ENABLE);
|
|
|
|
|
|
|
|
// ------------- Timer8 for ADC sampling ------------- //
|
|
|
|
// Time Base configuration
|
|
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
|
|
|
|
|
|
|
|
TIM_TimeBaseStructure.TIM_Prescaler = 0;
|
|
|
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
2014-09-12 04:03:34 -07:00
|
|
|
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
|
2014-01-09 06:20:26 -08:00
|
|
|
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
|
|
|
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
|
|
|
|
TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
|
|
|
|
|
|
|
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
|
|
|
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
|
2014-03-30 05:12:27 -07:00
|
|
|
TIM_OCInitStructure.TIM_Pulse = 500;
|
2014-01-09 06:20:26 -08:00
|
|
|
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
|
|
|
|
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
|
|
|
|
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
|
|
|
|
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set;
|
|
|
|
TIM_OC1Init(TIM8, &TIM_OCInitStructure);
|
2014-04-01 10:26:46 -07:00
|
|
|
TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
|
2014-01-09 06:20:26 -08:00
|
|
|
TIM_OC2Init(TIM8, &TIM_OCInitStructure);
|
2014-04-01 10:26:46 -07:00
|
|
|
TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);
|
2016-06-27 08:29:09 -07:00
|
|
|
TIM_OC3Init(TIM8, &TIM_OCInitStructure);
|
|
|
|
TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable);
|
2014-04-01 10:26:46 -07:00
|
|
|
|
|
|
|
TIM_ARRPreloadConfig(TIM8, ENABLE);
|
|
|
|
TIM_CCPreloadControl(TIM8, ENABLE);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
// PWM outputs have to be enabled in order to trigger ADC on CCx
|
|
|
|
TIM_CtrlPWMOutputs(TIM8, ENABLE);
|
|
|
|
|
|
|
|
// TIM1 Master and TIM8 slave
|
2014-09-12 04:03:34 -07:00
|
|
|
TIM_SelectOutputTrigger(TIM1, TIM_TRGOSource_Update);
|
2014-01-09 06:20:26 -08:00
|
|
|
TIM_SelectMasterSlaveMode(TIM1, TIM_MasterSlaveMode_Enable);
|
|
|
|
TIM_SelectInputTrigger(TIM8, TIM_TS_ITR0);
|
2014-09-12 04:03:34 -07:00
|
|
|
TIM_SelectSlaveMode(TIM8, TIM_SlaveMode_Reset);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-09-12 04:03:34 -07:00
|
|
|
// Enable TIM1 and TIM8
|
2014-01-09 06:20:26 -08:00
|
|
|
TIM_Cmd(TIM1, ENABLE);
|
2014-09-12 04:03:34 -07:00
|
|
|
TIM_Cmd(TIM8, ENABLE);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
// Main Output Enable
|
|
|
|
TIM_CtrlPWMOutputs(TIM1, ENABLE);
|
|
|
|
|
2014-03-30 05:12:27 -07:00
|
|
|
// ADC sampling locations
|
2014-05-05 11:39:25 -07:00
|
|
|
stop_pwm_hw();
|
2014-09-12 06:52:16 -07:00
|
|
|
mc_timer_struct timer_tmp;
|
|
|
|
timer_tmp.top = TIM1->ARR;
|
|
|
|
timer_tmp.duty = TIM1->ARR / 2;
|
|
|
|
update_adc_sample_pos(&timer_tmp);
|
|
|
|
set_next_timer_settings(&timer_tmp);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-08-29 21:00:48 -07:00
|
|
|
utils_sys_unlock_cnt();
|
2014-04-12 12:59:33 -07:00
|
|
|
|
2019-02-18 10:30:19 -08:00
|
|
|
CURRENT_FILTER_ON();
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
// Calibrate current offset
|
|
|
|
ENABLE_GATE();
|
2014-08-02 05:07:56 -07:00
|
|
|
DCCAL_OFF();
|
2014-06-29 05:23:39 -07:00
|
|
|
do_dc_cal();
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-08-01 06:43:36 -07:00
|
|
|
// Start threads
|
2015-12-08 12:01:23 -08:00
|
|
|
timer_thd_stop = false;
|
|
|
|
rpm_thd_stop = false;
|
2014-03-29 05:15:09 -07:00
|
|
|
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
2014-08-01 06:43:36 -07:00
|
|
|
chThdCreateStatic(rpm_thread_wa, sizeof(rpm_thread_wa), NORMALPRIO, rpm_thread, NULL);
|
2014-05-08 14:44:27 -07:00
|
|
|
|
2019-01-24 07:19:44 -08:00
|
|
|
// Check if the system has resumed from IWDG reset
|
2019-03-01 12:36:58 -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-03-01 12:36:58 -08:00
|
|
|
}
|
2019-01-24 07:19:44 -08:00
|
|
|
|
2014-10-16 08:52:29 -07:00
|
|
|
// Reset tachometers again
|
|
|
|
tachometer = 0;
|
|
|
|
tachometer_abs = 0;
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
|
|
init_done = true;
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
void mcpwm_deinit(void) {
|
2019-02-18 10:30:19 -08:00
|
|
|
if (!init_done) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
init_done = false;
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
timer_thd_stop = true;
|
|
|
|
rpm_thd_stop = true;
|
|
|
|
|
|
|
|
while (timer_thd_stop || rpm_thd_stop) {
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
}
|
|
|
|
|
|
|
|
TIM_DeInit(TIM1);
|
|
|
|
TIM_DeInit(TIM8);
|
|
|
|
ADC_DeInit();
|
|
|
|
DMA_DeInit(DMA2_Stream4);
|
|
|
|
nvicDisableVector(ADC_IRQn);
|
|
|
|
dmaStreamRelease(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)));
|
2014-09-14 14:08:22 -07:00
|
|
|
}
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
bool mcpwm_init_done(void) {
|
|
|
|
return init_done;
|
|
|
|
}
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
void mcpwm_set_configuration(volatile mc_configuration *configuration) {
|
2014-09-14 14:08:22 -07:00
|
|
|
// Stop everything first to be safe
|
|
|
|
control_mode = CONTROL_MODE_NONE;
|
|
|
|
stop_pwm_ll();
|
|
|
|
|
|
|
|
utils_sys_lock_cnt();
|
2015-12-08 12:01:23 -08:00
|
|
|
conf = configuration;
|
2017-09-04 12:12:43 -07:00
|
|
|
comm_mode_next = conf->comm_mode;
|
2015-12-08 12:01:23 -08:00
|
|
|
mcpwm_init_hall_table((int8_t*)conf->hall_table);
|
2015-06-26 12:45:28 -07:00
|
|
|
update_sensor_mode();
|
2014-09-14 14:08:22 -07:00
|
|
|
utils_sys_unlock_cnt();
|
|
|
|
}
|
|
|
|
|
2014-06-29 05:23:39 -07:00
|
|
|
/**
|
|
|
|
* Initialize the hall sensor lookup table
|
|
|
|
*
|
2015-06-19 14:38:46 -07:00
|
|
|
* @param table
|
|
|
|
* The commutations corresponding to the hall sensor states in the forward direction-
|
2014-06-29 05:23:39 -07:00
|
|
|
*/
|
2015-06-19 14:38:46 -07:00
|
|
|
void mcpwm_init_hall_table(int8_t *table) {
|
2016-02-25 11:48:17 -08:00
|
|
|
const int fwd_to_rev[7] = {-1,1,6,5,4,3,2};
|
2015-06-19 14:38:46 -07:00
|
|
|
|
|
|
|
for (int i = 0;i < 8;i++) {
|
|
|
|
hall_to_phase_table[8 + i] = table[i];
|
|
|
|
int ind_now = hall_to_phase_table[8 + i];
|
2014-06-30 00:50:05 -07:00
|
|
|
|
2015-06-19 14:38:46 -07:00
|
|
|
if (ind_now < 1) {
|
|
|
|
hall_to_phase_table[i] = ind_now;
|
|
|
|
continue;
|
|
|
|
}
|
2014-06-29 05:23:39 -07:00
|
|
|
|
2016-02-25 11:48:17 -08:00
|
|
|
hall_to_phase_table[i] = fwd_to_rev[ind_now];
|
2014-06-29 05:23:39 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
static void do_dc_cal(void) {
|
|
|
|
DCCAL_ON();
|
2017-09-06 12:13:28 -07:00
|
|
|
|
|
|
|
// Wait max 5 seconds
|
|
|
|
int cnt = 0;
|
|
|
|
while(IS_DRV_FAULT()){
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
cnt++;
|
|
|
|
if (cnt > 5000) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
2014-06-29 05:23:39 -07:00
|
|
|
chThdSleepMilliseconds(1000);
|
|
|
|
curr0_sum = 0;
|
|
|
|
curr1_sum = 0;
|
2016-06-27 08:29:09 -07:00
|
|
|
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
curr2_sum = 0;
|
|
|
|
#endif
|
|
|
|
|
2014-06-29 05:23:39 -07:00
|
|
|
curr_start_samples = 0;
|
2014-08-02 05:07:56 -07:00
|
|
|
while(curr_start_samples < 4000) {};
|
2014-06-29 05:23:39 -07:00
|
|
|
curr0_offset = curr0_sum / curr_start_samples;
|
|
|
|
curr1_offset = curr1_sum / curr_start_samples;
|
2016-06-27 08:29:09 -07:00
|
|
|
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
curr2_offset = curr2_sum / curr_start_samples;
|
|
|
|
#endif
|
|
|
|
|
2014-06-29 05:23:39 -07:00
|
|
|
DCCAL_OFF();
|
2014-10-24 14:04:10 -07:00
|
|
|
dccal_done = true;
|
2014-06-29 05:23:39 -07:00
|
|
|
}
|
|
|
|
|
2019-02-18 10:30:19 -08:00
|
|
|
static void pll_run(float phase, float dt, volatile float *phase_var,
|
|
|
|
volatile float *speed_var) {
|
|
|
|
UTILS_NAN_ZERO(*phase_var);
|
|
|
|
float delta_theta = phase - *phase_var;
|
|
|
|
utils_norm_angle_rad(&delta_theta);
|
|
|
|
UTILS_NAN_ZERO(*speed_var);
|
|
|
|
*phase_var += (*speed_var + conf->foc_pll_kp * delta_theta) * dt;
|
|
|
|
utils_norm_angle_rad((float*)phase_var);
|
|
|
|
*speed_var += conf->foc_pll_ki * delta_theta * dt;
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
|
|
|
* Use duty cycle control. Absolute values less than MCPWM_MIN_DUTY_CYCLE will
|
|
|
|
* stop the motor.
|
|
|
|
*
|
|
|
|
* @param dutyCycle
|
|
|
|
* The duty cycle to use.
|
|
|
|
*/
|
2014-01-09 06:20:26 -08:00
|
|
|
void mcpwm_set_duty(float dutyCycle) {
|
2014-03-30 15:51:59 -07:00
|
|
|
control_mode = CONTROL_MODE_DUTY;
|
|
|
|
set_duty_cycle_hl(dutyCycle);
|
|
|
|
}
|
|
|
|
|
2015-10-03 16:43:26 -07: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_set_duty_noramp(float dutyCycle) {
|
|
|
|
control_mode = CONTROL_MODE_DUTY;
|
|
|
|
|
|
|
|
if (state != MC_STATE_RUNNING) {
|
|
|
|
set_duty_cycle_hl(dutyCycle);
|
|
|
|
} else {
|
|
|
|
dutycycle_set = dutyCycle;
|
|
|
|
dutycycle_now = dutyCycle;
|
|
|
|
set_duty_cycle_ll(dutyCycle);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
|
|
|
* Use PID rpm control. Note that this value has to be multiplied by half of
|
|
|
|
* the number of motor poles.
|
|
|
|
*
|
|
|
|
* @param rpm
|
|
|
|
* The electrical RPM goal value to use.
|
|
|
|
*/
|
2014-03-30 15:51:59 -07:00
|
|
|
void mcpwm_set_pid_speed(float rpm) {
|
|
|
|
control_mode = CONTROL_MODE_SPEED;
|
|
|
|
speed_pid_set_rpm = rpm;
|
|
|
|
}
|
|
|
|
|
2015-04-26 15:02:32 -07: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_set_pid_pos(float pos) {
|
|
|
|
control_mode = CONTROL_MODE_POS;
|
|
|
|
pos_pid_set_pos = pos;
|
|
|
|
|
|
|
|
if (state != MC_STATE_RUNNING) {
|
2015-12-08 12:01:23 -08:00
|
|
|
set_duty_cycle_hl(conf->l_min_duty);
|
2015-04-26 15:02:32 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
|
|
|
* Use current control and specify a goal current to use. The sign determines
|
|
|
|
* the direction of the torque. Absolute values less than
|
2015-12-08 12:01:23 -08:00
|
|
|
* conf->cc_min_current will release the motor.
|
2014-04-02 02:54:21 -07:00
|
|
|
*
|
|
|
|
* @param current
|
|
|
|
* The current to use.
|
|
|
|
*/
|
2014-03-30 15:51:59 -07:00
|
|
|
void mcpwm_set_current(float current) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(current) < conf->cc_min_current) {
|
2014-04-12 12:59:33 -07:00
|
|
|
control_mode = CONTROL_MODE_NONE;
|
2014-05-05 11:39:25 -07:00
|
|
|
stop_pwm_ll();
|
2014-03-30 15:51:59 -07:00
|
|
|
return;
|
2014-03-29 05:15:09 -07:00
|
|
|
}
|
|
|
|
|
2019-02-18 10:30:19 -08:00
|
|
|
utils_truncate_number(¤t, -conf->l_current_max * conf->l_current_max_scale,
|
|
|
|
conf->l_current_max * conf->l_current_max_scale);
|
2014-01-18 13:10:10 -08:00
|
|
|
|
2014-03-30 15:51:59 -07:00
|
|
|
control_mode = CONTROL_MODE_CURRENT;
|
|
|
|
current_set = current;
|
2014-03-29 05:15:09 -07:00
|
|
|
|
2014-03-30 15:51:59 -07:00
|
|
|
if (state != MC_STATE_RUNNING) {
|
2015-12-08 12:01:23 -08:00
|
|
|
set_duty_cycle_hl(SIGN(current) * conf->l_min_duty);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2022-01-19 09:52:45 -08:00
|
|
|
void mcpwm_release_motor(void) {
|
|
|
|
current_set = 0.0;
|
|
|
|
control_mode = CONTROL_MODE_NONE;
|
|
|
|
stop_pwm_ll();
|
|
|
|
}
|
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
/**
|
|
|
|
* Brake the motor with a desired current. Absolute values less than
|
2015-12-08 12:01:23 -08:00
|
|
|
* conf->cc_min_current will release the motor.
|
2014-07-27 10:40:38 -07:00
|
|
|
*
|
|
|
|
* @param current
|
|
|
|
* The current to use. Positive and negative values give the same effect.
|
|
|
|
*/
|
|
|
|
void mcpwm_set_brake_current(float current) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(current) < conf->cc_min_current) {
|
2014-07-27 10:40:38 -07:00
|
|
|
control_mode = CONTROL_MODE_NONE;
|
|
|
|
stop_pwm_ll();
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2019-02-18 10:30:19 -08:00
|
|
|
utils_truncate_number(¤t, -fabsf(conf->lo_current_min), fabsf(conf->lo_current_min));
|
2014-07-27 10:40:38 -07:00
|
|
|
|
|
|
|
control_mode = CONTROL_MODE_CURRENT_BRAKE;
|
|
|
|
current_set = current;
|
|
|
|
|
2015-01-09 07:32:22 -08:00
|
|
|
if (state != MC_STATE_RUNNING && state != MC_STATE_FULL_BRAKE) {
|
2014-07-27 10:40:38 -07:00
|
|
|
// In case the motor is already spinning, set the state to running
|
|
|
|
// so that it can be ramped down before the full brake is applied.
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_DC) {
|
2015-04-12 09:23:17 -07:00
|
|
|
if (fabsf(dutycycle_now) > 0.1) {
|
|
|
|
state = MC_STATE_RUNNING;
|
|
|
|
} else {
|
|
|
|
full_brake_ll();
|
|
|
|
}
|
2014-07-27 10:40:38 -07:00
|
|
|
} else {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(rpm_now) > conf->l_max_erpm_fbrake) {
|
2015-04-12 09:23:17 -07:00
|
|
|
state = MC_STATE_RUNNING;
|
|
|
|
} else {
|
|
|
|
full_brake_ll();
|
|
|
|
}
|
2014-07-27 10:40:38 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
|
|
|
* Get the electrical position (or commutation step) of the motor.
|
|
|
|
*
|
|
|
|
* @return
|
|
|
|
* The current commutation step. Range [1 6]
|
|
|
|
*/
|
2014-01-09 06:20:26 -08:00
|
|
|
int mcpwm_get_comm_step(void) {
|
|
|
|
return comm_step;
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
float mcpwm_get_duty_cycle_set(void) {
|
2014-01-09 06:20:26 -08:00
|
|
|
return dutycycle_set;
|
|
|
|
}
|
|
|
|
|
2014-04-02 11:23:27 -07:00
|
|
|
float mcpwm_get_duty_cycle_now(void) {
|
2014-04-02 02:54:21 -07:00
|
|
|
return dutycycle_now;
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2014-08-05 13:24:13 -07:00
|
|
|
/**
|
|
|
|
* Get the current switching frequency.
|
|
|
|
*
|
|
|
|
* @return
|
|
|
|
* The switching frequency in Hz.
|
|
|
|
*/
|
|
|
|
float mcpwm_get_switching_frequency_now(void) {
|
|
|
|
return switching_frequency_now;
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07: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_get_rpm(void) {
|
2019-02-18 10:30:19 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_DC) {
|
2021-10-13 09:13:51 -07:00
|
|
|
return RADPS2RPM_f(m_pll_speed);
|
2019-02-18 10:30:19 -08:00
|
|
|
} else {
|
|
|
|
return direction ? rpm_now : -rpm_now;
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
mc_state mcpwm_get_state(void) {
|
|
|
|
return state;
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
|
|
|
* Calculate the KV (RPM per volt) value for the motor. This function has to
|
|
|
|
* be used while the motor is moving. Note that the return value has to be
|
|
|
|
* divided by half the number of motor poles.
|
|
|
|
*
|
|
|
|
* @return
|
|
|
|
* The KV value.
|
|
|
|
*/
|
|
|
|
float mcpwm_get_kv(void) {
|
2021-04-27 08:48:15 -07:00
|
|
|
return rpm_now / (mc_interface_get_input_voltage_filtered() * fabsf(dutycycle_now));
|
2014-04-02 02:54:21 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* Calculate the FIR-filtered KV (RPM per volt) value for the motor. This
|
|
|
|
* function has to be used while the motor is moving. Note that the return
|
|
|
|
* value has to be divided by half the number of motor poles.
|
|
|
|
*
|
|
|
|
* @return
|
|
|
|
* The filtered KV value.
|
|
|
|
*/
|
2014-01-09 06:20:26 -08:00
|
|
|
float mcpwm_get_kv_filtered(void) {
|
|
|
|
float value = filter_run_fir_iteration((float*)kv_fir_samples,
|
|
|
|
(float*)kv_fir_coeffs, KV_FIR_TAPS_BITS, kv_fir_index);
|
|
|
|
|
|
|
|
return value;
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
2014-12-20 03:52:38 -08:00
|
|
|
* Get the motor current. The sign of this value will
|
|
|
|
* represent whether the motor is drawing (positive) or generating
|
|
|
|
* (negative) current.
|
2014-04-02 02:54:21 -07:00
|
|
|
*
|
|
|
|
* @return
|
|
|
|
* The motor current.
|
|
|
|
*/
|
2014-01-09 06:20:26 -08:00
|
|
|
float mcpwm_get_tot_current(void) {
|
2015-08-23 09:26:05 -07:00
|
|
|
return last_current_sample;
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
2014-12-20 03:52:38 -08:00
|
|
|
* Get the FIR-filtered motor current. The sign of this value will
|
|
|
|
* represent whether the motor is drawing (positive) or generating
|
|
|
|
* (negative) current.
|
2014-04-02 02:54:21 -07:00
|
|
|
*
|
|
|
|
* @return
|
|
|
|
* The filtered motor current.
|
|
|
|
*/
|
|
|
|
float mcpwm_get_tot_current_filtered(void) {
|
2015-08-23 09:26:05 -07:00
|
|
|
return last_current_sample_filtered;
|
2014-04-02 02:54:21 -07:00
|
|
|
}
|
|
|
|
|
2014-12-20 03:52:38 -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_get_tot_current_directional(void) {
|
|
|
|
const float retval = mcpwm_get_tot_current();
|
|
|
|
return dutycycle_now > 0.0 ? retval : -retval;
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* 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_get_tot_current_directional_filtered(void) {
|
|
|
|
const float retval = mcpwm_get_tot_current_filtered();
|
|
|
|
return dutycycle_now > 0.0 ? retval : -retval;
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
|
|
|
* Get the input current to the motor controller.
|
|
|
|
*
|
|
|
|
* @return
|
|
|
|
* The input current.
|
|
|
|
*/
|
2014-01-09 06:20:26 -08:00
|
|
|
float mcpwm_get_tot_current_in(void) {
|
2014-03-21 08:13:50 -07:00
|
|
|
return mcpwm_get_tot_current() * fabsf(dutycycle_now);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
|
|
|
* Get the FIR-filtered input current to the motor controller.
|
|
|
|
*
|
|
|
|
* @return
|
|
|
|
* The filtered input current.
|
|
|
|
*/
|
|
|
|
float mcpwm_get_tot_current_in_filtered(void) {
|
|
|
|
return mcpwm_get_tot_current_filtered() * fabsf(dutycycle_now);
|
|
|
|
}
|
|
|
|
|
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 motor 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).
|
|
|
|
*/
|
|
|
|
int mcpwm_set_tachometer_value(int steps)
|
|
|
|
{
|
|
|
|
int val = tachometer;
|
|
|
|
|
|
|
|
tachometer = steps;
|
|
|
|
|
|
|
|
return val;
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07: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
|
2014-10-16 00:42:26 -07:00
|
|
|
* If true, the tachometer counter will be reset after this call.
|
2014-04-02 02:54:21 -07:00
|
|
|
*
|
|
|
|
* @return
|
|
|
|
* The tachometer value in motor steps. The number of motor revolutions will
|
|
|
|
* be this number divided by (3 * MOTOR_POLE_NUMBER).
|
|
|
|
*/
|
2014-10-16 00:42:26 -07:00
|
|
|
int mcpwm_get_tachometer_value(bool reset) {
|
2014-01-09 06:20:26 -08:00
|
|
|
int val = tachometer;
|
|
|
|
|
|
|
|
if (reset) {
|
|
|
|
tachometer = 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
return val;
|
|
|
|
}
|
|
|
|
|
2014-10-16 00:42:26 -07:00
|
|
|
/**
|
|
|
|
* 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_get_tachometer_abs_value(bool reset) {
|
|
|
|
int val = tachometer_abs;
|
|
|
|
|
|
|
|
if (reset) {
|
|
|
|
tachometer_abs = 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
return val;
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
2015-12-08 12:01:23 -08:00
|
|
|
* Switch off all FETs.
|
2014-10-16 00:42:26 -07:00
|
|
|
*/
|
2015-12-08 12:01:23 -08:00
|
|
|
void mcpwm_stop_pwm(void) {
|
|
|
|
control_mode = CONTROL_MODE_NONE;
|
|
|
|
stop_pwm_ll();
|
2014-10-16 00:42:26 -07:00
|
|
|
}
|
|
|
|
|
2014-05-05 11:39:25 -07:00
|
|
|
static void stop_pwm_ll(void) {
|
|
|
|
state = MC_STATE_OFF;
|
|
|
|
ignore_iterations = MCPWM_CMD_STOP_TIME;
|
|
|
|
stop_pwm_hw();
|
|
|
|
}
|
|
|
|
|
|
|
|
static void stop_pwm_hw(void) {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR();
|
|
|
|
#endif
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_ForcedAction_InActive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_ForcedAction_InActive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_ForcedAction_InActive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
|
2014-03-16 08:09:23 -07:00
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
set_switching_frequency(conf->m_bldc_f_sw_max);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2014-05-05 11:39:25 -07:00
|
|
|
static void full_brake_ll(void) {
|
2014-01-09 06:20:26 -08:00
|
|
|
state = MC_STATE_FULL_BRAKE;
|
2014-05-05 11:39:25 -07:00
|
|
|
ignore_iterations = MCPWM_CMD_STOP_TIME;
|
|
|
|
full_brake_hw();
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-05-05 11:39:25 -07:00
|
|
|
static void full_brake_hw(void) {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
ENABLE_BR();
|
|
|
|
#endif
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
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_Enable);
|
|
|
|
|
|
|
|
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_Enable);
|
|
|
|
|
|
|
|
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_Enable);
|
|
|
|
|
|
|
|
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
|
2014-03-16 08:09:23 -07:00
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
set_switching_frequency(conf->m_bldc_f_sw_max);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
|
|
|
* High-level duty cycle setter. Will set the ramping goal of the duty cycle.
|
|
|
|
* If motor is not running, it will be started in different ways depending on
|
|
|
|
* whether it is moving or not.
|
|
|
|
*
|
|
|
|
* @param dutyCycle
|
|
|
|
* The duty cycle in the range [-MCPWM_MAX_DUTY_CYCLE MCPWM_MAX_DUTY_CYCLE]
|
|
|
|
* If the absolute value of the duty cycle is less than MCPWM_MIN_DUTY_CYCLE,
|
2014-04-12 12:59:33 -07:00
|
|
|
* the motor phases will be shorted to brake the motor.
|
2014-04-02 02:54:21 -07:00
|
|
|
*/
|
2014-03-30 15:51:59 -07:00
|
|
|
static void set_duty_cycle_hl(float dutyCycle) {
|
2015-12-08 12:01:23 -08:00
|
|
|
utils_truncate_number(&dutyCycle, -conf->l_max_duty, conf->l_max_duty);
|
2014-03-30 15:51:59 -07:00
|
|
|
|
|
|
|
if (state == MC_STATE_DETECTING) {
|
2014-05-05 11:39:25 -07:00
|
|
|
stop_pwm_ll();
|
2014-04-12 12:59:33 -07:00
|
|
|
return;
|
2014-03-30 15:51:59 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
dutycycle_set = dutyCycle;
|
|
|
|
|
|
|
|
if (state != MC_STATE_RUNNING) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(dutyCycle) >= conf->l_min_duty) {
|
2014-07-27 10:40:38 -07:00
|
|
|
// dutycycle_now is updated by the back-emf detection. If the motor already
|
|
|
|
// is spinning, it will be non-zero.
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(dutycycle_now) < conf->l_min_duty) {
|
|
|
|
dutycycle_now = SIGN(dutyCycle) * conf->l_min_duty;
|
2014-03-30 15:51:59 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
set_duty_cycle_ll(dutycycle_now);
|
|
|
|
} else {
|
2014-05-05 11:39:25 -07:00
|
|
|
// In case the motor is already spinning, set the state to running
|
|
|
|
// so that it can be ramped down before the full brake is applied.
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_DC) {
|
2015-04-12 09:23:17 -07:00
|
|
|
if (fabsf(dutycycle_now) > 0.1) {
|
|
|
|
state = MC_STATE_RUNNING;
|
|
|
|
} else {
|
|
|
|
full_brake_ll();
|
|
|
|
}
|
2014-04-02 02:54:21 -07:00
|
|
|
} else {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(rpm_now) > conf->l_max_erpm_fbrake) {
|
2015-04-12 09:23:17 -07:00
|
|
|
state = MC_STATE_RUNNING;
|
|
|
|
} else {
|
|
|
|
full_brake_ll();
|
|
|
|
}
|
2014-04-02 02:54:21 -07:00
|
|
|
}
|
2014-03-30 15:51:59 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
|
|
|
* Low-level duty cycle setter. Will update the state of the application
|
|
|
|
* and the motor direction accordingly.
|
|
|
|
*
|
|
|
|
* This function should be used with care. Ramping together with current
|
|
|
|
* limiting should be used.
|
|
|
|
*
|
|
|
|
* @param dutyCycle
|
|
|
|
* The duty cycle in the range [-MCPWM_MAX_DUTY_CYCLE MCPWM_MAX_DUTY_CYCLE]
|
|
|
|
* If the absolute value of the duty cycle is less than MCPWM_MIN_DUTY_CYCLE,
|
|
|
|
* the motor will be switched off.
|
|
|
|
*/
|
2014-03-30 15:51:59 -07:00
|
|
|
static void set_duty_cycle_ll(float dutyCycle) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (dutyCycle >= conf->l_min_duty) {
|
2014-01-18 13:10:10 -08:00
|
|
|
direction = 1;
|
2015-12-08 12:01:23 -08:00
|
|
|
} else if (dutyCycle <= -conf->l_min_duty) {
|
2014-01-18 13:10:10 -08:00
|
|
|
dutyCycle = -dutyCycle;
|
|
|
|
direction = 0;
|
|
|
|
}
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
if (dutyCycle < conf->l_min_duty) {
|
2015-08-23 09:26:05 -07:00
|
|
|
float max_erpm_fbrake;
|
2017-09-04 12:12:43 -07:00
|
|
|
#if BLDC_SPEED_CONTROL_CURRENT
|
|
|
|
if (control_mode == CONTROL_MODE_CURRENT ||
|
|
|
|
control_mode == CONTROL_MODE_CURRENT_BRAKE ||
|
|
|
|
control_mode == CONTROL_MODE_SPEED) {
|
|
|
|
#else
|
2015-08-23 09:26:05 -07:00
|
|
|
if (control_mode == CONTROL_MODE_CURRENT || control_mode == CONTROL_MODE_CURRENT_BRAKE) {
|
2017-09-04 12:12:43 -07:00
|
|
|
#endif
|
2015-12-08 12:01:23 -08:00
|
|
|
max_erpm_fbrake = conf->l_max_erpm_fbrake_cc;
|
2015-08-23 09:26:05 -07:00
|
|
|
} else {
|
2015-12-08 12:01:23 -08:00
|
|
|
max_erpm_fbrake = conf->l_max_erpm_fbrake;
|
2015-08-23 09:26:05 -07:00
|
|
|
}
|
|
|
|
|
2014-01-18 13:10:10 -08:00
|
|
|
switch (state) {
|
2014-01-23 04:03:57 -08:00
|
|
|
case MC_STATE_RUNNING:
|
2015-08-23 09:26:05 -07:00
|
|
|
// TODO!!!
|
|
|
|
if (fabsf(rpm_now) > max_erpm_fbrake) {
|
2015-12-08 12:01:23 -08:00
|
|
|
dutyCycle = conf->l_min_duty;
|
2015-08-23 09:26:05 -07:00
|
|
|
} else {
|
|
|
|
full_brake_ll();
|
|
|
|
return;
|
|
|
|
}
|
2014-01-18 13:10:10 -08:00
|
|
|
break;
|
|
|
|
|
|
|
|
case MC_STATE_DETECTING:
|
2014-05-05 11:39:25 -07:00
|
|
|
stop_pwm_ll();
|
2015-08-23 09:26:05 -07:00
|
|
|
return;
|
2014-01-18 13:10:10 -08:00
|
|
|
break;
|
|
|
|
|
|
|
|
default:
|
2015-08-23 09:26:05 -07:00
|
|
|
return;
|
2014-01-18 13:10:10 -08:00
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
} else if (dutyCycle > conf->l_max_duty) {
|
|
|
|
dutyCycle = conf->l_max_duty;
|
2014-01-18 13:10:10 -08:00
|
|
|
}
|
|
|
|
|
2014-04-12 12:59:33 -07:00
|
|
|
set_duty_cycle_hw(dutyCycle);
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_DC) {
|
2015-04-12 09:23:17 -07:00
|
|
|
state = MC_STATE_RUNNING;
|
|
|
|
set_next_comm_step(comm_step);
|
|
|
|
commutate(1);
|
|
|
|
} else {
|
2015-06-19 14:38:46 -07:00
|
|
|
if (sensorless_now) {
|
2015-04-12 09:23:17 -07:00
|
|
|
if (state != MC_STATE_RUNNING) {
|
|
|
|
if (state == MC_STATE_OFF) {
|
2014-12-12 16:31:54 -08:00
|
|
|
state = MC_STATE_RUNNING;
|
2015-04-12 09:23:17 -07:00
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(rpm_now) < conf->sl_min_erpm) {
|
2015-04-12 09:23:17 -07:00
|
|
|
commutate(1);
|
|
|
|
}
|
|
|
|
} else if (state == MC_STATE_FULL_BRAKE) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(rpm_now) < conf->sl_min_erpm && mcpwm_get_tot_current_filtered() < conf->sl_max_fullbreak_current_dir_change) {
|
2015-04-12 09:23:17 -07:00
|
|
|
state = MC_STATE_RUNNING;
|
|
|
|
commutate(1);
|
|
|
|
}
|
2014-12-12 16:31:54 -08:00
|
|
|
}
|
2014-09-14 14:08:22 -07:00
|
|
|
}
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
|
|
|
if (state != MC_STATE_RUNNING) {
|
|
|
|
state = MC_STATE_RUNNING;
|
|
|
|
comm_step = mcpwm_read_hall_phase();
|
|
|
|
set_next_comm_step(comm_step);
|
|
|
|
commutate(1);
|
|
|
|
}
|
2014-08-17 06:55:59 -07:00
|
|
|
}
|
2014-01-18 13:10:10 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-04-02 02:54:21 -07:00
|
|
|
/**
|
2014-09-12 14:37:33 -07:00
|
|
|
* Lowest level (hardware) duty cycle setter. Will set the hardware timer to
|
2014-04-02 02:54:21 -07:00
|
|
|
* the specified duty cycle and update the ADC sampling positions.
|
|
|
|
*
|
|
|
|
* @param dutyCycle
|
2014-09-12 14:37:33 -07:00
|
|
|
* The duty cycle in the range [MCPWM_MIN_DUTY_CYCLE MCPWM_MAX_DUTY_CYCLE]
|
2014-04-02 02:54:21 -07:00
|
|
|
* (Only positive)
|
|
|
|
*/
|
2014-01-18 13:10:10 -08:00
|
|
|
static void set_duty_cycle_hw(float dutyCycle) {
|
2014-04-01 10:26:46 -07:00
|
|
|
mc_timer_struct timer_tmp;
|
2014-08-29 21:00:48 -07:00
|
|
|
|
|
|
|
utils_sys_lock_cnt();
|
2014-10-16 00:42:26 -07:00
|
|
|
timer_tmp = timer_struct;
|
2014-08-29 21:00:48 -07:00
|
|
|
utils_sys_unlock_cnt();
|
2014-04-01 10:26:46 -07:00
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
utils_truncate_number(&dutyCycle, conf->l_min_duty, conf->l_max_duty);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_DC) {
|
2017-09-04 12:12:43 -07:00
|
|
|
switching_frequency_now = conf->m_dc_f_sw;
|
2014-03-15 14:32:00 -07:00
|
|
|
} else {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (IS_DETECTING() || conf->pwm_mode == PWM_MODE_BIPOLAR) {
|
2017-09-04 12:12:43 -07:00
|
|
|
switching_frequency_now = conf->m_bldc_f_sw_max;
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
2017-09-04 12:12:43 -07:00
|
|
|
switching_frequency_now = (float)conf->m_bldc_f_sw_min * (1.0 - fabsf(dutyCycle)) +
|
|
|
|
conf->m_bldc_f_sw_max * fabsf(dutyCycle);
|
2015-04-12 09:23:17 -07:00
|
|
|
}
|
2014-03-15 14:32:00 -07:00
|
|
|
}
|
|
|
|
|
2014-08-05 13:24:13 -07:00
|
|
|
timer_tmp.top = SYSTEM_CORE_CLOCK / (int)switching_frequency_now;
|
2016-11-06 11:28:07 -08:00
|
|
|
|
|
|
|
if (conf->motor_type == MOTOR_TYPE_BLDC && conf->pwm_mode == PWM_MODE_BIPOLAR && !IS_DETECTING()) {
|
|
|
|
timer_tmp.duty = (uint16_t) (((float) timer_tmp.top / 2.0) * dutyCycle
|
|
|
|
+ ((float) timer_tmp.top / 2.0));
|
|
|
|
} else {
|
|
|
|
timer_tmp.duty = (uint16_t)((float)timer_tmp.top * dutyCycle);
|
|
|
|
}
|
|
|
|
|
2014-04-01 10:26:46 -07:00
|
|
|
update_adc_sample_pos(&timer_tmp);
|
|
|
|
set_next_timer_settings(&timer_tmp);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2015-04-26 15:02:32 -07:00
|
|
|
static void run_pid_control_speed(void) {
|
2014-01-09 06:20:26 -08:00
|
|
|
static float i_term = 0;
|
|
|
|
static float prev_error = 0;
|
|
|
|
float p_term;
|
|
|
|
float d_term;
|
|
|
|
|
|
|
|
// PID is off. Return.
|
2014-03-30 15:51:59 -07:00
|
|
|
if (control_mode != CONTROL_MODE_SPEED) {
|
2017-09-04 12:12:43 -07:00
|
|
|
#if BLDC_SPEED_CONTROL_CURRENT
|
|
|
|
i_term = 0.0;
|
|
|
|
#else
|
2014-10-18 16:00:53 -07:00
|
|
|
i_term = dutycycle_now;
|
2017-09-04 12:12:43 -07:00
|
|
|
#endif
|
|
|
|
prev_error = 0.0;
|
2014-01-09 06:20:26 -08:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
const float rpm = mcpwm_get_rpm();
|
|
|
|
float error = speed_pid_set_rpm - rpm;
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
// Too low RPM set. Stop and return.
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(speed_pid_set_rpm) < conf->s_pid_min_erpm) {
|
2014-10-18 16:00:53 -07:00
|
|
|
i_term = dutycycle_now;
|
2017-09-04 12:12:43 -07:00
|
|
|
prev_error = error;
|
2014-05-26 10:29:51 -07:00
|
|
|
mcpwm_set_duty(0.0);
|
2014-01-09 06:20:26 -08:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
#if BLDC_SPEED_CONTROL_CURRENT
|
|
|
|
// Compute parameters
|
|
|
|
p_term = error * conf->s_pid_kp * (1.0 / 20.0);
|
|
|
|
i_term += error * (conf->s_pid_ki * MCPWM_PID_TIME_K) * (1.0 / 20.0);
|
|
|
|
d_term = (error - prev_error) * (conf->s_pid_kd / MCPWM_PID_TIME_K) * (1.0 / 20.0);
|
|
|
|
|
2018-03-24 14:32:58 -07:00
|
|
|
// Filter D
|
|
|
|
static float d_filter = 0.0;
|
|
|
|
UTILS_LP_FAST(d_filter, d_term, conf->p_pid_kd_filter);
|
|
|
|
d_term = d_filter;
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
// I-term wind-up protection
|
|
|
|
utils_truncate_number(&i_term, -1.0, 1.0);
|
|
|
|
|
|
|
|
// Store previous error
|
|
|
|
prev_error = error;
|
|
|
|
|
|
|
|
// Calculate output
|
|
|
|
float output = p_term + i_term + d_term;
|
|
|
|
utils_truncate_number(&output, -1.0, 1.0);
|
|
|
|
|
|
|
|
// Optionally disable braking
|
|
|
|
if (!conf->s_pid_allow_braking) {
|
|
|
|
if (rpm > 0.0 && output < 0.0) {
|
|
|
|
output = 0.0;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (rpm < 0.0 && output > 0.0) {
|
|
|
|
output = 0.0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
current_set = output * conf->lo_current_max;
|
|
|
|
|
|
|
|
if (state != MC_STATE_RUNNING) {
|
|
|
|
set_duty_cycle_hl(SIGN(output) * conf->l_min_duty);
|
|
|
|
}
|
|
|
|
#else
|
2014-01-09 06:20:26 -08:00
|
|
|
// Compensation for supply voltage variations
|
2021-04-27 08:48:15 -07:00
|
|
|
float scale = 1.0 / mc_interface_get_input_voltage_filtered();
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
// Compute parameters
|
2015-12-08 12:01:23 -08:00
|
|
|
p_term = error * conf->s_pid_kp * scale;
|
|
|
|
i_term += error * (conf->s_pid_ki * MCPWM_PID_TIME_K) * scale;
|
|
|
|
d_term = (error - prev_error) * (conf->s_pid_kd / MCPWM_PID_TIME_K) * scale;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2018-03-24 14:32:58 -07:00
|
|
|
// Filter D
|
|
|
|
static float d_filter = 0.0;
|
|
|
|
UTILS_LP_FAST(d_filter, d_term, conf->s_pid_kd_filter);
|
|
|
|
d_term = d_filter;
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
// I-term wind-up protection
|
2014-03-29 05:15:09 -07:00
|
|
|
utils_truncate_number(&i_term, -1.0, 1.0);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
// Store previous error
|
|
|
|
prev_error = error;
|
|
|
|
|
|
|
|
// Calculate output
|
|
|
|
float output = p_term + i_term + d_term;
|
|
|
|
|
|
|
|
// Make sure that at least minimum output is used
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(output) < conf->l_min_duty) {
|
|
|
|
output = SIGN(output) * conf->l_min_duty;
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
// Do not output in reverse direction to oppose too high rpm
|
2014-03-30 15:51:59 -07:00
|
|
|
if (speed_pid_set_rpm > 0.0 && output < 0.0) {
|
2015-12-08 12:01:23 -08:00
|
|
|
output = conf->l_min_duty;
|
2014-01-09 06:20:26 -08:00
|
|
|
i_term = 0.0;
|
2014-03-30 15:51:59 -07:00
|
|
|
} else if (speed_pid_set_rpm < 0.0 && output > 0.0) {
|
2015-12-08 12:01:23 -08:00
|
|
|
output = -conf->l_min_duty;
|
2014-01-09 06:20:26 -08:00
|
|
|
i_term = 0.0;
|
|
|
|
}
|
|
|
|
|
2014-03-30 15:51:59 -07:00
|
|
|
set_duty_cycle_hl(output);
|
2017-09-04 12:12:43 -07:00
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2019-02-18 10:30:19 -08:00
|
|
|
static void run_pid_control_pos(float dt, float pos_now) {
|
2015-04-26 15:02:32 -07:00
|
|
|
static float i_term = 0;
|
|
|
|
static float prev_error = 0;
|
|
|
|
float p_term;
|
|
|
|
float d_term;
|
|
|
|
|
|
|
|
// PID is off. Return.
|
|
|
|
if (control_mode != CONTROL_MODE_POS) {
|
|
|
|
i_term = 0;
|
|
|
|
prev_error = 0;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Compute error
|
2019-02-18 10:30:19 -08:00
|
|
|
float error = utils_angle_difference(pos_now, pos_pid_set_pos);
|
2015-04-26 15:02:32 -07:00
|
|
|
|
|
|
|
// Compute parameters
|
2015-12-08 12:01:23 -08:00
|
|
|
p_term = error * conf->p_pid_kp;
|
|
|
|
i_term += error * (conf->p_pid_ki * dt);
|
|
|
|
d_term = (error - prev_error) * (conf->p_pid_kd / dt);
|
2015-04-26 15:02:32 -07:00
|
|
|
|
2018-03-24 14:32:58 -07:00
|
|
|
// Filter D
|
|
|
|
static float d_filter = 0.0;
|
|
|
|
UTILS_LP_FAST(d_filter, d_term, conf->p_pid_kd_filter);
|
|
|
|
d_term = d_filter;
|
|
|
|
|
2015-04-26 15:02:32 -07:00
|
|
|
// I-term wind-up protection
|
|
|
|
utils_truncate_number(&i_term, -1.0, 1.0);
|
|
|
|
|
|
|
|
// Store previous error
|
|
|
|
prev_error = error;
|
|
|
|
|
|
|
|
// Calculate output
|
|
|
|
float output = p_term + i_term + d_term;
|
|
|
|
utils_truncate_number(&output, -1.0, 1.0);
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
current_set = output * conf->lo_current_max;
|
2015-04-26 15:02:32 -07:00
|
|
|
}
|
|
|
|
|
2015-10-08 14:09:39 -07:00
|
|
|
static THD_FUNCTION(rpm_thread, arg) {
|
2014-08-01 06:43:36 -07:00
|
|
|
(void)arg;
|
|
|
|
|
|
|
|
chRegSetThreadName("rpm timer");
|
|
|
|
|
|
|
|
for (;;) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (rpm_thd_stop) {
|
|
|
|
rpm_thd_stop = false;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-09-16 01:44:56 -07:00
|
|
|
if (rpm_dep.comms != 0) {
|
2014-08-29 21:00:48 -07:00
|
|
|
utils_sys_lock_cnt();
|
2014-09-16 01:44:56 -07:00
|
|
|
const float comms = (float)rpm_dep.comms;
|
2019-03-10 06:57:42 -07:00
|
|
|
const float time_at_comm = rpm_dep.time_at_comm;
|
2014-08-01 06:43:36 -07:00
|
|
|
rpm_dep.comms = 0;
|
2019-03-10 06:57:42 -07:00
|
|
|
rpm_dep.time_at_comm = 0.0;
|
2014-08-29 21:00:48 -07:00
|
|
|
utils_sys_unlock_cnt();
|
2014-08-01 06:43:36 -07:00
|
|
|
|
2019-03-10 06:57:42 -07:00
|
|
|
rpm_now = (comms * 60.0) / (time_at_comm * 6.0);
|
2014-08-01 06:43:36 -07:00
|
|
|
} else {
|
|
|
|
// In case we have slowed down
|
2019-03-10 06:57:42 -07:00
|
|
|
float rpm_tmp = 60.0 / (timer_seconds_elapsed_since(rpm_timer_start) * 6.0);
|
2014-08-01 06:43:36 -07:00
|
|
|
|
2014-09-16 01:44:56 -07:00
|
|
|
if (fabsf(rpm_tmp) < fabsf(rpm_now)) {
|
2014-08-01 06:43:36 -07:00
|
|
|
rpm_now = rpm_tmp;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// Some low-pass filtering
|
2017-09-04 12:12:43 -07:00
|
|
|
static float rpm_filtered = 0.0;
|
|
|
|
UTILS_LP_FAST(rpm_filtered, rpm_now, 0.1);
|
|
|
|
rpm_now = rpm_filtered;
|
2014-10-07 13:44:44 -07:00
|
|
|
const float rpm_abs = fabsf(rpm_now);
|
2014-08-01 06:43:36 -07:00
|
|
|
|
|
|
|
// Update the cycle integrator limit
|
2015-12-08 12:01:23 -08:00
|
|
|
rpm_dep.cycle_int_limit = conf->sl_cycle_int_limit;
|
2014-08-11 13:38:50 -07:00
|
|
|
rpm_dep.cycle_int_limit_running = rpm_dep.cycle_int_limit + (float)ADC_Value[ADC_IND_VIN_SENS] *
|
2015-12-08 12:01:23 -08:00
|
|
|
conf->sl_bemf_coupling_k / (rpm_abs > conf->sl_min_erpm ? rpm_abs : conf->sl_min_erpm);
|
2015-02-19 12:20:07 -08:00
|
|
|
rpm_dep.cycle_int_limit_running = utils_map(rpm_abs, 0,
|
2015-12-08 12:01:23 -08:00
|
|
|
conf->sl_cycle_int_rpm_br, rpm_dep.cycle_int_limit_running,
|
|
|
|
rpm_dep.cycle_int_limit_running * conf->sl_phase_advance_at_br);
|
2014-09-13 01:46:46 -07:00
|
|
|
rpm_dep.cycle_int_limit_max = rpm_dep.cycle_int_limit + (float)ADC_Value[ADC_IND_VIN_SENS] *
|
2015-12-08 12:01:23 -08:00
|
|
|
conf->sl_bemf_coupling_k / conf->sl_min_erpm_cycle_int_limit;
|
2014-08-11 13:38:50 -07:00
|
|
|
|
2015-02-24 12:38:53 -08:00
|
|
|
if (rpm_dep.cycle_int_limit_running < 1.0) {
|
|
|
|
rpm_dep.cycle_int_limit_running = 1.0;
|
|
|
|
}
|
|
|
|
|
2014-09-13 01:46:46 -07:00
|
|
|
if (rpm_dep.cycle_int_limit_running > rpm_dep.cycle_int_limit_max) {
|
|
|
|
rpm_dep.cycle_int_limit_running = rpm_dep.cycle_int_limit_max;
|
2014-08-01 06:43:36 -07:00
|
|
|
}
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
rpm_dep.comm_time_sum = conf->m_bldc_f_sw_max / ((rpm_abs / 60.0) * 6.0);
|
|
|
|
rpm_dep.comm_time_sum_min_rpm = conf->m_bldc_f_sw_max / ((conf->sl_min_erpm / 60.0) * 6.0);
|
2014-09-12 14:37:33 -07:00
|
|
|
|
2015-04-26 15:02:32 -07:00
|
|
|
run_pid_control_speed();
|
2014-09-20 04:41:18 -07:00
|
|
|
|
2014-08-01 06:43:36 -07:00
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-10-08 14:09:39 -07:00
|
|
|
static THD_FUNCTION(timer_thread, arg) {
|
2014-01-09 06:20:26 -08:00
|
|
|
(void)arg;
|
|
|
|
|
|
|
|
chRegSetThreadName("mcpwm timer");
|
|
|
|
|
2014-03-29 05:15:09 -07:00
|
|
|
float amp;
|
2014-03-29 16:57:22 -07:00
|
|
|
float min_s;
|
|
|
|
float max_s;
|
2014-03-29 05:15:09 -07:00
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
for(;;) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (timer_thd_stop) {
|
|
|
|
timer_thd_stop = false;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
if (state == MC_STATE_OFF) {
|
2014-03-29 16:57:22 -07:00
|
|
|
// Track the motor back-emf and follow it with dutycycle_now. Also track
|
|
|
|
// the direction of the motor.
|
2014-03-29 05:15:09 -07:00
|
|
|
amp = filter_run_fir_iteration((float*)amp_fir_samples,
|
|
|
|
(float*)amp_fir_coeffs, AMP_FIR_TAPS_BITS, amp_fir_index);
|
2014-03-29 16:57:22 -07:00
|
|
|
|
2014-04-04 14:19:11 -07:00
|
|
|
// Direction tracking
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_DC) {
|
2015-04-12 09:23:17 -07:00
|
|
|
if (amp > 0) {
|
|
|
|
direction = 0;
|
|
|
|
} else {
|
|
|
|
direction = 1;
|
|
|
|
amp = -amp;
|
|
|
|
}
|
|
|
|
} else {
|
2015-06-19 14:38:46 -07:00
|
|
|
if (sensorless_now) {
|
2015-04-12 09:23:17 -07:00
|
|
|
min_s = 9999999999999.0;
|
|
|
|
max_s = 0.0;
|
2014-03-29 16:57:22 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
for (int i = 0;i < 6;i++) {
|
|
|
|
if (last_pwm_cycles_sums[i] < min_s) {
|
|
|
|
min_s = last_pwm_cycles_sums[i];
|
|
|
|
}
|
2014-03-29 16:57:22 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (last_pwm_cycles_sums[i] > max_s) {
|
|
|
|
max_s = last_pwm_cycles_sums[i];
|
|
|
|
}
|
2014-09-14 14:08:22 -07:00
|
|
|
}
|
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
// If the relative difference between the longest and shortest commutation is
|
|
|
|
// too large, we probably got the direction wrong. In that case, try the other
|
|
|
|
// direction.
|
|
|
|
//
|
|
|
|
// The tachometer_for_direction value is used to make sure that the samples
|
|
|
|
// have enough time after a direction change to get stable before trying to
|
|
|
|
// change direction again.
|
|
|
|
|
|
|
|
if ((max_s - min_s) / ((max_s + min_s) / 2.0) > 1.2) {
|
|
|
|
if (tachometer_for_direction > 12) {
|
|
|
|
if (direction == 1) {
|
|
|
|
direction = 0;
|
|
|
|
} else {
|
|
|
|
direction = 1;
|
|
|
|
}
|
|
|
|
tachometer_for_direction = 0;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
tachometer_for_direction = 0;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// If the direction tachometer is counting backwards, the motor is
|
|
|
|
// not moving in the direction we think it is.
|
|
|
|
if (tachometer_for_direction < -3) {
|
2014-09-14 14:08:22 -07:00
|
|
|
if (direction == 1) {
|
|
|
|
direction = 0;
|
|
|
|
} else {
|
|
|
|
direction = 1;
|
|
|
|
}
|
|
|
|
tachometer_for_direction = 0;
|
2015-04-12 09:23:17 -07:00
|
|
|
} else if (tachometer_for_direction > 0) {
|
|
|
|
tachometer_for_direction = 0;
|
2014-09-14 14:08:22 -07:00
|
|
|
}
|
2014-03-29 16:57:22 -07:00
|
|
|
}
|
2014-04-04 14:19:11 -07:00
|
|
|
}
|
2014-03-29 16:57:22 -07:00
|
|
|
|
2014-03-29 05:15:09 -07:00
|
|
|
if (direction == 1) {
|
2014-07-27 10:40:38 -07:00
|
|
|
dutycycle_now = amp / (float)ADC_Value[ADC_IND_VIN_SENS];
|
2014-03-29 05:15:09 -07:00
|
|
|
} else {
|
2014-07-27 10:40:38 -07:00
|
|
|
dutycycle_now = -amp / (float)ADC_Value[ADC_IND_VIN_SENS];
|
2014-03-29 05:15:09 -07:00
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
utils_truncate_number((float*)&dutycycle_now, -conf->l_max_duty, conf->l_max_duty);
|
2015-08-23 09:26:05 -07:00
|
|
|
} else {
|
|
|
|
tachometer_for_direction = 0;
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
// Fill KV filter vector at 100Hz
|
|
|
|
static int cnt_tmp = 0;
|
|
|
|
cnt_tmp++;
|
|
|
|
if (cnt_tmp >= 10) {
|
|
|
|
cnt_tmp = 0;
|
|
|
|
if (state == MC_STATE_RUNNING) {
|
|
|
|
filter_add_sample((float*)kv_fir_samples, mcpwm_get_kv(),
|
|
|
|
KV_FIR_TAPS_BITS, (uint32_t*)&kv_fir_index);
|
2014-03-29 05:15:09 -07:00
|
|
|
} else if (state == MC_STATE_OFF) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (dutycycle_now >= conf->l_min_duty) {
|
2014-03-29 05:15:09 -07:00
|
|
|
filter_add_sample((float*)kv_fir_samples, mcpwm_get_kv(),
|
|
|
|
KV_FIR_TAPS_BITS, (uint32_t*)&kv_fir_index);
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void mcpwm_adc_inj_int_handler(void) {
|
2019-03-10 06:57:42 -07:00
|
|
|
uint32_t t_start = timer_time_now();
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
int curr0 = ADC_GetInjectedConversionValue(ADC1, ADC_InjectedChannel_1);
|
|
|
|
int curr1 = ADC_GetInjectedConversionValue(ADC2, ADC_InjectedChannel_1);
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
int curr0_2 = ADC_GetInjectedConversionValue(ADC2, ADC_InjectedChannel_2);
|
|
|
|
int curr1_2 = ADC_GetInjectedConversionValue(ADC1, ADC_InjectedChannel_2);
|
|
|
|
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
int curr2 = ADC_GetInjectedConversionValue(ADC3, ADC_InjectedChannel_1);
|
|
|
|
#endif
|
|
|
|
|
2020-01-12 12:25:21 -08:00
|
|
|
#ifdef INVERTED_SHUNT_POLARITY
|
|
|
|
curr0 = 4095 - curr0;
|
|
|
|
curr1 = 4095 - curr1;
|
|
|
|
|
|
|
|
curr0_2 = 4095 - curr0_2;
|
|
|
|
curr1_2 = 4095 - curr1_2;
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
curr2 = 4095 - curr2;
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
float curr0_currsamp = curr0;
|
|
|
|
float curr1_currsamp = curr1;
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
float curr2_currsamp = curr2;
|
|
|
|
#endif
|
2015-08-23 09:26:05 -07:00
|
|
|
|
2016-06-27 08:29:09 -07:00
|
|
|
if (curr_samp_volt & (1 << 0)) {
|
2019-02-18 10:30:19 -08:00
|
|
|
curr0 = GET_CURRENT1();
|
2016-06-27 08:29:09 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
if (curr_samp_volt & (1 << 1)) {
|
2019-02-18 10:30:19 -08:00
|
|
|
curr1 = GET_CURRENT2();
|
2014-04-12 12:59:33 -07:00
|
|
|
}
|
|
|
|
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
if (curr_samp_volt & (1 << 2)) {
|
2019-02-18 10:30:19 -08:00
|
|
|
curr2 = GET_CURRENT3();
|
2016-06-27 08:29:09 -07:00
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
// DCCal every other cycle
|
2016-06-27 08:29:09 -07:00
|
|
|
// static bool sample_ofs = true;
|
|
|
|
// if (sample_ofs) {
|
|
|
|
// sample_ofs = false;
|
|
|
|
// curr0_offset = curr0;
|
|
|
|
// curr1_offset = curr1;
|
|
|
|
// DCCAL_OFF();
|
|
|
|
// return;
|
|
|
|
// } else {
|
|
|
|
// sample_ofs = true;
|
|
|
|
// DCCAL_ON();
|
|
|
|
// }
|
2015-08-23 09:26:05 -07:00
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
curr0_sum += curr0;
|
|
|
|
curr1_sum += curr1;
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
curr2_sum += curr2;
|
|
|
|
#endif
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
curr_start_samples++;
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
curr0_currsamp -= curr0_offset;
|
|
|
|
curr1_currsamp -= curr1_offset;
|
|
|
|
curr0 -= curr0_offset;
|
|
|
|
curr1 -= curr1_offset;
|
|
|
|
curr0_2 -= curr0_offset;
|
|
|
|
curr1_2 -= curr1_offset;
|
|
|
|
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
curr2_currsamp -= curr2_offset;
|
|
|
|
curr2 -= curr2_offset;
|
|
|
|
#endif
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
#if CURR1_DOUBLE_SAMPLE || CURR2_DOUBLE_SAMPLE
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->pwm_mode != PWM_MODE_BIPOLAR && conf->motor_type == MOTOR_TYPE_BLDC) {
|
2015-08-23 09:26:05 -07:00
|
|
|
if (direction) {
|
|
|
|
if (CURR1_DOUBLE_SAMPLE && comm_step == 3) {
|
|
|
|
curr0 = (curr0 + curr0_2) / 2.0;
|
|
|
|
} else if (CURR2_DOUBLE_SAMPLE && comm_step == 4) {
|
|
|
|
curr1 = (curr1 + curr1_2) / 2.0;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
if (CURR1_DOUBLE_SAMPLE && comm_step == 2) {
|
|
|
|
curr0 = (curr0 + curr0_2) / 2.0;
|
|
|
|
} else if (CURR2_DOUBLE_SAMPLE && comm_step == 1) {
|
|
|
|
curr1 = (curr1 + curr1_2) / 2.0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
ADC_curr_norm_value[0] = curr0;
|
|
|
|
ADC_curr_norm_value[1] = curr1;
|
2016-06-27 08:29:09 -07:00
|
|
|
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
ADC_curr_norm_value[2] = curr2;
|
|
|
|
#else
|
2014-01-09 06:20:26 -08:00
|
|
|
ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
|
2016-06-27 08:29:09 -07:00
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
|
|
|
|
float curr_tot_sample = 0;
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_DC) {
|
2015-04-12 09:23:17 -07:00
|
|
|
if (direction) {
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
2019-02-18 10:30:19 -08:00
|
|
|
curr_tot_sample = -(float)(GET_CURRENT3() - curr2_offset);
|
2016-06-27 08:29:09 -07:00
|
|
|
#else
|
2019-02-18 10:30:19 -08:00
|
|
|
curr_tot_sample = -(float)(GET_CURRENT2() - curr1_offset);
|
2016-06-27 08:29:09 -07:00
|
|
|
#endif
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
2019-02-18 10:30:19 -08:00
|
|
|
curr_tot_sample = -(float)(GET_CURRENT1() - curr0_offset);
|
2015-04-12 09:23:17 -07:00
|
|
|
}
|
2014-12-12 16:31:54 -08:00
|
|
|
} else {
|
2015-04-12 09:23:17 -07:00
|
|
|
static int detect_now = 0;
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Commutation Steps FORWARDS
|
|
|
|
* STEP BR1 BR2 BR3
|
|
|
|
* 1 0 + -
|
|
|
|
* 2 + 0 -
|
|
|
|
* 3 + - 0
|
|
|
|
* 4 0 - +
|
|
|
|
* 5 - 0 +
|
|
|
|
* 6 - + 0
|
|
|
|
*
|
|
|
|
* Commutation Steps REVERSE (switch phase 2 and 3)
|
|
|
|
* STEP BR1 BR2 BR3
|
|
|
|
* 1 0 - +
|
|
|
|
* 2 + - 0
|
|
|
|
* 3 + 0 -
|
|
|
|
* 4 0 + -
|
|
|
|
* 5 - + 0
|
|
|
|
* 6 - 0 +
|
|
|
|
*/
|
|
|
|
|
|
|
|
if (state == MC_STATE_FULL_BRAKE) {
|
|
|
|
float c0 = (float)ADC_curr_norm_value[0];
|
|
|
|
float c1 = (float)ADC_curr_norm_value[1];
|
|
|
|
float c2 = (float)ADC_curr_norm_value[2];
|
|
|
|
curr_tot_sample = sqrtf((c0*c0 + c1*c1 + c2*c2) / 1.5);
|
|
|
|
} else {
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
if (direction) {
|
|
|
|
switch (comm_step) {
|
|
|
|
case 1: curr_tot_sample = -(float)ADC_curr_norm_value[2]; break;
|
|
|
|
case 2: curr_tot_sample = -(float)ADC_curr_norm_value[2]; break;
|
|
|
|
case 3: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
|
|
|
case 4: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
|
|
|
case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
|
|
|
case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
|
|
|
default: break;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
switch (comm_step) {
|
|
|
|
case 1: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
|
|
|
case 2: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
|
|
|
case 3: curr_tot_sample = -(float)ADC_curr_norm_value[2]; break;
|
|
|
|
case 4: curr_tot_sample = -(float)ADC_curr_norm_value[2]; break;
|
|
|
|
case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
|
|
|
case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
|
|
|
default: break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#else
|
2015-08-23 09:26:05 -07:00
|
|
|
if (direction) {
|
|
|
|
switch (comm_step) {
|
|
|
|
case 1: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
|
|
|
case 2: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
|
|
|
case 3: curr_tot_sample = (float)ADC_curr_norm_value[0]; break;
|
|
|
|
case 4: curr_tot_sample = (float)ADC_curr_norm_value[1]; break;
|
|
|
|
case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
|
|
|
case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
|
|
|
default: break;
|
2014-12-12 16:31:54 -08:00
|
|
|
}
|
2015-08-23 09:26:05 -07:00
|
|
|
} else {
|
|
|
|
switch (comm_step) {
|
|
|
|
case 1: curr_tot_sample = (float)ADC_curr_norm_value[1]; break;
|
|
|
|
case 2: curr_tot_sample = (float)ADC_curr_norm_value[0]; break;
|
|
|
|
case 3: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
|
|
|
case 4: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
|
|
|
case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
|
|
|
case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
|
|
|
default: break;
|
2014-12-12 16:31:54 -08:00
|
|
|
}
|
2014-10-24 14:04:10 -07:00
|
|
|
}
|
2016-06-27 08:29:09 -07:00
|
|
|
#endif
|
2015-08-23 09:26:05 -07:00
|
|
|
|
|
|
|
const float tot_sample_tmp = curr_tot_sample;
|
|
|
|
static int comm_step_prev = 1;
|
|
|
|
static float prev_tot_sample = 0.0;
|
|
|
|
if (comm_step != comm_step_prev) {
|
|
|
|
curr_tot_sample = prev_tot_sample;
|
|
|
|
}
|
|
|
|
comm_step_prev = comm_step;
|
|
|
|
prev_tot_sample = tot_sample_tmp;
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (detect_now == 4) {
|
2015-08-23 09:26:05 -07:00
|
|
|
const float a = fabsf(ADC_curr_norm_value[0]);
|
|
|
|
const float b = fabsf(ADC_curr_norm_value[1]);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (a > b) {
|
|
|
|
mcpwm_detect_currents[detect_step] = a;
|
|
|
|
} else {
|
|
|
|
mcpwm_detect_currents[detect_step] = b;
|
|
|
|
}
|
2014-04-21 07:17:39 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (detect_step > 0) {
|
|
|
|
mcpwm_detect_currents_diff[detect_step] =
|
|
|
|
mcpwm_detect_currents[detect_step - 1] - mcpwm_detect_currents[detect_step];
|
|
|
|
} else {
|
|
|
|
mcpwm_detect_currents_diff[detect_step] =
|
|
|
|
mcpwm_detect_currents[5] - mcpwm_detect_currents[detect_step];
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
const int vzero = ADC_V_ZERO;
|
2016-06-27 08:29:09 -07:00
|
|
|
// const int vzero = (ADC_V_L1 + ADC_V_L2 + ADC_V_L3) / 3;
|
2015-06-26 12:45:28 -07:00
|
|
|
|
|
|
|
switch (comm_step) {
|
|
|
|
case 1:
|
|
|
|
case 4:
|
|
|
|
mcpwm_detect_voltages[detect_step] = ADC_V_L1 - vzero;
|
|
|
|
break;
|
|
|
|
|
|
|
|
case 2:
|
|
|
|
case 5:
|
|
|
|
mcpwm_detect_voltages[detect_step] = ADC_V_L2 - vzero;
|
|
|
|
break;
|
|
|
|
|
|
|
|
case 3:
|
|
|
|
case 6:
|
|
|
|
mcpwm_detect_voltages[detect_step] = ADC_V_L3 - vzero;
|
|
|
|
break;
|
|
|
|
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
mcpwm_detect_currents_avg[detect_step] += mcpwm_detect_currents[detect_step];
|
2015-06-26 12:45:28 -07:00
|
|
|
mcpwm_detect_avg_samples[detect_step]++;
|
2014-03-15 14:32:00 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
stop_pwm_hw();
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (detect_now) {
|
|
|
|
detect_now--;
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (IS_DETECTING() && detect_now == 0) {
|
|
|
|
detect_now = 5;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
set_duty_cycle_hw(0.2);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
detect_step++;
|
|
|
|
if (detect_step > 5) {
|
|
|
|
detect_step = 0;
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
comm_step = detect_step + 1;
|
2014-04-21 07:17:39 -07:00
|
|
|
|
2015-06-26 12:45:28 -07:00
|
|
|
set_next_comm_step(comm_step);
|
2015-04-12 09:23:17 -07:00
|
|
|
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2016-06-27 08:29:09 -07:00
|
|
|
last_current_sample = curr_tot_sample * FAC_CURRENT;
|
2015-08-23 09:26:05 -07:00
|
|
|
|
|
|
|
// Filter out outliers
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(last_current_sample) > (conf->l_abs_current_max * 1.2)) {
|
|
|
|
last_current_sample = SIGN(last_current_sample) * conf->l_abs_current_max * 1.2;
|
2015-08-23 09:26:05 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
filter_add_sample((float*) current_fir_samples, last_current_sample,
|
2014-04-02 02:54:21 -07:00
|
|
|
CURR_FIR_TAPS_BITS, (uint32_t*) ¤t_fir_index);
|
|
|
|
last_current_sample_filtered = filter_run_fir_iteration(
|
|
|
|
(float*) current_fir_samples, (float*) current_fir_coeffs,
|
|
|
|
CURR_FIR_TAPS_BITS, current_fir_index);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2019-03-10 06:57:42 -07:00
|
|
|
last_inj_adc_isr_duration = timer_seconds_elapsed_since(t_start);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
* New ADC samples ready. Do commutation!
|
|
|
|
*/
|
|
|
|
void mcpwm_adc_int_handler(void *p, uint32_t flags) {
|
|
|
|
(void)p;
|
|
|
|
(void)flags;
|
|
|
|
|
2019-03-10 06:57:42 -07:00
|
|
|
uint32_t t_start = timer_time_now();
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-10-16 00:42:26 -07:00
|
|
|
// Set the next timer settings if an update is far enough away
|
2014-10-24 14:04:10 -07:00
|
|
|
update_timer_attempt();
|
2014-10-16 00:42:26 -07:00
|
|
|
|
2014-05-08 14:44:27 -07:00
|
|
|
// Reset the watchdog
|
2019-01-24 07:19:44 -08:00
|
|
|
timeout_feed_WDT(THREAD_MCPWM);
|
2014-05-08 14:44:27 -07:00
|
|
|
|
2014-03-21 07:40:05 -07:00
|
|
|
const float input_voltage = GET_INPUT_VOLTAGE();
|
2014-08-01 06:43:36 -07:00
|
|
|
int ph1, ph2, ph3;
|
2014-03-21 07:40:05 -07:00
|
|
|
|
2014-08-01 06:43:36 -07:00
|
|
|
static int direction_before = 1;
|
2016-11-06 11:28:07 -08:00
|
|
|
if (!(state == MC_STATE_RUNNING && direction == direction_before)) {
|
2014-07-27 10:40:38 -07:00
|
|
|
has_commutated = 0;
|
2014-03-30 15:51:59 -07:00
|
|
|
}
|
|
|
|
direction_before = direction;
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_BLDC) {
|
2019-02-19 13:06:34 -08:00
|
|
|
int ph1_raw, ph2_raw, ph3_raw;
|
2014-03-15 14:32:00 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
/*
|
|
|
|
* Calculate the virtual ground, depending on the state.
|
|
|
|
*/
|
|
|
|
if (has_commutated && fabsf(dutycycle_now) > 0.2) {
|
|
|
|
mcpwm_vzero = ADC_V_ZERO;
|
|
|
|
} else {
|
|
|
|
mcpwm_vzero = (ADC_V_L1 + ADC_V_L2 + ADC_V_L3) / 3;
|
|
|
|
}
|
2014-03-29 05:15:09 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (direction) {
|
|
|
|
ph1 = ADC_V_L1 - mcpwm_vzero;
|
|
|
|
ph2 = ADC_V_L2 - mcpwm_vzero;
|
|
|
|
ph3 = ADC_V_L3 - mcpwm_vzero;
|
|
|
|
ph1_raw = ADC_V_L1;
|
|
|
|
ph2_raw = ADC_V_L2;
|
|
|
|
ph3_raw = ADC_V_L3;
|
|
|
|
} else {
|
|
|
|
ph1 = ADC_V_L1 - mcpwm_vzero;
|
|
|
|
ph2 = ADC_V_L3 - mcpwm_vzero;
|
|
|
|
ph3 = ADC_V_L2 - mcpwm_vzero;
|
|
|
|
ph1_raw = ADC_V_L1;
|
|
|
|
ph2_raw = ADC_V_L3;
|
|
|
|
ph3_raw = ADC_V_L2;
|
|
|
|
}
|
2014-03-29 05:15:09 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
update_timer_attempt();
|
2014-10-07 13:44:44 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
float amp = 0.0;
|
2015-01-14 10:06:05 -08:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (has_commutated) {
|
|
|
|
amp = fabsf(dutycycle_now) * (float)ADC_Value[ADC_IND_VIN_SENS];
|
|
|
|
} else {
|
|
|
|
amp = sqrtf((float)(ph1*ph1 + ph2*ph2 + ph3*ph3)) * sqrtf(2.0);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
// Fill the amplitude FIR filter
|
|
|
|
filter_add_sample((float*)amp_fir_samples, amp,
|
|
|
|
AMP_FIR_TAPS_BITS, (uint32_t*)&_fir_index);
|
|
|
|
|
2015-06-19 14:38:46 -07:00
|
|
|
if (sensorless_now) {
|
2015-04-12 09:23:17 -07:00
|
|
|
static float cycle_integrator = 0;
|
|
|
|
|
|
|
|
if (pwm_cycles_sum >= rpm_dep.comm_time_sum_min_rpm) {
|
|
|
|
if (state == MC_STATE_RUNNING) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->comm_mode == COMM_MODE_INTEGRATE) {
|
2015-04-12 09:23:17 -07:00
|
|
|
// This means that the motor is stuck. If this commutation does not
|
|
|
|
// produce any torque because of misalignment at start, two
|
|
|
|
// commutations ahead should produce full torque.
|
|
|
|
commutate(2);
|
2015-12-08 12:01:23 -08:00
|
|
|
} else if (conf->comm_mode == COMM_MODE_DELAY) {
|
2015-04-12 09:23:17 -07:00
|
|
|
commutate(1);
|
|
|
|
}
|
2014-10-07 13:44:44 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
cycle_integrator = 0.0;
|
2014-10-16 00:42:26 -07:00
|
|
|
}
|
2014-10-07 13:44:44 -07:00
|
|
|
}
|
2014-05-09 03:45:11 -07:00
|
|
|
|
2015-08-26 14:12:39 -07:00
|
|
|
if ((state == MC_STATE_RUNNING && pwm_cycles >= 2) || state == MC_STATE_OFF) {
|
2015-04-12 09:23:17 -07:00
|
|
|
int v_diff = 0;
|
|
|
|
int ph_now_raw = 0;
|
|
|
|
|
|
|
|
switch (comm_step) {
|
|
|
|
case 1:
|
|
|
|
v_diff = ph1;
|
|
|
|
ph_now_raw = ph1_raw;
|
|
|
|
break;
|
|
|
|
case 2:
|
|
|
|
v_diff = -ph2;
|
|
|
|
ph_now_raw = ph2_raw;
|
|
|
|
break;
|
|
|
|
case 3:
|
|
|
|
v_diff = ph3;
|
|
|
|
ph_now_raw = ph3_raw;
|
|
|
|
break;
|
|
|
|
case 4:
|
|
|
|
v_diff = -ph1;
|
|
|
|
ph_now_raw = ph1_raw;
|
|
|
|
break;
|
|
|
|
case 5:
|
|
|
|
v_diff = ph2;
|
|
|
|
ph_now_raw = ph2_raw;
|
|
|
|
break;
|
|
|
|
case 6:
|
|
|
|
v_diff = -ph3;
|
|
|
|
ph_now_raw = ph3_raw;
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
break;
|
2014-10-07 13:44:44 -07:00
|
|
|
}
|
|
|
|
|
2015-06-19 14:38:46 -07:00
|
|
|
// Collect hall sensor samples in the first half of the commutation cycle. This is
|
|
|
|
// because positive timing is much better than negative timing in case they are
|
|
|
|
// mis-aligned.
|
|
|
|
if (v_diff < 50) {
|
|
|
|
hall_detect_table[read_hall()][comm_step]++;
|
|
|
|
}
|
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
// Don't commutate while the motor is standing still and the signal only consists
|
|
|
|
// of weak noise.
|
|
|
|
if (abs(v_diff) < 10) {
|
|
|
|
v_diff = 0;
|
2014-10-07 13:44:44 -07:00
|
|
|
}
|
2015-04-12 09:23:17 -07:00
|
|
|
|
2014-10-07 13:44:44 -07:00
|
|
|
if (v_diff > 0) {
|
2015-08-26 14:12:39 -07:00
|
|
|
// TODO!
|
2016-06-27 08:29:09 -07:00
|
|
|
// const int min = 100;
|
2015-08-26 14:12:39 -07:00
|
|
|
int min = (int)((1.0 - fabsf(dutycycle_now)) * (float)ADC_Value[ADC_IND_VIN_SENS] * 0.3);
|
|
|
|
if (min > ADC_Value[ADC_IND_VIN_SENS] / 4) {
|
|
|
|
min = ADC_Value[ADC_IND_VIN_SENS] / 4;
|
|
|
|
}
|
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (pwm_cycles_sum > (last_pwm_cycles_sum / 2.0) ||
|
2015-08-26 14:12:39 -07:00
|
|
|
!has_commutated || (ph_now_raw > min && ph_now_raw < (ADC_Value[ADC_IND_VIN_SENS] - min))) {
|
2015-04-12 09:23:17 -07:00
|
|
|
cycle_integrator += (float)v_diff / switching_frequency_now;
|
|
|
|
}
|
|
|
|
}
|
2014-09-14 14:08:22 -07:00
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
static float cycle_sum = 0.0;
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->comm_mode == COMM_MODE_INTEGRATE) {
|
2015-04-12 09:23:17 -07:00
|
|
|
float limit;
|
|
|
|
if (has_commutated) {
|
|
|
|
limit = rpm_dep.cycle_int_limit_running * (0.0005 * VDIV_CORR);
|
|
|
|
} else {
|
|
|
|
limit = rpm_dep.cycle_int_limit * (0.0005 * VDIV_CORR);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (cycle_integrator >= (rpm_dep.cycle_int_limit_max * (0.0005 * VDIV_CORR)) ||
|
|
|
|
cycle_integrator >= limit) {
|
2014-10-07 13:44:44 -07:00
|
|
|
commutate(1);
|
2015-04-12 09:23:17 -07:00
|
|
|
cycle_integrator = 0.0;
|
2017-09-04 12:12:43 -07:00
|
|
|
cycle_sum = 0.0;
|
2015-04-12 09:23:17 -07:00
|
|
|
}
|
2015-12-08 12:01:23 -08:00
|
|
|
} else if (conf->comm_mode == COMM_MODE_DELAY) {
|
2015-04-12 09:23:17 -07:00
|
|
|
if (v_diff > 0) {
|
2017-09-04 12:12:43 -07:00
|
|
|
cycle_sum += conf->m_bldc_f_sw_max / switching_frequency_now;
|
2015-04-12 09:23:17 -07:00
|
|
|
|
|
|
|
if (cycle_sum >= utils_map(fabsf(rpm_now), 0,
|
2015-12-08 12:01:23 -08:00
|
|
|
conf->sl_cycle_int_rpm_br, rpm_dep.comm_time_sum / 2.0,
|
|
|
|
(rpm_dep.comm_time_sum / 2.0) * conf->sl_phase_advance_at_br)) {
|
2015-04-12 09:23:17 -07:00
|
|
|
commutate(1);
|
|
|
|
cycle_integrator_sum += cycle_integrator * (1.0 / (0.0005 * VDIV_CORR));
|
|
|
|
cycle_integrator_iterations += 1.0;
|
|
|
|
cycle_integrator = 0.0;
|
|
|
|
cycle_sum = 0.0;
|
|
|
|
}
|
|
|
|
} else {
|
2014-10-07 13:44:44 -07:00
|
|
|
cycle_integrator = 0.0;
|
2014-09-14 14:08:22 -07:00
|
|
|
cycle_sum = 0.0;
|
|
|
|
}
|
2014-09-12 14:37:33 -07:00
|
|
|
}
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
|
|
|
cycle_integrator = 0.0;
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
pwm_cycles_sum += conf->m_bldc_f_sw_max / switching_frequency_now;
|
2015-08-23 09:26:05 -07:00
|
|
|
pwm_cycles++;
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
2015-08-23 09:26:05 -07:00
|
|
|
const int hall_phase = mcpwm_read_hall_phase();
|
2015-04-12 09:23:17 -07:00
|
|
|
if (comm_step != hall_phase) {
|
|
|
|
comm_step = hall_phase;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
update_rpm_tacho();
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (state == MC_STATE_RUNNING) {
|
|
|
|
set_next_comm_step(comm_step);
|
|
|
|
commutate(0);
|
|
|
|
}
|
|
|
|
} else if (state == MC_STATE_RUNNING && !has_commutated) {
|
2014-09-14 14:08:22 -07:00
|
|
|
set_next_comm_step(comm_step);
|
2014-10-07 13:44:44 -07:00
|
|
|
commutate(0);
|
2014-09-14 14:08:22 -07:00
|
|
|
}
|
2015-04-12 09:23:17 -07:00
|
|
|
}
|
|
|
|
} else {
|
|
|
|
float amp = 0.0;
|
|
|
|
|
|
|
|
if (has_commutated) {
|
|
|
|
amp = dutycycle_now * (float)ADC_Value[ADC_IND_VIN_SENS];
|
|
|
|
} else {
|
|
|
|
amp = ADC_V_L3 - ADC_V_L1;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Fill the amplitude FIR filter
|
|
|
|
filter_add_sample((float*)amp_fir_samples, amp,
|
|
|
|
AMP_FIR_TAPS_BITS, (uint32_t*)&_fir_index);
|
|
|
|
|
|
|
|
if (state == MC_STATE_RUNNING && !has_commutated) {
|
2014-09-15 11:48:46 -07:00
|
|
|
set_next_comm_step(comm_step);
|
2014-10-07 13:44:44 -07:00
|
|
|
commutate(0);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-08-05 15:52:56 -07:00
|
|
|
const float current_nofilter = mcpwm_get_tot_current();
|
|
|
|
const float current_in_nofilter = current_nofilter * fabsf(dutycycle_now);
|
2014-10-16 00:42:26 -07:00
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
if (state == MC_STATE_RUNNING && has_commutated) {
|
2014-03-30 15:51:59 -07:00
|
|
|
// Compensation for supply voltage variations
|
2014-04-04 14:19:11 -07:00
|
|
|
const float voltage_scale = 20.0 / input_voltage;
|
2015-12-08 12:01:23 -08:00
|
|
|
float ramp_step = conf->m_duty_ramp_step / (switching_frequency_now / 1000.0);
|
2014-07-27 10:40:38 -07:00
|
|
|
float ramp_step_no_lim = ramp_step;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
if (slow_ramping_cycles) {
|
|
|
|
slow_ramping_cycles--;
|
|
|
|
ramp_step *= 0.1;
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-03-30 15:51:59 -07:00
|
|
|
float dutycycle_now_tmp = dutycycle_now;
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
#if BLDC_SPEED_CONTROL_CURRENT
|
|
|
|
if (control_mode == CONTROL_MODE_CURRENT ||
|
|
|
|
control_mode == CONTROL_MODE_POS ||
|
|
|
|
control_mode == CONTROL_MODE_SPEED) {
|
|
|
|
#else
|
2015-04-26 15:02:32 -07:00
|
|
|
if (control_mode == CONTROL_MODE_CURRENT || control_mode == CONTROL_MODE_POS) {
|
2017-09-04 12:12:43 -07:00
|
|
|
#endif
|
2014-03-30 15:51:59 -07:00
|
|
|
// Compute error
|
2014-08-06 09:44:15 -07:00
|
|
|
const float error = current_set - (direction ? current_nofilter : -current_nofilter);
|
2015-12-08 12:01:23 -08:00
|
|
|
float step = error * conf->cc_gain * voltage_scale;
|
|
|
|
const float start_boost = conf->cc_startup_boost_duty * voltage_scale;
|
2014-03-30 15:51:59 -07:00
|
|
|
|
2014-05-05 11:39:25 -07:00
|
|
|
// Do not ramp too much
|
2015-12-08 12:01:23 -08:00
|
|
|
utils_truncate_number(&step, -conf->cc_ramp_step_max, conf->cc_ramp_step_max);
|
2014-05-05 11:39:25 -07:00
|
|
|
|
|
|
|
// Switching frequency correction
|
2014-08-05 13:24:13 -07:00
|
|
|
step /= switching_frequency_now / 1000.0;
|
2014-05-05 11:39:25 -07:00
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
if (slow_ramping_cycles) {
|
|
|
|
slow_ramping_cycles--;
|
|
|
|
step *= 0.1;
|
2014-03-30 15:51:59 -07:00
|
|
|
}
|
|
|
|
|
2014-05-10 01:21:47 -07:00
|
|
|
// Optionally apply startup boost.
|
2014-08-08 09:27:47 -07:00
|
|
|
if (fabsf(dutycycle_now_tmp) < start_boost) {
|
2014-07-27 10:40:38 -07:00
|
|
|
utils_step_towards(&dutycycle_now_tmp,
|
|
|
|
current_set > 0.0 ?
|
|
|
|
start_boost :
|
|
|
|
-start_boost, ramp_step);
|
|
|
|
} else {
|
|
|
|
dutycycle_now_tmp += step;
|
2014-03-30 15:51:59 -07:00
|
|
|
}
|
|
|
|
|
2014-05-08 14:44:27 -07:00
|
|
|
// Upper truncation
|
2015-12-08 12:01:23 -08:00
|
|
|
utils_truncate_number((float*)&dutycycle_now_tmp, -conf->l_max_duty, conf->l_max_duty);
|
2014-04-02 02:54:21 -07:00
|
|
|
|
2014-05-08 14:44:27 -07:00
|
|
|
// Lower truncation
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(dutycycle_now_tmp) < conf->l_min_duty) {
|
2014-07-27 10:40:38 -07:00
|
|
|
if (dutycycle_now_tmp < 0.0 && current_set > 0.0) {
|
2015-12-08 12:01:23 -08:00
|
|
|
dutycycle_now_tmp = conf->l_min_duty;
|
2014-07-27 10:40:38 -07:00
|
|
|
} else if (dutycycle_now_tmp > 0.0 && current_set < 0.0) {
|
2015-12-08 12:01:23 -08:00
|
|
|
dutycycle_now_tmp = -conf->l_min_duty;
|
2014-07-27 10:40:38 -07:00
|
|
|
}
|
|
|
|
}
|
2014-07-19 02:41:16 -07:00
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
// The set dutycycle should be in the correct direction in case the output is lower
|
|
|
|
// than the minimum duty cycle and the mechanism below gets activated.
|
2015-12-08 12:01:23 -08:00
|
|
|
dutycycle_set = dutycycle_now_tmp >= 0.0 ? conf->l_min_duty : -conf->l_min_duty;
|
2014-07-27 10:40:38 -07:00
|
|
|
} else if (control_mode == CONTROL_MODE_CURRENT_BRAKE) {
|
|
|
|
// Compute error
|
2014-08-06 09:44:15 -07:00
|
|
|
const float error = -fabsf(current_set) - current_nofilter;
|
2015-12-08 12:01:23 -08:00
|
|
|
float step = error * conf->cc_gain * voltage_scale;
|
2014-05-10 01:21:47 -07:00
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
// Do not ramp too much
|
2015-12-08 12:01:23 -08:00
|
|
|
utils_truncate_number(&step, -conf->cc_ramp_step_max, conf->cc_ramp_step_max);
|
2014-05-10 01:21:47 -07:00
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
// Switching frequency correction
|
2014-08-05 13:24:13 -07:00
|
|
|
step /= switching_frequency_now / 1000.0;
|
2014-05-10 01:21:47 -07:00
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
if (slow_ramping_cycles) {
|
|
|
|
slow_ramping_cycles--;
|
|
|
|
step *= 0.1;
|
|
|
|
}
|
|
|
|
|
|
|
|
dutycycle_now_tmp += SIGN(dutycycle_now_tmp) * step;
|
2014-05-08 14:44:27 -07:00
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
// Upper truncation
|
2015-12-08 12:01:23 -08:00
|
|
|
utils_truncate_number((float*)&dutycycle_now_tmp, -conf->l_max_duty, conf->l_max_duty);
|
2014-07-27 10:40:38 -07:00
|
|
|
|
|
|
|
// Lower truncation
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(dutycycle_now_tmp) < conf->l_min_duty) {
|
|
|
|
if (fabsf(rpm_now) < conf->l_max_erpm_fbrake_cc) {
|
2014-12-13 02:46:26 -08:00
|
|
|
dutycycle_now_tmp = 0.0;
|
|
|
|
dutycycle_set = dutycycle_now_tmp;
|
|
|
|
} else {
|
2015-12-08 12:01:23 -08:00
|
|
|
dutycycle_now_tmp = SIGN(dutycycle_now_tmp) * conf->l_min_duty;
|
2014-12-13 02:46:26 -08:00
|
|
|
dutycycle_set = dutycycle_now_tmp;
|
|
|
|
}
|
2014-05-08 14:44:27 -07:00
|
|
|
}
|
2014-03-30 15:51:59 -07:00
|
|
|
} else {
|
2014-07-27 10:40:38 -07:00
|
|
|
utils_step_towards((float*)&dutycycle_now_tmp, dutycycle_set, ramp_step);
|
2014-03-30 15:51:59 -07:00
|
|
|
}
|
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
static int limit_delay = 0;
|
|
|
|
|
2014-04-04 14:19:11 -07:00
|
|
|
// Apply limits in priority order
|
2015-12-08 12:01:23 -08:00
|
|
|
if (current_nofilter > conf->lo_current_max) {
|
2014-07-27 10:40:38 -07:00
|
|
|
utils_step_towards((float*) &dutycycle_now, 0.0,
|
2015-12-08 12:01:23 -08:00
|
|
|
ramp_step_no_lim * fabsf(current_nofilter - conf->lo_current_max) * conf->m_current_backoff_gain);
|
2014-07-27 10:40:38 -07:00
|
|
|
limit_delay = 1;
|
2015-12-08 12:01:23 -08:00
|
|
|
} else if (current_nofilter < conf->lo_current_min) {
|
|
|
|
utils_step_towards((float*) &dutycycle_now, direction ? conf->l_max_duty : -conf->l_max_duty,
|
|
|
|
ramp_step_no_lim * fabsf(current_nofilter - conf->lo_current_min) * conf->m_current_backoff_gain);
|
2014-07-27 10:40:38 -07:00
|
|
|
limit_delay = 1;
|
2015-12-08 12:01:23 -08:00
|
|
|
} else if (current_in_nofilter > conf->lo_in_current_max) {
|
2014-07-27 10:40:38 -07:00
|
|
|
utils_step_towards((float*) &dutycycle_now, 0.0,
|
2015-12-08 12:01:23 -08:00
|
|
|
ramp_step_no_lim * fabsf(current_in_nofilter - conf->lo_in_current_max) * conf->m_current_backoff_gain);
|
2014-07-27 10:40:38 -07:00
|
|
|
limit_delay = 1;
|
2015-12-08 12:01:23 -08:00
|
|
|
} else if (current_in_nofilter < conf->lo_in_current_min) {
|
|
|
|
utils_step_towards((float*) &dutycycle_now, direction ? conf->l_max_duty : -conf->l_max_duty,
|
|
|
|
ramp_step_no_lim * fabsf(current_in_nofilter - conf->lo_in_current_min) * conf->m_current_backoff_gain);
|
2014-07-27 10:40:38 -07:00
|
|
|
limit_delay = 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (limit_delay > 0) {
|
|
|
|
limit_delay--;
|
2014-01-09 06:20:26 -08:00
|
|
|
} else {
|
2014-03-30 15:51:59 -07:00
|
|
|
dutycycle_now = dutycycle_now_tmp;
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2014-01-18 13:10:10 -08:00
|
|
|
// When the set duty cycle is in the opposite direction, make sure that the motor
|
|
|
|
// starts again after stopping completely
|
2015-12-08 12:01:23 -08:00
|
|
|
if (fabsf(dutycycle_now) < conf->l_min_duty) {
|
|
|
|
if (dutycycle_set >= conf->l_min_duty) {
|
|
|
|
dutycycle_now = conf->l_min_duty;
|
|
|
|
} else if (dutycycle_set <= -conf->l_min_duty) {
|
|
|
|
dutycycle_now = -conf->l_min_duty;
|
2014-01-18 13:10:10 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-08-16 12:55:47 -07:00
|
|
|
// Don't start in the opposite direction when the RPM is too high even if the current is low enough.
|
2019-02-18 10:30:19 -08:00
|
|
|
if (conf->motor_type != MOTOR_TYPE_DC) {
|
|
|
|
const float rpm = mcpwm_get_rpm();
|
|
|
|
if (dutycycle_now >= conf->l_min_duty && rpm < -conf->l_max_erpm_fbrake) {
|
|
|
|
dutycycle_now = -conf->l_min_duty;
|
|
|
|
} else if (dutycycle_now <= -conf->l_min_duty && rpm > conf->l_max_erpm_fbrake) {
|
|
|
|
dutycycle_now = conf->l_min_duty;
|
|
|
|
}
|
2014-08-16 12:55:47 -07:00
|
|
|
}
|
|
|
|
|
2014-03-30 15:51:59 -07:00
|
|
|
set_duty_cycle_ll(dutycycle_now);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2020-03-16 10:32:39 -07:00
|
|
|
mc_interface_mc_timer_isr(false);
|
2015-10-03 16:43:26 -07:00
|
|
|
|
2022-01-09 08:10:40 -08:00
|
|
|
if (encoder_is_configured()) {
|
|
|
|
float pos = encoder_read_deg();
|
2019-02-18 10:30:19 -08:00
|
|
|
run_pid_control_pos(1.0 / switching_frequency_now, pos);
|
2021-10-13 09:13:51 -07:00
|
|
|
pll_run(-DEG2RAD_f(pos), 1.0 / switching_frequency_now, &m_pll_phase, &m_pll_speed);
|
2015-04-26 15:02:32 -07:00
|
|
|
}
|
|
|
|
|
2019-03-10 06:57:42 -07:00
|
|
|
last_adc_isr_duration = timer_seconds_elapsed_since(t_start);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2014-03-15 14:32:00 -07:00
|
|
|
void mcpwm_set_detect(void) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (mc_interface_try_input()) {
|
2014-03-21 07:40:05 -07:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-04-12 12:59:33 -07:00
|
|
|
control_mode = CONTROL_MODE_NONE;
|
2014-05-05 11:39:25 -07:00
|
|
|
stop_pwm_hw();
|
2014-04-01 10:26:46 -07:00
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
set_switching_frequency(conf->m_bldc_f_sw_max);
|
2014-03-15 14:32:00 -07:00
|
|
|
|
|
|
|
for(int i = 0;i < 6;i++) {
|
|
|
|
mcpwm_detect_currents[i] = 0;
|
|
|
|
mcpwm_detect_currents_avg[i] = 0;
|
2015-06-26 12:45:28 -07:00
|
|
|
mcpwm_detect_avg_samples[i] = 0;
|
2014-03-15 14:32:00 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
state = MC_STATE_DETECTING;
|
|
|
|
}
|
|
|
|
|
|
|
|
float mcpwm_get_detect_pos(void) {
|
|
|
|
float v[6];
|
2015-06-26 12:45:28 -07:00
|
|
|
v[0] = mcpwm_detect_currents_avg[0] / mcpwm_detect_avg_samples[0];
|
|
|
|
v[1] = mcpwm_detect_currents_avg[1] / mcpwm_detect_avg_samples[1];
|
|
|
|
v[2] = mcpwm_detect_currents_avg[2] / mcpwm_detect_avg_samples[2];
|
|
|
|
v[3] = mcpwm_detect_currents_avg[3] / mcpwm_detect_avg_samples[3];
|
|
|
|
v[4] = mcpwm_detect_currents_avg[4] / mcpwm_detect_avg_samples[4];
|
|
|
|
v[5] = mcpwm_detect_currents_avg[5] / mcpwm_detect_avg_samples[5];
|
2014-03-15 14:32:00 -07:00
|
|
|
|
|
|
|
for(int i = 0;i < 6;i++) {
|
|
|
|
mcpwm_detect_currents_avg[i] = 0;
|
2015-06-26 12:45:28 -07:00
|
|
|
mcpwm_detect_avg_samples[i] = 0;
|
2014-03-15 14:32:00 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
float v0 = v[0] + v[3];
|
|
|
|
float v1 = v[1] + v[4];
|
|
|
|
float v2 = v[2] + v[5];
|
|
|
|
|
|
|
|
float offset = (v0 + v1 + v2) / 3.0;
|
|
|
|
v0 -= offset;
|
|
|
|
v1 -= offset;
|
|
|
|
v2 -= offset;
|
|
|
|
|
|
|
|
float amp = sqrtf((v0*v0 + v1*v1 + v2*v2) / 1.5);
|
|
|
|
v0 /= amp;
|
|
|
|
v1 /= amp;
|
|
|
|
v2 /= amp;
|
|
|
|
|
|
|
|
float ph[1];
|
2021-10-13 09:13:51 -07:00
|
|
|
ph[0] = RAD2DEG_f(asinf(v0));
|
2014-03-15 14:32:00 -07:00
|
|
|
|
|
|
|
float res = ph[0];
|
|
|
|
if (v1 < v2) {
|
|
|
|
res = 180 - ph[0];
|
|
|
|
}
|
|
|
|
|
|
|
|
utils_norm_angle(&res);
|
|
|
|
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
2014-09-12 14:37:33 -07:00
|
|
|
float mcpwm_read_reset_avg_cycle_integrator(void) {
|
|
|
|
float res = cycle_integrator_sum / cycle_integrator_iterations;
|
|
|
|
cycle_integrator_sum = 0;
|
|
|
|
cycle_integrator_iterations = 0;
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* Set the commutation mode for sensorless commutation.
|
|
|
|
*
|
|
|
|
* @param mode
|
|
|
|
* COMM_MODE_INTEGRATE: More robust, but requires many parameters.
|
|
|
|
* COMM_MODE_DELAY: Like most hobby ESCs. Requires less parameters,
|
2015-02-19 12:20:07 -08:00
|
|
|
* but has worse startup and is less robust.
|
2014-09-12 14:37:33 -07:00
|
|
|
*
|
|
|
|
*/
|
|
|
|
void mcpwm_set_comm_mode(mc_comm_mode mode) {
|
2015-12-08 12:01:23 -08:00
|
|
|
conf->comm_mode = mode;
|
2014-09-12 14:37:33 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
mc_comm_mode mcpwm_get_comm_mode(void) {
|
2015-12-08 12:01:23 -08:00
|
|
|
return conf->comm_mode;
|
2014-09-12 14:37:33 -07:00
|
|
|
}
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
float mcpwm_get_last_adc_isr_duration(void) {
|
|
|
|
return last_adc_isr_duration;
|
|
|
|
}
|
|
|
|
|
|
|
|
float mcpwm_get_last_inj_adc_isr_duration(void) {
|
|
|
|
return last_inj_adc_isr_duration;
|
|
|
|
}
|
|
|
|
|
2014-09-13 01:46:46 -07:00
|
|
|
mc_rpm_dep_struct mcpwm_get_rpm_dep(void) {
|
|
|
|
return rpm_dep;
|
|
|
|
}
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
bool mcpwm_is_dccal_done(void) {
|
|
|
|
return dccal_done;
|
2015-10-03 16:43:26 -07:00
|
|
|
}
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
void mcpwm_switch_comm_mode(mc_comm_mode next) {
|
|
|
|
comm_mode_next = next;
|
|
|
|
}
|
|
|
|
|
2015-06-19 14:38:46 -07:00
|
|
|
/**
|
|
|
|
* Reset the hall sensor detection table
|
|
|
|
*/
|
|
|
|
void mcpwm_reset_hall_detect_table(void) {
|
2016-06-27 08:29:09 -07:00
|
|
|
memset((void*)hall_detect_table, 0, sizeof(hall_detect_table[0][0]) * 8 * 7);
|
2015-06-19 14:38:46 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* Get the current detected hall sensor table
|
|
|
|
*
|
|
|
|
* @param table
|
|
|
|
* Pointer to a table where the result should be stored
|
|
|
|
*
|
|
|
|
* @return
|
|
|
|
* 0: OK
|
|
|
|
* -1: Invalid hall sensor output
|
|
|
|
* -3: Encoder enabled
|
|
|
|
*/
|
|
|
|
int mcpwm_get_hall_detect_result(int8_t *table) {
|
2021-05-11 00:57:27 -07:00
|
|
|
if (conf->m_sensor_port_mode != SENSOR_PORT_MODE_HALL) {
|
2015-06-19 14:38:46 -07:00
|
|
|
return -3;
|
|
|
|
}
|
|
|
|
|
|
|
|
for (int i = 0;i < 8;i++) {
|
|
|
|
int samples = 0;
|
|
|
|
int res = -1;
|
|
|
|
for (int j = 1;j < 7;j++) {
|
|
|
|
if (hall_detect_table[i][j] > samples) {
|
|
|
|
samples = hall_detect_table[i][j];
|
|
|
|
if (samples > 15) {
|
|
|
|
res = j;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
table[i] = res;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
int invalid_samp_num = 0;
|
|
|
|
int nums[7] = {0, 0, 0, 0, 0, 0, 0};
|
|
|
|
int tot_nums = 0;
|
|
|
|
for (int i = 0;i < 8;i++) {
|
|
|
|
if (table[i] == -1) {
|
|
|
|
invalid_samp_num++;
|
|
|
|
} else {
|
|
|
|
if (!nums[table[i]]) {
|
|
|
|
nums[table[i]] = 1;
|
|
|
|
tot_nums++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (invalid_samp_num == 2 && tot_nums == 6) {
|
|
|
|
return 0;
|
|
|
|
} else {
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-06-28 10:35:11 -07:00
|
|
|
/**
|
|
|
|
* Read the current phase of the motor using hall effect sensors
|
|
|
|
* @return
|
|
|
|
* The phase read.
|
|
|
|
*/
|
2015-06-19 14:38:46 -07:00
|
|
|
int mcpwm_read_hall_phase(void) {
|
|
|
|
return hall_to_phase_table[read_hall() + (direction ? 8 : 0)];
|
|
|
|
}
|
|
|
|
|
|
|
|
static int read_hall(void) {
|
|
|
|
return READ_HALL1() | (READ_HALL2() << 1) | (READ_HALL3() << 2);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Commutation Steps FORWARDS
|
|
|
|
* STEP BR1 BR2 BR3
|
|
|
|
* 1 0 + -
|
|
|
|
* 2 + 0 -
|
|
|
|
* 3 + - 0
|
|
|
|
* 4 0 - +
|
|
|
|
* 5 - 0 +
|
|
|
|
* 6 - + 0
|
|
|
|
*
|
|
|
|
* Commutation Steps REVERSE (switch phase 2 and 3)
|
|
|
|
* STEP BR1 BR2 BR3
|
|
|
|
* 1 0 - +
|
|
|
|
* 2 + - 0
|
|
|
|
* 3 + 0 -
|
|
|
|
* 4 0 + -
|
|
|
|
* 5 - + 0
|
|
|
|
* 6 - 0 +
|
|
|
|
*/
|
|
|
|
|
2014-04-01 10:26:46 -07:00
|
|
|
static void update_adc_sample_pos(mc_timer_struct *timer_tmp) {
|
|
|
|
volatile uint32_t duty = timer_tmp->duty;
|
|
|
|
volatile uint32_t top = timer_tmp->top;
|
|
|
|
volatile uint32_t val_sample = timer_tmp->val_sample;
|
|
|
|
volatile uint32_t curr1_sample = timer_tmp->curr1_sample;
|
|
|
|
volatile uint32_t curr2_sample = timer_tmp->curr2_sample;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
volatile uint32_t curr3_sample = timer_tmp->curr3_sample;
|
|
|
|
#endif
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
if (duty > (uint32_t)((float)top * conf->l_max_duty)) {
|
|
|
|
duty = (uint32_t)((float)top * conf->l_max_duty);
|
2015-08-23 09:26:05 -07:00
|
|
|
}
|
|
|
|
|
2014-09-14 17:39:58 -07:00
|
|
|
curr_samp_volt = 0;
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_DC) {
|
2015-04-12 09:23:17 -07:00
|
|
|
curr1_sample = top - 10; // Not used anyway
|
|
|
|
curr2_sample = top - 10;
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
curr3_sample = top - 10;
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
if (duty > 1000) {
|
|
|
|
val_sample = duty / 2;
|
|
|
|
} else {
|
|
|
|
val_sample = duty + 800;
|
2019-02-19 13:18:30 -08:00
|
|
|
curr_samp_volt = (1 << 0) | (1 << 1) | (1 << 2);
|
2015-04-12 09:23:17 -07:00
|
|
|
}
|
|
|
|
|
2016-06-27 08:29:09 -07:00
|
|
|
// if (duty < (top / 2)) {
|
|
|
|
// val_sample = (top - duty) / 2 + duty;
|
|
|
|
// } else {
|
|
|
|
// val_sample = duty / 2;
|
|
|
|
// }
|
2014-03-15 14:32:00 -07:00
|
|
|
} else {
|
2015-04-12 09:23:17 -07:00
|
|
|
// Sample the ADC at an appropriate time during the pwm cycle
|
|
|
|
if (IS_DETECTING()) {
|
|
|
|
// Voltage samples
|
2015-06-26 12:45:28 -07:00
|
|
|
val_sample = duty / 2;
|
2014-03-15 14:32:00 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
// Current samples
|
|
|
|
curr1_sample = (top - duty) / 2 + duty;
|
|
|
|
curr2_sample = (top - duty) / 2 + duty;
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
curr3_sample = (top - duty) / 2 + duty;
|
|
|
|
#endif
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->pwm_mode == PWM_MODE_BIPOLAR) {
|
2015-04-12 09:23:17 -07:00
|
|
|
uint32_t samp_neg = top - 2;
|
|
|
|
uint32_t samp_pos = duty + (top - duty) / 2;
|
|
|
|
uint32_t samp_zero = top - 2;
|
|
|
|
|
|
|
|
// Voltage and other sampling
|
|
|
|
val_sample = top / 4;
|
|
|
|
|
|
|
|
// Current sampling
|
2016-06-27 08:29:09 -07:00
|
|
|
// TODO: Adapt for 3 shunts
|
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
curr3_sample = samp_zero;
|
|
|
|
#endif
|
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
switch (comm_step) {
|
|
|
|
case 1:
|
|
|
|
if (direction) {
|
|
|
|
curr1_sample = samp_zero;
|
|
|
|
curr2_sample = samp_neg;
|
2016-06-27 08:29:09 -07:00
|
|
|
curr_samp_volt = (1 << 1);
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
|
|
|
curr1_sample = samp_zero;
|
|
|
|
curr2_sample = samp_pos;
|
|
|
|
}
|
|
|
|
break;
|
2014-03-15 14:32:00 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
case 2:
|
|
|
|
if (direction) {
|
|
|
|
curr1_sample = samp_pos;
|
|
|
|
curr2_sample = samp_neg;
|
2016-06-27 08:29:09 -07:00
|
|
|
curr_samp_volt = (1 << 1);
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
|
|
|
curr1_sample = samp_pos;
|
|
|
|
curr2_sample = samp_zero;
|
|
|
|
}
|
|
|
|
break;
|
2014-03-15 14:32:00 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
case 3:
|
|
|
|
if (direction) {
|
|
|
|
curr1_sample = samp_pos;
|
|
|
|
curr2_sample = samp_zero;
|
|
|
|
} else {
|
|
|
|
curr1_sample = samp_pos;
|
|
|
|
curr2_sample = samp_neg;
|
2016-06-27 08:29:09 -07:00
|
|
|
curr_samp_volt = (1 << 1);
|
2015-04-12 09:23:17 -07:00
|
|
|
}
|
|
|
|
break;
|
2014-03-15 14:32:00 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
case 4:
|
|
|
|
if (direction) {
|
|
|
|
curr1_sample = samp_zero;
|
|
|
|
curr2_sample = samp_pos;
|
|
|
|
} else {
|
|
|
|
curr1_sample = samp_zero;
|
|
|
|
curr2_sample = samp_neg;
|
2016-06-27 08:29:09 -07:00
|
|
|
curr_samp_volt = (1 << 1);
|
2015-04-12 09:23:17 -07:00
|
|
|
}
|
|
|
|
break;
|
2014-03-15 14:32:00 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
case 5:
|
|
|
|
if (direction) {
|
|
|
|
curr1_sample = samp_neg;
|
|
|
|
curr2_sample = samp_pos;
|
2016-06-27 08:29:09 -07:00
|
|
|
curr_samp_volt = (1 << 0);
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
|
|
|
curr1_sample = samp_neg;
|
|
|
|
curr2_sample = samp_zero;
|
2016-06-27 08:29:09 -07:00
|
|
|
curr_samp_volt = (1 << 0);
|
2015-04-12 09:23:17 -07:00
|
|
|
}
|
|
|
|
break;
|
2014-03-15 14:32:00 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
case 6:
|
|
|
|
if (direction) {
|
|
|
|
curr1_sample = samp_neg;
|
|
|
|
curr2_sample = samp_zero;
|
2016-06-27 08:29:09 -07:00
|
|
|
curr_samp_volt = (1 << 0);
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
|
|
|
curr1_sample = samp_neg;
|
|
|
|
curr2_sample = samp_pos;
|
2016-06-27 08:29:09 -07:00
|
|
|
curr_samp_volt = (1 << 0);
|
2015-04-12 09:23:17 -07:00
|
|
|
}
|
|
|
|
break;
|
2014-03-15 14:32:00 -07:00
|
|
|
}
|
2015-04-12 09:23:17 -07:00
|
|
|
} else {
|
|
|
|
// Voltage samples
|
|
|
|
val_sample = duty / 2;
|
2014-03-15 14:32:00 -07:00
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
// Current samples
|
2015-08-23 09:26:05 -07:00
|
|
|
curr1_sample = duty + (top - duty) / 2;
|
|
|
|
if (curr1_sample > (top - 70)) {
|
|
|
|
curr1_sample = top - 70;
|
2014-03-15 14:32:00 -07:00
|
|
|
}
|
|
|
|
|
2015-04-12 09:23:17 -07:00
|
|
|
curr2_sample = curr1_sample;
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
curr3_sample = curr1_sample;
|
|
|
|
#endif
|
2015-08-23 09:26:05 -07:00
|
|
|
|
|
|
|
// The off sampling time is short, so use the on sampling time
|
|
|
|
// where possible
|
|
|
|
if (duty > (top / 2)) {
|
|
|
|
#if CURR1_DOUBLE_SAMPLE
|
|
|
|
if (comm_step == 2 || comm_step == 3) {
|
|
|
|
curr1_sample = duty + 90;
|
|
|
|
curr2_sample = top - 230;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
#if CURR2_DOUBLE_SAMPLE
|
|
|
|
if (direction) {
|
|
|
|
if (comm_step == 4 || comm_step == 5) {
|
|
|
|
curr1_sample = duty + 90;
|
|
|
|
curr2_sample = top - 230;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
if (comm_step == 1 || comm_step == 6) {
|
|
|
|
curr1_sample = duty + 90;
|
|
|
|
curr2_sample = top - 230;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
if (direction) {
|
|
|
|
switch (comm_step) {
|
|
|
|
case 1: curr_samp_volt = (1 << 0) || (1 << 2); break;
|
|
|
|
case 2: curr_samp_volt = (1 << 1) || (1 << 2); break;
|
|
|
|
case 3: curr_samp_volt = (1 << 1) || (1 << 2); break;
|
|
|
|
case 4: curr_samp_volt = (1 << 0) || (1 << 1); break;
|
|
|
|
case 5: curr_samp_volt = (1 << 0) || (1 << 1); break;
|
|
|
|
case 6: curr_samp_volt = (1 << 0) || (1 << 2); break;
|
|
|
|
default: break;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
switch (comm_step) {
|
|
|
|
case 1: curr_samp_volt = (1 << 0) || (1 << 1); break;
|
|
|
|
case 2: curr_samp_volt = (1 << 1) || (1 << 2); break;
|
|
|
|
case 3: curr_samp_volt = (1 << 1) || (1 << 2); break;
|
|
|
|
case 4: curr_samp_volt = (1 << 0) || (1 << 2); break;
|
|
|
|
case 5: curr_samp_volt = (1 << 0) || (1 << 2); break;
|
|
|
|
case 6: curr_samp_volt = (1 << 0) || (1 << 1); break;
|
|
|
|
default: break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#else
|
2015-08-23 09:26:05 -07:00
|
|
|
if (direction) {
|
|
|
|
switch (comm_step) {
|
2016-06-27 08:29:09 -07:00
|
|
|
case 1: curr_samp_volt = (1 << 0) || (1 << 1); break;
|
|
|
|
case 2: curr_samp_volt = (1 << 1); break;
|
|
|
|
case 3: curr_samp_volt = (1 << 1); break;
|
|
|
|
case 4: curr_samp_volt = (1 << 0); break;
|
|
|
|
case 5: curr_samp_volt = (1 << 0); break;
|
|
|
|
case 6: curr_samp_volt = (1 << 0) || (1 << 1); break;
|
2015-08-23 09:26:05 -07:00
|
|
|
default: break;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
switch (comm_step) {
|
2016-06-27 08:29:09 -07:00
|
|
|
case 1: curr_samp_volt = (1 << 0); break;
|
|
|
|
case 2: curr_samp_volt = (1 << 1); break;
|
|
|
|
case 3: curr_samp_volt = (1 << 1); break;
|
|
|
|
case 4: curr_samp_volt = (1 << 0) || (1 << 1); break;
|
|
|
|
case 5: curr_samp_volt = (1 << 0) || (1 << 1); break;
|
|
|
|
case 6: curr_samp_volt = (1 << 0); break;
|
2015-08-23 09:26:05 -07:00
|
|
|
default: break;
|
|
|
|
}
|
|
|
|
}
|
2016-06-27 08:29:09 -07:00
|
|
|
#endif
|
2015-08-23 09:26:05 -07:00
|
|
|
}
|
2015-04-12 09:23:17 -07:00
|
|
|
}
|
2014-03-15 14:32:00 -07:00
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
2014-04-01 10:26:46 -07:00
|
|
|
|
|
|
|
timer_tmp->val_sample = val_sample;
|
|
|
|
timer_tmp->curr1_sample = curr1_sample;
|
|
|
|
timer_tmp->curr2_sample = curr2_sample;
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
timer_tmp->curr3_sample = curr3_sample;
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
static void update_rpm_tacho(void) {
|
2014-09-16 01:44:56 -07:00
|
|
|
int step = comm_step - 1;
|
2014-01-09 06:20:26 -08:00
|
|
|
static int last_step = 0;
|
2014-09-16 01:44:56 -07:00
|
|
|
int tacho_diff = (step - last_step) % 6;
|
|
|
|
last_step = step;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-09-16 01:44:56 -07:00
|
|
|
if (tacho_diff > 3) {
|
|
|
|
tacho_diff -= 6;
|
|
|
|
} else if (tacho_diff < -2) {
|
|
|
|
tacho_diff += 6;
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
2014-09-16 01:44:56 -07:00
|
|
|
if (tacho_diff != 0) {
|
|
|
|
rpm_dep.comms += tacho_diff;
|
2019-03-10 06:57:42 -07:00
|
|
|
rpm_dep.time_at_comm += timer_seconds_elapsed_since(rpm_timer_start);
|
|
|
|
rpm_timer_start = timer_time_now();
|
2014-09-16 01:44:56 -07:00
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-03-29 16:57:22 -07:00
|
|
|
// Tachometers
|
|
|
|
tachometer_for_direction += tacho_diff;
|
2014-10-16 00:42:26 -07:00
|
|
|
tachometer_abs += tacho_diff;
|
2014-03-29 16:57:22 -07:00
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
if (direction) {
|
|
|
|
tachometer += tacho_diff;
|
|
|
|
} else {
|
|
|
|
tachometer -= tacho_diff;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-06-19 14:38:46 -07:00
|
|
|
static void update_sensor_mode(void) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->sensor_mode == SENSOR_MODE_SENSORLESS ||
|
|
|
|
(conf->sensor_mode == SENSOR_MODE_HYBRID &&
|
|
|
|
fabsf(mcpwm_get_rpm()) > conf->hall_sl_erpm)) {
|
2015-06-19 14:38:46 -07:00
|
|
|
sensorless_now = true;
|
|
|
|
} else {
|
|
|
|
sensorless_now = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-10-07 13:44:44 -07:00
|
|
|
static void commutate(int steps) {
|
2015-06-19 14:38:46 -07:00
|
|
|
last_pwm_cycles_sum = pwm_cycles_sum;
|
|
|
|
last_pwm_cycles_sums[comm_step - 1] = pwm_cycles_sum;
|
|
|
|
pwm_cycles_sum = 0;
|
2015-08-23 09:26:05 -07:00
|
|
|
pwm_cycles = 0;
|
2014-09-14 14:08:22 -07:00
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_BLDC && sensorless_now) {
|
2014-10-07 13:44:44 -07:00
|
|
|
comm_step += steps;
|
|
|
|
while (comm_step > 6) {
|
|
|
|
comm_step -= 6;
|
|
|
|
}
|
|
|
|
while (comm_step < 1) {
|
|
|
|
comm_step += 6;
|
2014-09-14 14:08:22 -07:00
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-10-07 13:44:44 -07:00
|
|
|
update_rpm_tacho();
|
|
|
|
|
2014-09-14 14:08:22 -07:00
|
|
|
if (!(state == MC_STATE_RUNNING)) {
|
2015-06-19 14:38:46 -07:00
|
|
|
update_sensor_mode();
|
2014-09-14 14:08:22 -07:00
|
|
|
return;
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-09-14 14:08:22 -07:00
|
|
|
set_next_comm_step(comm_step);
|
2014-03-16 08:09:23 -07:00
|
|
|
}
|
|
|
|
|
2014-04-13 14:15:53 -07:00
|
|
|
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
|
2014-07-27 10:40:38 -07:00
|
|
|
has_commutated = 1;
|
2014-04-13 14:15:53 -07:00
|
|
|
|
2014-04-01 10:26:46 -07:00
|
|
|
mc_timer_struct timer_tmp;
|
2014-08-29 21:21:39 -07:00
|
|
|
|
|
|
|
utils_sys_lock_cnt();
|
2014-10-16 00:42:26 -07:00
|
|
|
timer_tmp = timer_struct;
|
2014-08-29 21:21:39 -07:00
|
|
|
utils_sys_unlock_cnt();
|
|
|
|
|
2014-04-01 10:26:46 -07:00
|
|
|
update_adc_sample_pos(&timer_tmp);
|
|
|
|
set_next_timer_settings(&timer_tmp);
|
2015-06-19 14:38:46 -07:00
|
|
|
update_sensor_mode();
|
2017-09-04 12:12:43 -07:00
|
|
|
conf->comm_mode = comm_mode_next;
|
2014-04-01 10:26:46 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
static void set_next_timer_settings(mc_timer_struct *settings) {
|
2014-08-29 21:00:48 -07:00
|
|
|
utils_sys_lock_cnt();
|
2014-10-16 00:42:26 -07:00
|
|
|
timer_struct = *settings;
|
2014-10-24 14:04:10 -07:00
|
|
|
timer_struct.updated = false;
|
|
|
|
utils_sys_unlock_cnt();
|
|
|
|
|
|
|
|
update_timer_attempt();
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* Try to apply the new timer settings. This is really not an elegant solution, but for now it is
|
|
|
|
* the best I can come up with.
|
|
|
|
*/
|
|
|
|
static void update_timer_attempt(void) {
|
|
|
|
utils_sys_lock_cnt();
|
2014-10-16 00:42:26 -07:00
|
|
|
|
|
|
|
// Set the next timer settings if an update is far enough away
|
2014-10-24 14:04:10 -07:00
|
|
|
if (!timer_struct.updated && TIM1->CNT > 10 && TIM1->CNT < (TIM1->ARR - 500)) {
|
2014-10-16 00:42:26 -07:00
|
|
|
// Disable preload register updates
|
|
|
|
TIM1->CR1 |= TIM_CR1_UDIS;
|
|
|
|
TIM8->CR1 |= TIM_CR1_UDIS;
|
|
|
|
|
|
|
|
// Set the new configuration
|
|
|
|
TIM1->ARR = timer_struct.top;
|
|
|
|
TIM1->CCR1 = timer_struct.duty;
|
|
|
|
TIM1->CCR2 = timer_struct.duty;
|
|
|
|
TIM1->CCR3 = timer_struct.duty;
|
|
|
|
TIM8->CCR1 = timer_struct.val_sample;
|
|
|
|
TIM1->CCR4 = timer_struct.curr1_sample;
|
|
|
|
TIM8->CCR2 = timer_struct.curr2_sample;
|
2016-06-27 08:29:09 -07:00
|
|
|
#ifdef HW_HAS_3_SHUNTS
|
|
|
|
TIM8->CCR3 = timer_struct.curr3_sample;
|
|
|
|
#endif
|
2014-10-16 00:42:26 -07:00
|
|
|
|
|
|
|
// Enables preload register updates
|
|
|
|
TIM1->CR1 &= ~TIM_CR1_UDIS;
|
|
|
|
TIM8->CR1 &= ~TIM_CR1_UDIS;
|
2014-10-24 14:04:10 -07:00
|
|
|
timer_struct.updated = true;
|
2014-10-16 00:42:26 -07:00
|
|
|
}
|
2014-04-13 22:10:01 -07:00
|
|
|
|
2014-08-29 21:00:48 -07:00
|
|
|
utils_sys_unlock_cnt();
|
2014-04-01 10:26:46 -07:00
|
|
|
}
|
|
|
|
|
2014-08-05 13:24:13 -07:00
|
|
|
static void set_switching_frequency(float frequency) {
|
2014-04-01 10:26:46 -07:00
|
|
|
switching_frequency_now = frequency;
|
|
|
|
mc_timer_struct timer_tmp;
|
2014-08-29 21:21:39 -07:00
|
|
|
|
|
|
|
utils_sys_lock_cnt();
|
2014-10-16 00:42:26 -07:00
|
|
|
timer_tmp = timer_struct;
|
2014-08-29 21:21:39 -07:00
|
|
|
utils_sys_unlock_cnt();
|
|
|
|
|
2014-08-05 13:24:13 -07:00
|
|
|
timer_tmp.top = SYSTEM_CORE_CLOCK / (int)switching_frequency_now;
|
2014-04-01 10:26:46 -07:00
|
|
|
update_adc_sample_pos(&timer_tmp);
|
|
|
|
set_next_timer_settings(&timer_tmp);
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
static void set_next_comm_step(int next_step) {
|
2015-12-08 12:01:23 -08:00
|
|
|
if (conf->motor_type == MOTOR_TYPE_DC) {
|
2015-04-12 09:23:17 -07:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
if (direction) {
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_PWM1);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable);
|
|
|
|
} else {
|
|
|
|
// +
|
|
|
|
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_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
|
|
|
|
}
|
|
|
|
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-01-09 06:20:26 -08:00
|
|
|
uint16_t positive_oc_mode = TIM_OCMode_PWM1;
|
|
|
|
uint16_t negative_oc_mode = TIM_OCMode_Inactive;
|
|
|
|
|
|
|
|
uint16_t positive_highside = TIM_CCx_Enable;
|
|
|
|
uint16_t positive_lowside = TIM_CCxN_Enable;
|
|
|
|
|
|
|
|
uint16_t negative_highside = TIM_CCx_Enable;
|
|
|
|
uint16_t negative_lowside = TIM_CCxN_Enable;
|
|
|
|
|
2014-04-21 07:17:39 -07:00
|
|
|
if (!IS_DETECTING()) {
|
2015-12-08 12:01:23 -08:00
|
|
|
switch (conf->pwm_mode) {
|
2014-03-13 07:28:56 -07:00
|
|
|
case PWM_MODE_NONSYNCHRONOUS_HISW:
|
|
|
|
positive_lowside = TIM_CCxN_Disable;
|
|
|
|
break;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-03-13 07:28:56 -07:00
|
|
|
case PWM_MODE_SYNCHRONOUS:
|
|
|
|
break;
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2014-03-13 07:28:56 -07:00
|
|
|
case PWM_MODE_BIPOLAR:
|
|
|
|
negative_oc_mode = TIM_OCMode_PWM2;
|
|
|
|
break;
|
|
|
|
}
|
2014-01-09 06:20:26 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
if (next_step == 1) {
|
|
|
|
if (direction) {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR1();
|
|
|
|
ENABLE_BR2();
|
|
|
|
ENABLE_BR3();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, negative_lowside);
|
|
|
|
} else {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR1();
|
|
|
|
ENABLE_BR3();
|
|
|
|
ENABLE_BR2();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, negative_lowside);
|
|
|
|
}
|
|
|
|
} else if (next_step == 2) {
|
|
|
|
if (direction) {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR2();
|
|
|
|
ENABLE_BR1();
|
|
|
|
ENABLE_BR3();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, negative_lowside);
|
|
|
|
} else {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR3();
|
|
|
|
ENABLE_BR1();
|
|
|
|
ENABLE_BR2();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, negative_lowside);
|
|
|
|
}
|
|
|
|
} else if (next_step == 3) {
|
|
|
|
if (direction) {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR3();
|
|
|
|
ENABLE_BR1();
|
|
|
|
ENABLE_BR2();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, negative_lowside);
|
|
|
|
} else {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR2();
|
|
|
|
ENABLE_BR1();
|
|
|
|
ENABLE_BR3();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, negative_lowside);
|
|
|
|
}
|
|
|
|
} else if (next_step == 4) {
|
|
|
|
if (direction) {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR1();
|
|
|
|
ENABLE_BR3();
|
|
|
|
ENABLE_BR2();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, negative_lowside);
|
|
|
|
} else {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR1();
|
|
|
|
ENABLE_BR2();
|
|
|
|
ENABLE_BR3();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, negative_lowside);
|
|
|
|
}
|
|
|
|
} else if (next_step == 5) {
|
|
|
|
if (direction) {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR2();
|
|
|
|
ENABLE_BR3();
|
|
|
|
ENABLE_BR1();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, negative_lowside);
|
|
|
|
} else {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR3();
|
|
|
|
ENABLE_BR2();
|
|
|
|
ENABLE_BR1();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, negative_lowside);
|
|
|
|
}
|
|
|
|
} else if (next_step == 6) {
|
|
|
|
if (direction) {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR3();
|
|
|
|
ENABLE_BR2();
|
|
|
|
ENABLE_BR1();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, negative_lowside);
|
|
|
|
} else {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR2();
|
|
|
|
ENABLE_BR3();
|
|
|
|
ENABLE_BR1();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// 0
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_Inactive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
// +
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, positive_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, positive_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, positive_lowside);
|
|
|
|
|
|
|
|
// -
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, negative_oc_mode);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, negative_highside);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, negative_lowside);
|
|
|
|
}
|
|
|
|
} else {
|
2016-11-04 07:18:34 -07:00
|
|
|
#ifdef HW_HAS_DRV8313
|
|
|
|
DISABLE_BR1();
|
|
|
|
DISABLE_BR2();
|
|
|
|
DISABLE_BR3();
|
|
|
|
#endif
|
2014-01-09 06:20:26 -08:00
|
|
|
// Invalid phase.. stop PWM!
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_ForcedAction_InActive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_ForcedAction_InActive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable);
|
|
|
|
|
|
|
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_ForcedAction_InActive);
|
|
|
|
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
|
|
|
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable);
|
|
|
|
}
|
|
|
|
}
|