mirror of https://github.com/rusefi/bldc.git
FW 2.11: Refactoring, dead time comp fix for smoother braking when the speed is zero
This commit is contained in:
parent
e6ea308a48
commit
02f140e335
|
@ -25,7 +25,7 @@
|
||||||
#include "commands.h"
|
#include "commands.h"
|
||||||
#include "ch.h"
|
#include "ch.h"
|
||||||
#include "hal.h"
|
#include "hal.h"
|
||||||
#include "main.h"
|
#include "mc_interface.h"
|
||||||
#include "stm32f4xx_conf.h"
|
#include "stm32f4xx_conf.h"
|
||||||
#include "servo.h"
|
#include "servo.h"
|
||||||
#include "servo_simple.h"
|
#include "servo_simple.h"
|
||||||
|
@ -577,7 +577,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
||||||
at_start = data[ind++];
|
at_start = data[ind++];
|
||||||
sample_len = buffer_get_uint16(data, &ind);
|
sample_len = buffer_get_uint16(data, &ind);
|
||||||
decimation = data[ind++];
|
decimation = data[ind++];
|
||||||
main_sample_print_data(at_start, sample_len, decimation);
|
mc_interface_sample_print_data(at_start, sample_len, decimation);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case COMM_TERMINAL_CMD:
|
case COMM_TERMINAL_CMD:
|
||||||
|
|
|
@ -27,7 +27,7 @@
|
||||||
|
|
||||||
// Firmware version
|
// Firmware version
|
||||||
#define FW_VERSION_MAJOR 2
|
#define FW_VERSION_MAJOR 2
|
||||||
#define FW_VERSION_MINOR 10
|
#define FW_VERSION_MINOR 11
|
||||||
|
|
||||||
#include "datatypes.h"
|
#include "datatypes.h"
|
||||||
|
|
||||||
|
|
|
@ -19,7 +19,6 @@
|
||||||
#include "hal.h"
|
#include "hal.h"
|
||||||
#include "stm32f4xx_conf.h"
|
#include "stm32f4xx_conf.h"
|
||||||
#include "isr_vector_table.h"
|
#include "isr_vector_table.h"
|
||||||
#include "main.h"
|
|
||||||
#include "mc_interface.h"
|
#include "mc_interface.h"
|
||||||
#include "servo.h"
|
#include "servo.h"
|
||||||
#include "hw.h"
|
#include "hw.h"
|
||||||
|
|
154
main.c
154
main.c
|
@ -24,7 +24,6 @@
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
|
||||||
#include "main.h"
|
|
||||||
#include "mc_interface.h"
|
#include "mc_interface.h"
|
||||||
#include "mcpwm.h"
|
#include "mcpwm.h"
|
||||||
#include "mcpwm_foc.h"
|
#include "mcpwm_foc.h"
|
||||||
|
@ -68,48 +67,12 @@
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/*
|
|
||||||
* Notes:
|
|
||||||
*
|
|
||||||
* Disable USB VBUS sensing:
|
|
||||||
* ChibiOS-RT-master/os/hal/platforms/STM32/OTGv1/usb_lld.c
|
|
||||||
*
|
|
||||||
* change
|
|
||||||
* otgp->GCCFG = GCCFG_VBUSASEN | GCCFG_VBUSBSEN | GCCFG_PWRDWN;
|
|
||||||
* to
|
|
||||||
* otgp->GCCFG = GCCFG_NOVBUSSENS | GCCFG_PWRDWN;
|
|
||||||
*
|
|
||||||
* This should be handled automatically with the latest version of
|
|
||||||
* ChibiOS since I have added an option to the makefile.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
// Private variables
|
// Private variables
|
||||||
#define ADC_SAMPLE_MAX_LEN 2000
|
|
||||||
static volatile int16_t curr0_samples[ADC_SAMPLE_MAX_LEN];
|
|
||||||
static volatile int16_t curr1_samples[ADC_SAMPLE_MAX_LEN];
|
|
||||||
static volatile int16_t ph1_samples[ADC_SAMPLE_MAX_LEN];
|
|
||||||
static volatile int16_t ph2_samples[ADC_SAMPLE_MAX_LEN];
|
|
||||||
static volatile int16_t ph3_samples[ADC_SAMPLE_MAX_LEN];
|
|
||||||
static volatile int16_t vzero_samples[ADC_SAMPLE_MAX_LEN];
|
|
||||||
static volatile uint8_t status_samples[ADC_SAMPLE_MAX_LEN];
|
|
||||||
static volatile int16_t curr_fir_samples[ADC_SAMPLE_MAX_LEN];
|
|
||||||
static volatile int16_t f_sw_samples[ADC_SAMPLE_MAX_LEN];
|
|
||||||
|
|
||||||
static volatile int sample_len = 1000;
|
|
||||||
static volatile int sample_int = 1;
|
|
||||||
static volatile int sample_ready = 1;
|
|
||||||
static volatile int sample_now = 0;
|
|
||||||
static volatile int sample_at_start = 0;
|
|
||||||
static volatile int start_comm = 0;
|
|
||||||
static volatile float main_last_adc_duration = 0.0;
|
|
||||||
|
|
||||||
static THD_WORKING_AREA(periodic_thread_wa, 1024);
|
static THD_WORKING_AREA(periodic_thread_wa, 1024);
|
||||||
static THD_WORKING_AREA(sample_send_thread_wa, 1024);
|
|
||||||
static THD_WORKING_AREA(timer_thread_wa, 128);
|
static THD_WORKING_AREA(timer_thread_wa, 128);
|
||||||
|
|
||||||
static thread_t *sample_send_tp;
|
|
||||||
|
|
||||||
static THD_FUNCTION(periodic_thread, arg) {
|
static THD_FUNCTION(periodic_thread, arg) {
|
||||||
(void)arg;
|
(void)arg;
|
||||||
|
|
||||||
|
@ -174,43 +137,6 @@ static THD_FUNCTION(periodic_thread, arg) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static THD_FUNCTION(sample_send_thread, arg) {
|
|
||||||
(void)arg;
|
|
||||||
|
|
||||||
chRegSetThreadName("Main sample");
|
|
||||||
|
|
||||||
sample_send_tp = chThdGetSelfX();
|
|
||||||
|
|
||||||
for(;;) {
|
|
||||||
chEvtWaitAny((eventmask_t) 1);
|
|
||||||
|
|
||||||
for (int i = 0;i < sample_len;i++) {
|
|
||||||
uint8_t buffer[20];
|
|
||||||
int index = 0;
|
|
||||||
|
|
||||||
buffer[index++] = curr0_samples[i] >> 8;
|
|
||||||
buffer[index++] = curr0_samples[i];
|
|
||||||
buffer[index++] = curr1_samples[i] >> 8;
|
|
||||||
buffer[index++] = curr1_samples[i];
|
|
||||||
buffer[index++] = ph1_samples[i] >> 8;
|
|
||||||
buffer[index++] = ph1_samples[i];
|
|
||||||
buffer[index++] = ph2_samples[i] >> 8;
|
|
||||||
buffer[index++] = ph2_samples[i];
|
|
||||||
buffer[index++] = ph3_samples[i] >> 8;
|
|
||||||
buffer[index++] = ph3_samples[i];
|
|
||||||
buffer[index++] = vzero_samples[i] >> 8;
|
|
||||||
buffer[index++] = vzero_samples[i];
|
|
||||||
buffer[index++] = status_samples[i];
|
|
||||||
buffer[index++] = curr_fir_samples[i] >> 8;
|
|
||||||
buffer[index++] = curr_fir_samples[i];
|
|
||||||
buffer[index++] = f_sw_samples[i] >> 8;
|
|
||||||
buffer[index++] = f_sw_samples[i];
|
|
||||||
|
|
||||||
commands_send_samples(buffer, index);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static THD_FUNCTION(timer_thread, arg) {
|
static THD_FUNCTION(timer_thread, arg) {
|
||||||
(void)arg;
|
(void)arg;
|
||||||
|
|
||||||
|
@ -222,85 +148,6 @@ static THD_FUNCTION(timer_thread, arg) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
* Called every time new ADC values are available. Note that
|
|
||||||
* the ADC is initialized from mcpwm.c
|
|
||||||
*/
|
|
||||||
void main_dma_adc_handler(void) {
|
|
||||||
ledpwm_update_pwm();
|
|
||||||
|
|
||||||
if (sample_at_start && (mc_interface_get_state() == MC_STATE_RUNNING ||
|
|
||||||
start_comm != mcpwm_get_comm_step())) {
|
|
||||||
sample_now = 0;
|
|
||||||
sample_ready = 0;
|
|
||||||
sample_at_start = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int a = 0;
|
|
||||||
if (!sample_ready) {
|
|
||||||
a++;
|
|
||||||
if (a >= sample_int) {
|
|
||||||
a = 0;
|
|
||||||
|
|
||||||
if (mc_interface_get_state() == MC_STATE_DETECTING) {
|
|
||||||
curr0_samples[sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
|
|
||||||
curr1_samples[sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
|
|
||||||
|
|
||||||
ph1_samples[sample_now] = (int16_t)mcpwm_detect_voltages[0];
|
|
||||||
ph2_samples[sample_now] = (int16_t)mcpwm_detect_voltages[1];
|
|
||||||
ph3_samples[sample_now] = (int16_t)mcpwm_detect_voltages[2];
|
|
||||||
} else {
|
|
||||||
curr0_samples[sample_now] = ADC_curr_norm_value[0];
|
|
||||||
curr1_samples[sample_now] = ADC_curr_norm_value[1];
|
|
||||||
|
|
||||||
ph1_samples[sample_now] = ADC_V_L1 - mcpwm_vzero;
|
|
||||||
ph2_samples[sample_now] = ADC_V_L2 - mcpwm_vzero;
|
|
||||||
ph3_samples[sample_now] = ADC_V_L3 - mcpwm_vzero;
|
|
||||||
}
|
|
||||||
|
|
||||||
vzero_samples[sample_now] = mcpwm_vzero;
|
|
||||||
|
|
||||||
curr_fir_samples[sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
|
|
||||||
f_sw_samples[sample_now] = (int16_t)(mc_interface_get_switching_frequency_now() / 10.0);
|
|
||||||
|
|
||||||
status_samples[sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
|
|
||||||
|
|
||||||
sample_now++;
|
|
||||||
|
|
||||||
if (sample_now == sample_len) {
|
|
||||||
sample_ready = 1;
|
|
||||||
sample_now = 0;
|
|
||||||
chSysLockFromISR();
|
|
||||||
chEvtSignalI(sample_send_tp, (eventmask_t) 1);
|
|
||||||
chSysUnlockFromISR();
|
|
||||||
}
|
|
||||||
|
|
||||||
main_last_adc_duration = mcpwm_get_last_adc_isr_duration();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
float main_get_last_adc_isr_duration(void) {
|
|
||||||
return main_last_adc_duration;
|
|
||||||
}
|
|
||||||
|
|
||||||
void main_sample_print_data(bool at_start, uint16_t len, uint8_t decimation) {
|
|
||||||
if (len > ADC_SAMPLE_MAX_LEN) {
|
|
||||||
len = ADC_SAMPLE_MAX_LEN;
|
|
||||||
}
|
|
||||||
|
|
||||||
sample_len = len;
|
|
||||||
sample_int = decimation;
|
|
||||||
|
|
||||||
if (at_start) {
|
|
||||||
sample_at_start = 1;
|
|
||||||
start_comm = mcpwm_get_comm_step();
|
|
||||||
} else {
|
|
||||||
sample_now = 0;
|
|
||||||
sample_ready = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(void) {
|
int main(void) {
|
||||||
halInit();
|
halInit();
|
||||||
chSysInit();
|
chSysInit();
|
||||||
|
@ -352,7 +199,6 @@ int main(void) {
|
||||||
|
|
||||||
// Threads
|
// Threads
|
||||||
chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
|
chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
|
||||||
chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
|
|
||||||
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
||||||
|
|
||||||
for(;;) {
|
for(;;) {
|
||||||
|
|
36
main.h
36
main.h
|
@ -1,36 +0,0 @@
|
||||||
/*
|
|
||||||
Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se
|
|
||||||
|
|
||||||
This program is free software: you can redistribute it and/or modify
|
|
||||||
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.
|
|
||||||
|
|
||||||
This program is distributed in the hope that it will be useful,
|
|
||||||
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/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/*
|
|
||||||
* main.h
|
|
||||||
*
|
|
||||||
* Created on: 10 jul 2012
|
|
||||||
* Author: BenjaminVe
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef MAIN_H_
|
|
||||||
#define MAIN_H_
|
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
#include "conf_general.h"
|
|
||||||
|
|
||||||
// Function prototypes
|
|
||||||
void main_dma_adc_handler(void);
|
|
||||||
float main_get_last_adc_isr_duration(void);
|
|
||||||
void main_sample_print_data(bool at_start, uint16_t len, uint8_t decimation);
|
|
||||||
|
|
||||||
#endif /* MAIN_H_ */
|
|
383
mc_interface.c
383
mc_interface.c
|
@ -25,13 +25,14 @@
|
||||||
#include "mc_interface.h"
|
#include "mc_interface.h"
|
||||||
#include "mcpwm.h"
|
#include "mcpwm.h"
|
||||||
#include "mcpwm_foc.h"
|
#include "mcpwm_foc.h"
|
||||||
#include "main.h"
|
#include "ledpwm.h"
|
||||||
#include "stm32f4xx_conf.h"
|
#include "stm32f4xx_conf.h"
|
||||||
#include "hw.h"
|
#include "hw.h"
|
||||||
#include "terminal.h"
|
#include "terminal.h"
|
||||||
#include "utils.h"
|
#include "utils.h"
|
||||||
#include "ch.h"
|
#include "ch.h"
|
||||||
#include "hal.h"
|
#include "hal.h"
|
||||||
|
#include "commands.h"
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
// Global variables
|
// Global variables
|
||||||
|
@ -39,21 +40,40 @@ volatile uint16_t ADC_Value[HW_ADC_CHANNELS];
|
||||||
volatile int ADC_curr_norm_value[3];
|
volatile int ADC_curr_norm_value[3];
|
||||||
|
|
||||||
// Private variables
|
// Private variables
|
||||||
static volatile mc_configuration conf;
|
static volatile mc_configuration m_conf;
|
||||||
static mc_fault_code fault_now;
|
static mc_fault_code m_fault_now;
|
||||||
static int ignore_iterations;
|
static int m_ignore_iterations;
|
||||||
static volatile unsigned int cycles_running;
|
static volatile unsigned int m_cycles_running;
|
||||||
static volatile bool lock_enabled;
|
static volatile bool m_lock_enabled;
|
||||||
static volatile bool lock_override_once;
|
static volatile bool m_lock_override_once;
|
||||||
static volatile float motor_current_sum;
|
static volatile float m_motor_current_sum;
|
||||||
static volatile float input_current_sum;
|
static volatile float m_input_current_sum;
|
||||||
static volatile float motor_current_iterations;
|
static volatile float m_motor_current_iterations;
|
||||||
static volatile float input_current_iterations;
|
static volatile float m_input_current_iterations;
|
||||||
static volatile float amp_seconds;
|
static volatile float m_amp_seconds;
|
||||||
static volatile float amp_seconds_charged;
|
static volatile float m_amp_seconds_charged;
|
||||||
static volatile float watt_seconds;
|
static volatile float m_watt_seconds;
|
||||||
static volatile float watt_seconds_charged;
|
static volatile float m_watt_seconds_charged;
|
||||||
static volatile float position_set;
|
static volatile float m_position_set;
|
||||||
|
|
||||||
|
// Sampling variables
|
||||||
|
#define ADC_SAMPLE_MAX_LEN 2000
|
||||||
|
static volatile int16_t m_curr0_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
|
static volatile int16_t m_curr1_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
|
static volatile int16_t m_ph1_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
|
static volatile int16_t m_ph2_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
|
static volatile int16_t m_ph3_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
|
static volatile int16_t m_vzero_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
|
static volatile uint8_t m_status_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
|
static volatile int16_t m_curr_fir_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
|
static volatile int16_t m_f_sw_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
|
static volatile int m_sample_len;
|
||||||
|
static volatile int m_sample_int;
|
||||||
|
static volatile int m_sample_ready;
|
||||||
|
static volatile int m_sample_now;
|
||||||
|
static volatile int m_sample_at_start;
|
||||||
|
static volatile int m_start_comm;
|
||||||
|
static volatile float m_last_adc_duration_sample;
|
||||||
|
|
||||||
// Private functions
|
// Private functions
|
||||||
static void update_override_limits(volatile mc_configuration *conf);
|
static void update_override_limits(volatile mc_configuration *conf);
|
||||||
|
@ -64,36 +84,48 @@ static void(*pwn_done_func)(void) = 0;
|
||||||
// Threads
|
// Threads
|
||||||
static THD_WORKING_AREA(timer_thread_wa, 1024);
|
static THD_WORKING_AREA(timer_thread_wa, 1024);
|
||||||
static THD_FUNCTION(timer_thread, arg);
|
static THD_FUNCTION(timer_thread, arg);
|
||||||
|
static THD_WORKING_AREA(sample_send_thread_wa, 1024);
|
||||||
|
static THD_FUNCTION(sample_send_thread, arg);
|
||||||
|
static thread_t *sample_send_tp;
|
||||||
|
|
||||||
void mc_interface_init(mc_configuration *configuration) {
|
void mc_interface_init(mc_configuration *configuration) {
|
||||||
conf = *configuration;
|
m_conf = *configuration;
|
||||||
fault_now = FAULT_CODE_NONE;
|
m_fault_now = FAULT_CODE_NONE;
|
||||||
ignore_iterations = 0;
|
m_ignore_iterations = 0;
|
||||||
cycles_running = 0;
|
m_cycles_running = 0;
|
||||||
lock_enabled = false;
|
m_lock_enabled = false;
|
||||||
lock_override_once = false;
|
m_lock_override_once = false;
|
||||||
motor_current_sum = 0.0;
|
m_motor_current_sum = 0.0;
|
||||||
input_current_sum = 0.0;
|
m_input_current_sum = 0.0;
|
||||||
motor_current_iterations = 0.0;
|
m_motor_current_iterations = 0.0;
|
||||||
input_current_iterations = 0.0;
|
m_input_current_iterations = 0.0;
|
||||||
amp_seconds = 0.0;
|
m_amp_seconds = 0.0;
|
||||||
amp_seconds_charged = 0.0;
|
m_amp_seconds_charged = 0.0;
|
||||||
watt_seconds = 0.0;
|
m_watt_seconds = 0.0;
|
||||||
watt_seconds_charged = 0.0;
|
m_watt_seconds_charged = 0.0;
|
||||||
position_set = 0.0;
|
m_position_set = 0.0;
|
||||||
|
m_last_adc_duration_sample = 0.0;
|
||||||
|
|
||||||
|
m_sample_len = 1000;
|
||||||
|
m_sample_int = 1;
|
||||||
|
m_sample_ready = 1;
|
||||||
|
m_sample_now = 0;
|
||||||
|
m_sample_at_start = 0;
|
||||||
|
m_start_comm = 0;
|
||||||
|
|
||||||
// Start threads
|
// Start threads
|
||||||
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
||||||
|
chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
|
||||||
|
|
||||||
// Initialize selected implementation
|
// Initialize selected implementation
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
mcpwm_init(&conf);
|
mcpwm_init(&m_conf);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MOTOR_TYPE_FOC:
|
case MOTOR_TYPE_FOC:
|
||||||
mcpwm_foc_init(&conf);
|
mcpwm_foc_init(&m_conf);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
|
@ -102,34 +134,34 @@ void mc_interface_init(mc_configuration *configuration) {
|
||||||
}
|
}
|
||||||
|
|
||||||
const volatile mc_configuration* mc_interface_get_configuration(void) {
|
const volatile mc_configuration* mc_interface_get_configuration(void) {
|
||||||
return &conf;
|
return &m_conf;
|
||||||
}
|
}
|
||||||
|
|
||||||
void mc_interface_set_configuration(mc_configuration *configuration) {
|
void mc_interface_set_configuration(mc_configuration *configuration) {
|
||||||
if (conf.motor_type == MOTOR_TYPE_FOC
|
if (m_conf.motor_type == MOTOR_TYPE_FOC
|
||||||
&& configuration->motor_type != MOTOR_TYPE_FOC) {
|
&& configuration->motor_type != MOTOR_TYPE_FOC) {
|
||||||
mcpwm_foc_deinit();
|
mcpwm_foc_deinit();
|
||||||
conf = *configuration;
|
m_conf = *configuration;
|
||||||
mcpwm_init(&conf);
|
mcpwm_init(&m_conf);
|
||||||
} else if (conf.motor_type != MOTOR_TYPE_FOC
|
} else if (m_conf.motor_type != MOTOR_TYPE_FOC
|
||||||
&& configuration->motor_type == MOTOR_TYPE_FOC) {
|
&& configuration->motor_type == MOTOR_TYPE_FOC) {
|
||||||
mcpwm_deinit();
|
mcpwm_deinit();
|
||||||
conf = *configuration;
|
m_conf = *configuration;
|
||||||
mcpwm_foc_init(&conf);
|
mcpwm_foc_init(&m_conf);
|
||||||
} else {
|
} else {
|
||||||
conf = *configuration;
|
m_conf = *configuration;
|
||||||
}
|
}
|
||||||
|
|
||||||
update_override_limits(&conf);
|
update_override_limits(&m_conf);
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
mcpwm_set_configuration(&conf);
|
mcpwm_set_configuration(&m_conf);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MOTOR_TYPE_FOC:
|
case MOTOR_TYPE_FOC:
|
||||||
mcpwm_foc_set_configuration(&conf);
|
mcpwm_foc_set_configuration(&m_conf);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
|
@ -139,7 +171,7 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
|
||||||
|
|
||||||
bool mc_interface_dccal_done(void) {
|
bool mc_interface_dccal_done(void) {
|
||||||
bool ret = false;
|
bool ret = false;
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_is_dccal_done();
|
ret = mcpwm_is_dccal_done();
|
||||||
|
@ -170,25 +202,25 @@ void mc_interface_set_pwm_callback(void (*p_func)(void)) {
|
||||||
* Lock the control by disabling all control commands.
|
* Lock the control by disabling all control commands.
|
||||||
*/
|
*/
|
||||||
void mc_interface_lock(void) {
|
void mc_interface_lock(void) {
|
||||||
lock_enabled = true;
|
m_lock_enabled = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Unlock all control commands.
|
* Unlock all control commands.
|
||||||
*/
|
*/
|
||||||
void mc_interface_unlock(void) {
|
void mc_interface_unlock(void) {
|
||||||
lock_enabled = false;
|
m_lock_enabled = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Allow just one motor control command in the locked state.
|
* Allow just one motor control command in the locked state.
|
||||||
*/
|
*/
|
||||||
void mc_interface_lock_override_once(void) {
|
void mc_interface_lock_override_once(void) {
|
||||||
lock_override_once = true;
|
m_lock_override_once = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
mc_fault_code mc_interface_get_fault(void) {
|
mc_fault_code mc_interface_get_fault(void) {
|
||||||
return fault_now;
|
return m_fault_now;
|
||||||
}
|
}
|
||||||
|
|
||||||
const char* mc_interface_fault_to_string(mc_fault_code fault) {
|
const char* mc_interface_fault_to_string(mc_fault_code fault) {
|
||||||
|
@ -206,7 +238,7 @@ const char* mc_interface_fault_to_string(mc_fault_code fault) {
|
||||||
|
|
||||||
mc_state mc_interface_get_state(void) {
|
mc_state mc_interface_get_state(void) {
|
||||||
mc_state ret = MC_STATE_OFF;
|
mc_state ret = MC_STATE_OFF;
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_state();
|
ret = mcpwm_get_state();
|
||||||
|
@ -228,7 +260,7 @@ void mc_interface_set_duty(float dutyCycle) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
mcpwm_set_duty(dutyCycle);
|
mcpwm_set_duty(dutyCycle);
|
||||||
|
@ -248,7 +280,7 @@ void mc_interface_set_duty_noramp(float dutyCycle) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
mcpwm_set_duty_noramp(dutyCycle);
|
mcpwm_set_duty_noramp(dutyCycle);
|
||||||
|
@ -268,7 +300,7 @@ void mc_interface_set_pid_speed(float rpm) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
mcpwm_set_pid_speed(rpm);
|
mcpwm_set_pid_speed(rpm);
|
||||||
|
@ -288,9 +320,9 @@ void mc_interface_set_pid_pos(float pos) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
position_set = pos;
|
m_position_set = pos;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
mcpwm_set_pid_pos(pos);
|
mcpwm_set_pid_pos(pos);
|
||||||
|
@ -310,7 +342,7 @@ void mc_interface_set_current(float current) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
mcpwm_set_current(current);
|
mcpwm_set_current(current);
|
||||||
|
@ -330,7 +362,7 @@ void mc_interface_set_brake_current(float current) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
mcpwm_set_brake_current(current);
|
mcpwm_set_brake_current(current);
|
||||||
|
@ -362,7 +394,7 @@ void mc_interface_release_motor(void) {
|
||||||
float mc_interface_get_duty_cycle_set(void) {
|
float mc_interface_get_duty_cycle_set(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_duty_cycle_set();
|
ret = mcpwm_get_duty_cycle_set();
|
||||||
|
@ -382,7 +414,7 @@ float mc_interface_get_duty_cycle_set(void) {
|
||||||
float mc_interface_get_duty_cycle_now(void) {
|
float mc_interface_get_duty_cycle_now(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_duty_cycle_now();
|
ret = mcpwm_get_duty_cycle_now();
|
||||||
|
@ -402,7 +434,7 @@ float mc_interface_get_duty_cycle_now(void) {
|
||||||
float mc_interface_get_switching_frequency_now(void) {
|
float mc_interface_get_switching_frequency_now(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_switching_frequency_now();
|
ret = mcpwm_get_switching_frequency_now();
|
||||||
|
@ -422,7 +454,7 @@ float mc_interface_get_switching_frequency_now(void) {
|
||||||
float mc_interface_get_rpm(void) {
|
float mc_interface_get_rpm(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_rpm();
|
ret = mcpwm_get_rpm();
|
||||||
|
@ -449,10 +481,10 @@ float mc_interface_get_rpm(void) {
|
||||||
* The amount of amp hours drawn.
|
* The amount of amp hours drawn.
|
||||||
*/
|
*/
|
||||||
float mc_interface_get_amp_hours(bool reset) {
|
float mc_interface_get_amp_hours(bool reset) {
|
||||||
float val = amp_seconds / 3600;
|
float val = m_amp_seconds / 3600;
|
||||||
|
|
||||||
if (reset) {
|
if (reset) {
|
||||||
amp_seconds = 0.0;
|
m_amp_seconds = 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
return val;
|
return val;
|
||||||
|
@ -468,10 +500,10 @@ float mc_interface_get_amp_hours(bool reset) {
|
||||||
* The amount of amp hours fed back.
|
* The amount of amp hours fed back.
|
||||||
*/
|
*/
|
||||||
float mc_interface_get_amp_hours_charged(bool reset) {
|
float mc_interface_get_amp_hours_charged(bool reset) {
|
||||||
float val = amp_seconds_charged / 3600;
|
float val = m_amp_seconds_charged / 3600;
|
||||||
|
|
||||||
if (reset) {
|
if (reset) {
|
||||||
amp_seconds_charged = 0.0;
|
m_amp_seconds_charged = 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
return val;
|
return val;
|
||||||
|
@ -487,10 +519,10 @@ float mc_interface_get_amp_hours_charged(bool reset) {
|
||||||
* The amount of watt hours drawn.
|
* The amount of watt hours drawn.
|
||||||
*/
|
*/
|
||||||
float mc_interface_get_watt_hours(bool reset) {
|
float mc_interface_get_watt_hours(bool reset) {
|
||||||
float val = watt_seconds / 3600;
|
float val = m_watt_seconds / 3600;
|
||||||
|
|
||||||
if (reset) {
|
if (reset) {
|
||||||
amp_seconds = 0.0;
|
m_amp_seconds = 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
return val;
|
return val;
|
||||||
|
@ -506,10 +538,10 @@ float mc_interface_get_watt_hours(bool reset) {
|
||||||
* The amount of watt hours fed back.
|
* The amount of watt hours fed back.
|
||||||
*/
|
*/
|
||||||
float mc_interface_get_watt_hours_charged(bool reset) {
|
float mc_interface_get_watt_hours_charged(bool reset) {
|
||||||
float val = watt_seconds_charged / 3600;
|
float val = m_watt_seconds_charged / 3600;
|
||||||
|
|
||||||
if (reset) {
|
if (reset) {
|
||||||
watt_seconds_charged = 0.0;
|
m_watt_seconds_charged = 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
return val;
|
return val;
|
||||||
|
@ -518,7 +550,7 @@ float mc_interface_get_watt_hours_charged(bool reset) {
|
||||||
float mc_interface_get_tot_current(void) {
|
float mc_interface_get_tot_current(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_tot_current();
|
ret = mcpwm_get_tot_current();
|
||||||
|
@ -538,7 +570,7 @@ float mc_interface_get_tot_current(void) {
|
||||||
float mc_interface_get_tot_current_filtered(void) {
|
float mc_interface_get_tot_current_filtered(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_tot_current_filtered();
|
ret = mcpwm_get_tot_current_filtered();
|
||||||
|
@ -558,7 +590,7 @@ float mc_interface_get_tot_current_filtered(void) {
|
||||||
float mc_interface_get_tot_current_directional(void) {
|
float mc_interface_get_tot_current_directional(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_tot_current_directional();
|
ret = mcpwm_get_tot_current_directional();
|
||||||
|
@ -578,7 +610,7 @@ float mc_interface_get_tot_current_directional(void) {
|
||||||
float mc_interface_get_tot_current_directional_filtered(void) {
|
float mc_interface_get_tot_current_directional_filtered(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_tot_current_directional_filtered();
|
ret = mcpwm_get_tot_current_directional_filtered();
|
||||||
|
@ -598,7 +630,7 @@ float mc_interface_get_tot_current_directional_filtered(void) {
|
||||||
float mc_interface_get_tot_current_in(void) {
|
float mc_interface_get_tot_current_in(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_tot_current_in();
|
ret = mcpwm_get_tot_current_in();
|
||||||
|
@ -618,7 +650,7 @@ float mc_interface_get_tot_current_in(void) {
|
||||||
float mc_interface_get_tot_current_in_filtered(void) {
|
float mc_interface_get_tot_current_in_filtered(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_tot_current_in_filtered();
|
ret = mcpwm_get_tot_current_in_filtered();
|
||||||
|
@ -638,7 +670,7 @@ float mc_interface_get_tot_current_in_filtered(void) {
|
||||||
int mc_interface_get_tachometer_value(bool reset) {
|
int mc_interface_get_tachometer_value(bool reset) {
|
||||||
int ret = 0;
|
int ret = 0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_tachometer_value(reset);
|
ret = mcpwm_get_tachometer_value(reset);
|
||||||
|
@ -658,7 +690,7 @@ int mc_interface_get_tachometer_value(bool reset) {
|
||||||
int mc_interface_get_tachometer_abs_value(bool reset) {
|
int mc_interface_get_tachometer_abs_value(bool reset) {
|
||||||
int ret = 0;
|
int ret = 0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_tachometer_abs_value(reset);
|
ret = mcpwm_get_tachometer_abs_value(reset);
|
||||||
|
@ -678,7 +710,7 @@ int mc_interface_get_tachometer_abs_value(bool reset) {
|
||||||
float mc_interface_get_last_inj_adc_isr_duration(void) {
|
float mc_interface_get_last_inj_adc_isr_duration(void) {
|
||||||
float ret = 0.0;
|
float ret = 0.0;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
ret = mcpwm_get_last_inj_adc_isr_duration();
|
ret = mcpwm_get_last_inj_adc_isr_duration();
|
||||||
|
@ -696,21 +728,42 @@ float mc_interface_get_last_inj_adc_isr_duration(void) {
|
||||||
}
|
}
|
||||||
|
|
||||||
float mc_interface_read_reset_avg_motor_current(void) {
|
float mc_interface_read_reset_avg_motor_current(void) {
|
||||||
float res = motor_current_sum / motor_current_iterations;
|
float res = m_motor_current_sum / m_motor_current_iterations;
|
||||||
motor_current_sum = 0;
|
m_motor_current_sum = 0;
|
||||||
motor_current_iterations = 0;
|
m_motor_current_iterations = 0;
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
float mc_interface_read_reset_avg_input_current(void) {
|
float mc_interface_read_reset_avg_input_current(void) {
|
||||||
float res = input_current_sum / input_current_iterations;
|
float res = m_input_current_sum / m_input_current_iterations;
|
||||||
input_current_sum = 0;
|
m_input_current_sum = 0;
|
||||||
input_current_iterations = 0;
|
m_input_current_iterations = 0;
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
float mc_interface_get_pos_set(void) {
|
float mc_interface_get_pos_set(void) {
|
||||||
return position_set;
|
return m_position_set;
|
||||||
|
}
|
||||||
|
|
||||||
|
float mc_interface_get_last_sample_adc_isr_duration(void) {
|
||||||
|
return m_last_adc_duration_sample;
|
||||||
|
}
|
||||||
|
|
||||||
|
void mc_interface_sample_print_data(bool at_start, uint16_t len, uint8_t decimation) {
|
||||||
|
if (len > ADC_SAMPLE_MAX_LEN) {
|
||||||
|
len = ADC_SAMPLE_MAX_LEN;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_sample_len = len;
|
||||||
|
m_sample_int = decimation;
|
||||||
|
|
||||||
|
if (at_start) {
|
||||||
|
m_sample_at_start = 1;
|
||||||
|
m_start_comm = mcpwm_get_comm_step();
|
||||||
|
} else {
|
||||||
|
m_sample_now = 0;
|
||||||
|
m_sample_ready = 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// MC implementation functions
|
// MC implementation functions
|
||||||
|
@ -726,16 +779,16 @@ int mc_interface_try_input(void) {
|
||||||
// TODO: Remove this later
|
// TODO: Remove this later
|
||||||
if (mc_interface_get_state() == MC_STATE_DETECTING) {
|
if (mc_interface_get_state() == MC_STATE_DETECTING) {
|
||||||
mcpwm_stop_pwm();
|
mcpwm_stop_pwm();
|
||||||
ignore_iterations = MCPWM_DETECT_STOP_TIME;
|
m_ignore_iterations = MCPWM_DETECT_STOP_TIME;
|
||||||
}
|
}
|
||||||
|
|
||||||
int retval = ignore_iterations;
|
int retval = m_ignore_iterations;
|
||||||
|
|
||||||
if (!ignore_iterations && lock_enabled) {
|
if (!m_ignore_iterations && m_lock_enabled) {
|
||||||
if (!lock_override_once) {
|
if (!m_lock_override_once) {
|
||||||
retval = 1;
|
retval = 1;
|
||||||
} else {
|
} else {
|
||||||
lock_override_once = false;
|
m_lock_override_once = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -743,7 +796,7 @@ int mc_interface_try_input(void) {
|
||||||
}
|
}
|
||||||
|
|
||||||
void mc_interface_fault_stop(mc_fault_code fault) {
|
void mc_interface_fault_stop(mc_fault_code fault) {
|
||||||
if (mc_interface_dccal_done() && fault_now == FAULT_CODE_NONE) {
|
if (mc_interface_dccal_done() && m_fault_now == FAULT_CODE_NONE) {
|
||||||
// Sent to terminal fault logger so that all faults and their conditions
|
// Sent to terminal fault logger so that all faults and their conditions
|
||||||
// can be printed for debugging.
|
// can be printed for debugging.
|
||||||
chSysLock();
|
chSysLock();
|
||||||
|
@ -760,7 +813,7 @@ void mc_interface_fault_stop(mc_fault_code fault) {
|
||||||
fdata.duty = mc_interface_get_duty_cycle_now();
|
fdata.duty = mc_interface_get_duty_cycle_now();
|
||||||
fdata.rpm = mc_interface_get_rpm();
|
fdata.rpm = mc_interface_get_rpm();
|
||||||
fdata.tacho = mc_interface_get_tachometer_value(false);
|
fdata.tacho = mc_interface_get_tachometer_value(false);
|
||||||
fdata.cycles_running = cycles_running;
|
fdata.cycles_running = m_cycles_running;
|
||||||
fdata.tim_val_samp = val_samp;
|
fdata.tim_val_samp = val_samp;
|
||||||
fdata.tim_current_samp = current_samp;
|
fdata.tim_current_samp = current_samp;
|
||||||
fdata.tim_top = tim_top;
|
fdata.tim_top = tim_top;
|
||||||
|
@ -769,9 +822,9 @@ void mc_interface_fault_stop(mc_fault_code fault) {
|
||||||
terminal_add_fault_data(&fdata);
|
terminal_add_fault_data(&fdata);
|
||||||
}
|
}
|
||||||
|
|
||||||
ignore_iterations = conf.m_fault_stop_time_ms;
|
m_ignore_iterations = m_conf.m_fault_stop_time_ms;
|
||||||
|
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
mcpwm_stop_pwm();
|
mcpwm_stop_pwm();
|
||||||
|
@ -785,20 +838,22 @@ void mc_interface_fault_stop(mc_fault_code fault) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
fault_now = fault;
|
m_fault_now = fault;
|
||||||
}
|
}
|
||||||
|
|
||||||
void mc_interface_mc_timer_isr(void) {
|
void mc_interface_mc_timer_isr(void) {
|
||||||
|
ledpwm_update_pwm(); // LED PWM Driver update
|
||||||
|
|
||||||
const float input_voltage = GET_INPUT_VOLTAGE();
|
const float input_voltage = GET_INPUT_VOLTAGE();
|
||||||
|
|
||||||
// Check for faults that should stop the motor
|
// Check for faults that should stop the motor
|
||||||
static int wrong_voltage_iterations = 0;
|
static int wrong_voltage_iterations = 0;
|
||||||
if (input_voltage < conf.l_min_vin ||
|
if (input_voltage < m_conf.l_min_vin ||
|
||||||
input_voltage > conf.l_max_vin) {
|
input_voltage > m_conf.l_max_vin) {
|
||||||
wrong_voltage_iterations++;
|
wrong_voltage_iterations++;
|
||||||
|
|
||||||
if ((wrong_voltage_iterations >= 8)) {
|
if ((wrong_voltage_iterations >= 8)) {
|
||||||
mc_interface_fault_stop(input_voltage < conf.l_min_vin ?
|
mc_interface_fault_stop(input_voltage < m_conf.l_min_vin ?
|
||||||
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE);
|
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
|
@ -806,39 +861,37 @@ void mc_interface_mc_timer_isr(void) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mc_interface_get_state() == MC_STATE_RUNNING) {
|
if (mc_interface_get_state() == MC_STATE_RUNNING) {
|
||||||
cycles_running++;
|
m_cycles_running++;
|
||||||
} else {
|
} else {
|
||||||
cycles_running = 0;
|
m_cycles_running = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
main_dma_adc_handler();
|
|
||||||
|
|
||||||
if (pwn_done_func) {
|
if (pwn_done_func) {
|
||||||
pwn_done_func();
|
pwn_done_func();
|
||||||
}
|
}
|
||||||
|
|
||||||
const float current = mc_interface_get_tot_current_filtered();
|
const float current = mc_interface_get_tot_current_filtered();
|
||||||
const float current_in = mc_interface_get_tot_current_in_filtered();
|
const float current_in = mc_interface_get_tot_current_in_filtered();
|
||||||
motor_current_sum += current;
|
m_motor_current_sum += current;
|
||||||
input_current_sum += current_in;
|
m_input_current_sum += current_in;
|
||||||
motor_current_iterations++;
|
m_motor_current_iterations++;
|
||||||
input_current_iterations++;
|
m_input_current_iterations++;
|
||||||
|
|
||||||
float abs_current = mc_interface_get_tot_current();
|
float abs_current = mc_interface_get_tot_current();
|
||||||
float abs_current_filtered = current;
|
float abs_current_filtered = current;
|
||||||
if (conf.motor_type == MOTOR_TYPE_FOC) {
|
if (m_conf.motor_type == MOTOR_TYPE_FOC) {
|
||||||
// TODO: Make this more general
|
// TODO: Make this more general
|
||||||
abs_current = mcpwm_foc_get_abs_motor_current();
|
abs_current = mcpwm_foc_get_abs_motor_current();
|
||||||
abs_current_filtered = mcpwm_foc_get_abs_motor_current_filtered();
|
abs_current_filtered = mcpwm_foc_get_abs_motor_current_filtered();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Current fault code
|
// Current fault code
|
||||||
if (conf.l_slow_abs_current) {
|
if (m_conf.l_slow_abs_current) {
|
||||||
if (fabsf(abs_current_filtered) > conf.l_abs_current_max) {
|
if (fabsf(abs_current_filtered) > m_conf.l_abs_current_max) {
|
||||||
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
|
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
if (fabsf(abs_current) > conf.l_abs_current_max) {
|
if (fabsf(abs_current) > m_conf.l_abs_current_max) {
|
||||||
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
|
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -855,21 +908,72 @@ void mc_interface_mc_timer_isr(void) {
|
||||||
|
|
||||||
if (curr_diff_samples >= 0.01) {
|
if (curr_diff_samples >= 0.01) {
|
||||||
if (curr_diff_sum > 0.0) {
|
if (curr_diff_sum > 0.0) {
|
||||||
amp_seconds += curr_diff_sum;
|
m_amp_seconds += curr_diff_sum;
|
||||||
watt_seconds += curr_diff_sum * input_voltage;
|
m_watt_seconds += curr_diff_sum * input_voltage;
|
||||||
} else {
|
} else {
|
||||||
amp_seconds_charged -= curr_diff_sum;
|
m_amp_seconds_charged -= curr_diff_sum;
|
||||||
watt_seconds_charged -= curr_diff_sum * input_voltage;
|
m_watt_seconds_charged -= curr_diff_sum * input_voltage;
|
||||||
}
|
}
|
||||||
|
|
||||||
curr_diff_samples = 0.0;
|
curr_diff_samples = 0.0;
|
||||||
curr_diff_sum = 0.0;
|
curr_diff_sum = 0.0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Sample collection
|
||||||
|
if (m_sample_at_start && (mc_interface_get_state() == MC_STATE_RUNNING ||
|
||||||
|
m_start_comm != mcpwm_get_comm_step())) {
|
||||||
|
m_sample_now = 0;
|
||||||
|
m_sample_ready = 0;
|
||||||
|
m_sample_at_start = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int a = 0;
|
||||||
|
if (!m_sample_ready) {
|
||||||
|
a++;
|
||||||
|
if (a >= m_sample_int) {
|
||||||
|
a = 0;
|
||||||
|
|
||||||
|
if (mc_interface_get_state() == MC_STATE_DETECTING) {
|
||||||
|
m_curr0_samples[m_sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
|
||||||
|
m_curr1_samples[m_sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
|
||||||
|
|
||||||
|
m_ph1_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[0];
|
||||||
|
m_ph2_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[1];
|
||||||
|
m_ph3_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[2];
|
||||||
|
} else {
|
||||||
|
m_curr0_samples[m_sample_now] = ADC_curr_norm_value[0];
|
||||||
|
m_curr1_samples[m_sample_now] = ADC_curr_norm_value[1];
|
||||||
|
|
||||||
|
m_ph1_samples[m_sample_now] = ADC_V_L1 - mcpwm_vzero;
|
||||||
|
m_ph2_samples[m_sample_now] = ADC_V_L2 - mcpwm_vzero;
|
||||||
|
m_ph3_samples[m_sample_now] = ADC_V_L3 - mcpwm_vzero;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_vzero_samples[m_sample_now] = mcpwm_vzero;
|
||||||
|
|
||||||
|
m_curr_fir_samples[m_sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
|
||||||
|
m_f_sw_samples[m_sample_now] = (int16_t)(mc_interface_get_switching_frequency_now() / 10.0);
|
||||||
|
|
||||||
|
m_status_samples[m_sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
|
||||||
|
|
||||||
|
m_sample_now++;
|
||||||
|
|
||||||
|
if (m_sample_now == m_sample_len) {
|
||||||
|
m_sample_ready = 1;
|
||||||
|
m_sample_now = 0;
|
||||||
|
chSysLockFromISR();
|
||||||
|
chEvtSignalI(sample_send_tp, (eventmask_t) 1);
|
||||||
|
chSysUnlockFromISR();
|
||||||
|
}
|
||||||
|
|
||||||
|
m_last_adc_duration_sample = mcpwm_get_last_adc_isr_duration();
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void mc_interface_adc_inj_int_handler(void) {
|
void mc_interface_adc_inj_int_handler(void) {
|
||||||
switch (conf.motor_type) {
|
switch (m_conf.motor_type) {
|
||||||
case MOTOR_TYPE_BLDC:
|
case MOTOR_TYPE_BLDC:
|
||||||
case MOTOR_TYPE_DC:
|
case MOTOR_TYPE_DC:
|
||||||
mcpwm_adc_inj_int_handler();
|
mcpwm_adc_inj_int_handler();
|
||||||
|
@ -944,16 +1048,53 @@ static THD_FUNCTION(timer_thread, arg) {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Decrease fault iterations
|
// Decrease fault iterations
|
||||||
if (ignore_iterations > 0) {
|
if (m_ignore_iterations > 0) {
|
||||||
ignore_iterations--;
|
m_ignore_iterations--;
|
||||||
} else {
|
} else {
|
||||||
if (!IS_DRV_FAULT()) {
|
if (!IS_DRV_FAULT()) {
|
||||||
fault_now = FAULT_CODE_NONE;
|
m_fault_now = FAULT_CODE_NONE;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
update_override_limits(&conf);
|
update_override_limits(&m_conf);
|
||||||
|
|
||||||
chThdSleepMilliseconds(1);
|
chThdSleepMilliseconds(1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static THD_FUNCTION(sample_send_thread, arg) {
|
||||||
|
(void)arg;
|
||||||
|
|
||||||
|
chRegSetThreadName("SampleSender");
|
||||||
|
|
||||||
|
sample_send_tp = chThdGetSelfX();
|
||||||
|
|
||||||
|
for(;;) {
|
||||||
|
chEvtWaitAny((eventmask_t) 1);
|
||||||
|
|
||||||
|
for (int i = 0;i < m_sample_len;i++) {
|
||||||
|
uint8_t buffer[20];
|
||||||
|
int index = 0;
|
||||||
|
|
||||||
|
buffer[index++] = m_curr0_samples[i] >> 8;
|
||||||
|
buffer[index++] = m_curr0_samples[i];
|
||||||
|
buffer[index++] = m_curr1_samples[i] >> 8;
|
||||||
|
buffer[index++] = m_curr1_samples[i];
|
||||||
|
buffer[index++] = m_ph1_samples[i] >> 8;
|
||||||
|
buffer[index++] = m_ph1_samples[i];
|
||||||
|
buffer[index++] = m_ph2_samples[i] >> 8;
|
||||||
|
buffer[index++] = m_ph2_samples[i];
|
||||||
|
buffer[index++] = m_ph3_samples[i] >> 8;
|
||||||
|
buffer[index++] = m_ph3_samples[i];
|
||||||
|
buffer[index++] = m_vzero_samples[i] >> 8;
|
||||||
|
buffer[index++] = m_vzero_samples[i];
|
||||||
|
buffer[index++] = m_status_samples[i];
|
||||||
|
buffer[index++] = m_curr_fir_samples[i] >> 8;
|
||||||
|
buffer[index++] = m_curr_fir_samples[i];
|
||||||
|
buffer[index++] = m_f_sw_samples[i] >> 8;
|
||||||
|
buffer[index++] = m_f_sw_samples[i];
|
||||||
|
|
||||||
|
commands_send_samples(buffer, index);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -66,6 +66,8 @@ float mc_interface_get_last_inj_adc_isr_duration(void);
|
||||||
float mc_interface_read_reset_avg_motor_current(void);
|
float mc_interface_read_reset_avg_motor_current(void);
|
||||||
float mc_interface_read_reset_avg_input_current(void);
|
float mc_interface_read_reset_avg_input_current(void);
|
||||||
float mc_interface_get_pos_set(void);
|
float mc_interface_get_pos_set(void);
|
||||||
|
float mc_interface_get_last_sample_adc_isr_duration(void);
|
||||||
|
void mc_interface_sample_print_data(bool at_start, uint16_t len, uint8_t decimation);
|
||||||
|
|
||||||
// MC implementation functions
|
// MC implementation functions
|
||||||
void mc_interface_fault_stop(mc_fault_code fault);
|
void mc_interface_fault_stop(mc_fault_code fault);
|
||||||
|
|
|
@ -52,7 +52,7 @@
|
||||||
#define MCCONF_L_MIN_VOLTAGE 8.0 // Minimum input voltage
|
#define MCCONF_L_MIN_VOLTAGE 8.0 // Minimum input voltage
|
||||||
#endif
|
#endif
|
||||||
#ifndef MCCONF_L_MAX_VOLTAGE
|
#ifndef MCCONF_L_MAX_VOLTAGE
|
||||||
#define MCCONF_L_MAX_VOLTAGE 50.0 // Maximum input voltage
|
#define MCCONF_L_MAX_VOLTAGE 57.0 // Maximum input voltage
|
||||||
#endif
|
#endif
|
||||||
#ifndef MCCONF_L_BATTERY_CUT_START
|
#ifndef MCCONF_L_BATTERY_CUT_START
|
||||||
#define MCCONF_L_BATTERY_CUT_START 10.0 // Start limiting the positive current at this voltage
|
#define MCCONF_L_BATTERY_CUT_START 10.0 // Start limiting the positive current at this voltage
|
||||||
|
@ -199,7 +199,7 @@
|
||||||
#define MCCONF_FOC_F_SW 20000.0
|
#define MCCONF_FOC_F_SW 20000.0
|
||||||
#endif
|
#endif
|
||||||
#ifndef MCCONF_FOC_DT_US
|
#ifndef MCCONF_FOC_DT_US
|
||||||
#define MCCONF_FOC_DT_US 0.15 // Microseconds for dead time compensation
|
#define MCCONF_FOC_DT_US 0.08 // Microseconds for dead time compensation
|
||||||
#endif
|
#endif
|
||||||
#ifndef MCCONF_FOC_ENCODER_INVERTED
|
#ifndef MCCONF_FOC_ENCODER_INVERTED
|
||||||
#define MCCONF_FOC_ENCODER_INVERTED false
|
#define MCCONF_FOC_ENCODER_INVERTED false
|
||||||
|
|
21
mcpwm_foc.c
21
mcpwm_foc.c
|
@ -1782,13 +1782,6 @@ static void control_current(volatile motor_state_t *state_m, float dt) {
|
||||||
float mod_alpha = c * state_m->mod_d - s * state_m->mod_q;
|
float mod_alpha = c * state_m->mod_d - s * state_m->mod_q;
|
||||||
float mod_beta = c * state_m->mod_q + s * state_m->mod_d;
|
float mod_beta = c * state_m->mod_q + s * state_m->mod_d;
|
||||||
|
|
||||||
state_m->v_alpha = mod_alpha * (2.0 / 3.0) * state_m->v_bus;
|
|
||||||
state_m->v_beta = mod_beta * (2.0 / 3.0) * state_m->v_bus;
|
|
||||||
|
|
||||||
// Set output (HW Dependent)
|
|
||||||
uint32_t duty1, duty2, duty3, top;
|
|
||||||
top = TIM1->ARR;
|
|
||||||
|
|
||||||
// Deadtime compensation
|
// Deadtime compensation
|
||||||
const float i_alpha_set = c * state_m->id_target - s * state_m->iq_target;
|
const float i_alpha_set = c * state_m->id_target - s * state_m->iq_target;
|
||||||
const float i_beta_set = c * state_m->iq_target + s * state_m->id_target;
|
const float i_beta_set = c * state_m->iq_target + s * state_m->id_target;
|
||||||
|
@ -1797,10 +1790,20 @@ static void control_current(volatile motor_state_t *state_m, float dt) {
|
||||||
const float ic_set = -0.5 * i_alpha_set - SQRT3_BY_2 * i_beta_set;
|
const float ic_set = -0.5 * i_alpha_set - SQRT3_BY_2 * i_beta_set;
|
||||||
const float mod_alpha_set_sgn = (2.0 / 3.0) * SIGN(ia_set) - (1.0 / 3.0) * SIGN(ib_set) - (1.0 / 3.0) * SIGN(ic_set);
|
const float mod_alpha_set_sgn = (2.0 / 3.0) * SIGN(ia_set) - (1.0 / 3.0) * SIGN(ib_set) - (1.0 / 3.0) * SIGN(ic_set);
|
||||||
const float mod_beta_set_sgn = ONE_BY_SQRT3 * SIGN(ib_set) - ONE_BY_SQRT3 * SIGN(ic_set);
|
const float mod_beta_set_sgn = ONE_BY_SQRT3 * SIGN(ib_set) - ONE_BY_SQRT3 * SIGN(ic_set);
|
||||||
|
const float mod_comp_fact = m_conf->foc_dt_us * 1e-6 * m_conf->foc_f_sw;
|
||||||
|
const float mod_alpha_comp = mod_alpha_set_sgn * mod_comp_fact;
|
||||||
|
const float mod_beta_comp = mod_beta_set_sgn * mod_comp_fact;
|
||||||
|
|
||||||
mod_alpha += mod_alpha_set_sgn * m_conf->foc_dt_us * 1e-6 * m_conf->foc_f_sw;
|
// mod_alpha += mod_alpha_comp;
|
||||||
mod_beta += mod_beta_set_sgn * m_conf->foc_dt_us * 1e-6 * m_conf->foc_f_sw;
|
// mod_beta += mod_beta_comp;
|
||||||
|
|
||||||
|
// Apply compensation here so that 0 duty cyle has no glitches.
|
||||||
|
state_m->v_alpha = (mod_alpha - mod_alpha_comp) * (2.0 / 3.0) * state_m->v_bus;
|
||||||
|
state_m->v_beta = (mod_beta - mod_beta_comp) * (2.0 / 3.0) * state_m->v_bus;
|
||||||
|
|
||||||
|
// Set output (HW Dependent)
|
||||||
|
uint32_t duty1, duty2, duty3, top;
|
||||||
|
top = TIM1->ARR;
|
||||||
svm(-mod_alpha, -mod_beta, top, &duty1, &duty2, &duty3);
|
svm(-mod_alpha, -mod_beta, top, &duty1, &duty2, &duty3);
|
||||||
TIMER_UPDATE_DUTY(duty1, duty2, duty3);
|
TIMER_UPDATE_DUTY(duty1, duty2, duty3);
|
||||||
|
|
||||||
|
|
|
@ -29,7 +29,6 @@
|
||||||
#include "mcpwm_foc.h"
|
#include "mcpwm_foc.h"
|
||||||
#include "mc_interface.h"
|
#include "mc_interface.h"
|
||||||
#include "commands.h"
|
#include "commands.h"
|
||||||
#include "main.h"
|
|
||||||
#include "hw.h"
|
#include "hw.h"
|
||||||
#include "comm_can.h"
|
#include "comm_can.h"
|
||||||
#include "utils.h"
|
#include "utils.h"
|
||||||
|
@ -73,7 +72,7 @@ void terminal_process_string(char *str) {
|
||||||
} else if (strcmp(argv[0], "last_adc_duration") == 0) {
|
} else if (strcmp(argv[0], "last_adc_duration") == 0) {
|
||||||
commands_printf("Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0));
|
commands_printf("Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0));
|
||||||
commands_printf("Latest injected ADC duration: %.4f ms", (double)(mc_interface_get_last_inj_adc_isr_duration() * 1000.0));
|
commands_printf("Latest injected ADC duration: %.4f ms", (double)(mc_interface_get_last_inj_adc_isr_duration() * 1000.0));
|
||||||
commands_printf("Latest main ADC duration: %.4f ms\n", (double)(main_get_last_adc_isr_duration() * 1000.0));
|
commands_printf("Latest sample ADC duration: %.4f ms\n", (double)(mc_interface_get_last_sample_adc_isr_duration() * 1000.0));
|
||||||
} else if (strcmp(argv[0], "kv") == 0) {
|
} else if (strcmp(argv[0], "kv") == 0) {
|
||||||
commands_printf("Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered());
|
commands_printf("Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered());
|
||||||
} else if (strcmp(argv[0], "mem") == 0) {
|
} else if (strcmp(argv[0], "mem") == 0) {
|
||||||
|
|
Loading…
Reference in New Issue