mirror of https://github.com/rusefi/bldc.git
FW 2.17: as5047 support, change sensor port mode in conf, better encoder detection, FOC ah and wh counter fixes
This commit is contained in:
Benjamin Vedder
parent
3e18879e10
commit
cb2a205cb8
4
Makefile
4
Makefile
|
|
@ -270,8 +270,8 @@ build/$(PROJECT).bin: build/$(PROJECT).elf
|
|||
# Program
|
||||
upload: build/$(PROJECT).bin
|
||||
#qstlink2 --cli --erase --write build/$(PROJECT).bin
|
||||
openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset"
|
||||
#openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
|
||||
#openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset" # Older openocd
|
||||
openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
|
||||
|
||||
#program with olimex arm-usb-tiny-h and jtag-swd adapter board. needs openocd>=0.9
|
||||
upload-olimex: build/$(PROJECT).bin
|
||||
|
|
|
|||
|
|
@ -0,0 +1,31 @@
|
|||
/*
|
||||
Copyright 2016 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/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* appconf_example_ppm.h
|
||||
*
|
||||
* Created on: 9 sep 2015
|
||||
* Author: benjamin
|
||||
*/
|
||||
|
||||
#ifndef APPCONF_APPCONF_EXAMPLE_PPM_H_
|
||||
#define APPCONF_APPCONF_EXAMPLE_PPM_H_
|
||||
|
||||
// Use custom user application
|
||||
#define APPCONF_APP_TO_USE APP_CUSTOM
|
||||
|
||||
#endif
|
||||
|
|
@ -32,6 +32,8 @@
|
|||
#include "utils.h"
|
||||
#include "hw.h"
|
||||
#include "timeout.h"
|
||||
#include "comm_can.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
// Settings
|
||||
|
|
@ -147,15 +149,7 @@ static THD_FUNCTION(uart_thread, arg) {
|
|||
}
|
||||
|
||||
static void set_output(float output) {
|
||||
output /= (1.0 - HYST);
|
||||
|
||||
if (output > HYST) {
|
||||
output -= HYST;
|
||||
} else if (output < -HYST) {
|
||||
output += HYST;
|
||||
} else {
|
||||
output = 0.0;
|
||||
}
|
||||
utils_deadband(&output, HYST, 1.0);
|
||||
|
||||
const float rpm = mc_interface_get_rpm();
|
||||
|
||||
|
|
@ -182,13 +176,11 @@ static void set_output(float output) {
|
|||
current_p2 = current_p1;
|
||||
current_p1 = current;
|
||||
|
||||
if (fabsf(current) < mc_interface_get_configuration()->cc_min_current) {
|
||||
current = -mc_interface_get_configuration()->cc_min_current;
|
||||
}
|
||||
|
||||
mc_interface_set_current(current);
|
||||
comm_can_set_current(255, current);
|
||||
} else {
|
||||
mc_interface_set_brake_current(output * mc_interface_get_configuration()->l_current_min);
|
||||
comm_can_set_current_brake(255, output * mc_interface_get_configuration()->l_current_min);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
26
commands.c
26
commands.c
|
|
@ -314,7 +314,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
mcconf.foc_sl_d_current_factor = buffer_get_float32(data, 1e3, &ind);
|
||||
memcpy(mcconf.foc_hall_table, data + ind, 8);
|
||||
ind += 8;
|
||||
mcconf.foc_hall_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
mcconf.foc_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
|
||||
mcconf.s_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
|
||||
mcconf.s_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
|
||||
|
|
@ -336,14 +336,11 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
mcconf.m_duty_ramp_step_rpm_lim = (float)buffer_get_float32(data, 1000000.0, &ind);
|
||||
mcconf.m_current_backoff_gain = (float)buffer_get_float32(data, 1000000.0, &ind);
|
||||
mcconf.m_encoder_counts = buffer_get_uint32(data, &ind);
|
||||
mcconf.m_sensor_port_mode = data[ind++];
|
||||
|
||||
conf_general_store_mc_configuration(&mcconf);
|
||||
mc_interface_set_configuration(&mcconf);
|
||||
|
||||
#if ENCODER_ENABLE
|
||||
encoder_set_counts(mcconf.m_encoder_counts);
|
||||
#endif
|
||||
|
||||
ind = 0;
|
||||
send_buffer[ind++] = packet_id;
|
||||
commands_send_packet(send_buffer, ind);
|
||||
|
|
@ -422,7 +419,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_factor, 1e3, &ind);
|
||||
memcpy(send_buffer + ind, mcconf.foc_hall_table, 8);
|
||||
ind += 8;
|
||||
buffer_append_int32(send_buffer, (int32_t)(mcconf.foc_hall_sl_erpm * 1000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(mcconf.foc_sl_erpm * 1000.0), &ind);
|
||||
|
||||
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kp * 1000000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_ki * 1000000.0), &ind);
|
||||
|
|
@ -444,6 +441,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step_rpm_lim, 1000000.0, &ind);
|
||||
buffer_append_float32(send_buffer, mcconf.m_current_backoff_gain, 1000000.0, &ind);
|
||||
buffer_append_uint32(send_buffer, mcconf.m_encoder_counts, &ind);
|
||||
send_buffer[ind++] = mcconf.m_sensor_port_mode;
|
||||
|
||||
commands_send_packet(send_buffer, ind);
|
||||
break;
|
||||
|
|
@ -663,7 +661,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
break;
|
||||
|
||||
case COMM_DETECT_ENCODER: {
|
||||
#if ENCODER_ENABLE
|
||||
if (encoder_is_configured()) {
|
||||
mcconf = *mc_interface_get_configuration();
|
||||
mcconf_old = mcconf;
|
||||
|
||||
|
|
@ -688,21 +686,22 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
buffer_append_float32(send_buffer, ratio, 1e6, &ind);
|
||||
send_buffer[ind++] = inverted;
|
||||
commands_send_packet(send_buffer, ind);
|
||||
#else
|
||||
} else {
|
||||
ind = 0;
|
||||
send_buffer[ind++] = COMM_DETECT_ENCODER;
|
||||
buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
|
||||
buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
|
||||
send_buffer[ind++] = false;
|
||||
commands_send_packet(send_buffer, ind);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case COMM_DETECT_HALL_FOC: {
|
||||
mcconf = *mc_interface_get_configuration();
|
||||
mcconf_old = mcconf;
|
||||
|
||||
if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_HALL) {
|
||||
mcconf_old = mcconf;
|
||||
ind = 0;
|
||||
float current = buffer_get_float32(data, 1e3, &ind);
|
||||
|
||||
|
|
@ -723,6 +722,13 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
send_buffer[ind++] = res ? 0 : 1;
|
||||
|
||||
commands_send_packet(send_buffer, ind);
|
||||
} else {
|
||||
ind = 0;
|
||||
send_buffer[ind++] = COMM_DETECT_HALL_FOC;
|
||||
memset(send_buffer, 255, 8);
|
||||
ind += 8;
|
||||
send_buffer[ind++] = 0;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
|
|||
|
|
@ -218,7 +218,7 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
|
|||
conf->foc_hall_table[5] = MCCONF_FOC_HALL_TAB_5;
|
||||
conf->foc_hall_table[6] = MCCONF_FOC_HALL_TAB_6;
|
||||
conf->foc_hall_table[7] = MCCONF_FOC_HALL_TAB_7;
|
||||
conf->foc_hall_sl_erpm = MCCONF_FOC_HALL_ERPM;
|
||||
conf->foc_sl_erpm = MCCONF_FOC_SL_ERPM;
|
||||
|
||||
conf->s_pid_kp = MCCONF_S_PID_KP;
|
||||
conf->s_pid_ki = MCCONF_S_PID_KI;
|
||||
|
|
@ -240,6 +240,7 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
|
|||
conf->m_duty_ramp_step_rpm_lim = MCCONF_M_RAMP_STEP_RPM_LIM;
|
||||
conf->m_current_backoff_gain = MCCONF_M_CURRENT_BACKOFF_GAIN;
|
||||
conf->m_encoder_counts = MCCONF_M_ENCODER_COUNTS;
|
||||
conf->m_sensor_port_mode = MCCONF_M_SENSOR_PORT_MODE;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
|
||||
// Firmware version
|
||||
#define FW_VERSION_MAJOR 2
|
||||
#define FW_VERSION_MINOR 16
|
||||
#define FW_VERSION_MINOR 17
|
||||
|
||||
#include "datatypes.h"
|
||||
|
||||
|
|
@ -38,6 +38,7 @@
|
|||
|
||||
// Settings and parameters to override
|
||||
//#define VIN_R1 33000.0
|
||||
//#define VIN_R1 39200.0
|
||||
//#define VIN_R2 2200.0
|
||||
//#define CURRENT_AMP_GAIN 10.0
|
||||
//#define CURRENT_SHUNT_RES 0.0005
|
||||
|
|
@ -65,11 +66,13 @@
|
|||
* Select default user motor configuration
|
||||
*/
|
||||
//#define MCCONF_DEFAULT_USER "mcconf_sten.h"
|
||||
//#define MCCONF_DEFAULT_USER "mcconf_sp_540kv.h"
|
||||
|
||||
/*
|
||||
* Select default user app configuration
|
||||
*/
|
||||
//#define APPCONF_DEFAULT_USER "appconf_example_ppm.h"
|
||||
//#define APPCONF_DEFAULT_USER "appconf_custom.h"
|
||||
|
||||
/*
|
||||
* Select which custom application to use. To configure the default applications and
|
||||
|
|
@ -78,13 +81,6 @@
|
|||
*/
|
||||
//#define USE_APP_STEN
|
||||
|
||||
/*
|
||||
* Use encoder
|
||||
*/
|
||||
#ifndef ENCODER_ENABLE
|
||||
#define ENCODER_ENABLE 0
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Enable CAN-bus
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -95,6 +95,12 @@ typedef enum {
|
|||
DISP_POS_MODE_ENCODER_OBSERVER_ERROR
|
||||
} disp_pos_mode;
|
||||
|
||||
typedef enum {
|
||||
SENSOR_PORT_MODE_HALL = 0,
|
||||
SENSOR_PORT_MODE_ABI,
|
||||
SENSOR_PORT_MODE_AS5047_SPI
|
||||
} sensor_port_mode;
|
||||
|
||||
typedef struct {
|
||||
float cycle_int_limit;
|
||||
float cycle_int_limit_running;
|
||||
|
|
@ -172,7 +178,7 @@ typedef struct {
|
|||
float foc_sl_d_current_factor;
|
||||
mc_foc_sensor_mode foc_sensor_mode;
|
||||
uint8_t foc_hall_table[8];
|
||||
float foc_hall_sl_erpm;
|
||||
float foc_sl_erpm;
|
||||
// Speed PID
|
||||
float s_pid_kp;
|
||||
float s_pid_ki;
|
||||
|
|
@ -194,6 +200,7 @@ typedef struct {
|
|||
float m_duty_ramp_step_rpm_lim;
|
||||
float m_current_backoff_gain;
|
||||
uint32_t m_encoder_counts;
|
||||
sensor_port_mode m_sensor_port_mode;
|
||||
} mc_configuration;
|
||||
|
||||
// Applications to use
|
||||
|
|
|
|||
183
encoder.c
183
encoder.c
|
|
@ -1,5 +1,5 @@
|
|||
/*
|
||||
Copyright 2012-2015 Benjamin Vedder benjamin@vedder.se
|
||||
Copyright 2012-2016 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
|
||||
|
|
@ -15,26 +15,64 @@
|
|||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* encoder.c
|
||||
*
|
||||
* Created on: 7 mar 2015
|
||||
* Author: benjamin
|
||||
*/
|
||||
|
||||
#include "encoder.h"
|
||||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
#include "stm32f4xx_conf.h"
|
||||
#include "hw.h"
|
||||
#include "utils.h"
|
||||
|
||||
// Defines
|
||||
#define AS5047P_READ_ANGLECOM (0x3FFF | 0x4000 | 0x8000) // This is just ones
|
||||
#define AS5047_SAMPLE_RATE_HZ 20000
|
||||
|
||||
#define SPI_SW_MISO_GPIO HW_HALL_ENC_GPIO2
|
||||
#define SPI_SW_MISO_PIN HW_HALL_ENC_PIN2
|
||||
#define SPI_SW_SCK_GPIO HW_HALL_ENC_GPIO1
|
||||
#define SPI_SW_SCK_PIN HW_HALL_ENC_PIN1
|
||||
#define SPI_SW_CS_GPIO HW_HALL_ENC_GPIO3
|
||||
#define SPI_SW_CS_PIN HW_HALL_ENC_PIN3
|
||||
|
||||
// Private types
|
||||
typedef enum {
|
||||
ENCODER_MODE_NONE = 0,
|
||||
ENCODER_MODE_ABI,
|
||||
ENCODER_MODE_AS5047P_SPI
|
||||
} encoder_mode;
|
||||
|
||||
// Private variables
|
||||
static bool index_found;
|
||||
static bool index_found = false;
|
||||
static uint32_t enc_counts = 10000;
|
||||
static encoder_mode mode = ENCODER_MODE_NONE;
|
||||
static float last_enc_angle = 0.0;
|
||||
|
||||
void encoder_init(uint32_t counts) {
|
||||
// Private functions
|
||||
uint16_t spi_exchange(uint16_t x);
|
||||
static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length);
|
||||
static void spi_begin(void);
|
||||
static void spi_end(void);
|
||||
static void spi_delay(void);
|
||||
|
||||
void encoder_deinit(void) {
|
||||
nvicDisableVector(HW_ENC_EXTI_CH);
|
||||
nvicDisableVector(HW_ENC_TIM_ISR_CH);
|
||||
|
||||
TIM_DeInit(HW_ENC_TIM);
|
||||
|
||||
palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT_PULLUP);
|
||||
palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_INPUT_PULLUP);
|
||||
palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_INPUT_PULLUP);
|
||||
|
||||
palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP);
|
||||
palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP);
|
||||
|
||||
index_found = false;
|
||||
mode = ENCODER_MODE_NONE;
|
||||
last_enc_angle = 0.0;
|
||||
}
|
||||
|
||||
void encoder_init_abi(uint32_t counts) {
|
||||
EXTI_InitTypeDef EXTI_InitStructure;
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
|
||||
// Initialize variables
|
||||
index_found = false;
|
||||
|
|
@ -76,15 +114,62 @@ void encoder_init(uint32_t counts) {
|
|||
EXTI_Init(&EXTI_InitStructure);
|
||||
|
||||
// Enable and set EXTI Line Interrupt to the highest priority
|
||||
NVIC_InitStructure.NVIC_IRQChannel = HW_ENC_EXTI_CH;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00;
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
nvicEnableVector(HW_ENC_EXTI_CH, 0);
|
||||
|
||||
mode = ENCODER_MODE_ABI;
|
||||
}
|
||||
|
||||
void encoder_init_as5047p_spi(void) {
|
||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
||||
|
||||
palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
|
||||
palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
|
||||
|
||||
// Enable timer clock
|
||||
HW_ENC_TIM_CLK_EN();
|
||||
|
||||
// Time Base configuration
|
||||
TIM_TimeBaseStructure.TIM_Prescaler = 0;
|
||||
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||
TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / AS5047_SAMPLE_RATE_HZ) - 1);
|
||||
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
||||
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
|
||||
TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
|
||||
|
||||
// Enable overflow interrupt
|
||||
TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
|
||||
|
||||
// Enable timer
|
||||
TIM_Cmd(HW_ENC_TIM, ENABLE);
|
||||
|
||||
nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
|
||||
|
||||
mode = ENCODER_MODE_AS5047P_SPI;
|
||||
index_found = true;
|
||||
}
|
||||
|
||||
bool encoder_is_configured(void) {
|
||||
return mode != ENCODER_MODE_NONE;
|
||||
}
|
||||
|
||||
float encoder_read_deg(void) {
|
||||
return ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
|
||||
static float angle = 0.0;
|
||||
|
||||
switch (mode) {
|
||||
case ENCODER_MODE_ABI:
|
||||
angle = ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
|
||||
break;
|
||||
|
||||
case ENCODER_MODE_AS5047P_SPI:
|
||||
angle = last_enc_angle;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -95,6 +180,20 @@ void encoder_reset(void) {
|
|||
index_found = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Timer interrupt
|
||||
*/
|
||||
void encoder_tim_isr(void) {
|
||||
uint16_t pos;
|
||||
|
||||
spi_begin();
|
||||
spi_transfer(&pos, 0, 1);
|
||||
spi_end();
|
||||
|
||||
pos &= 0x3FFF;
|
||||
last_enc_angle = ((float)pos * 360.0) / 16384.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the number of encoder counts.
|
||||
*
|
||||
|
|
@ -118,3 +217,53 @@ void encoder_set_counts(uint32_t counts) {
|
|||
bool encoder_index_found(void) {
|
||||
return index_found;
|
||||
}
|
||||
|
||||
// Software SPI
|
||||
uint16_t spi_exchange(uint16_t x) {
|
||||
uint16_t rx;
|
||||
spi_transfer(&rx, &x, 1);
|
||||
return rx;
|
||||
}
|
||||
|
||||
static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length) {
|
||||
for (int i = 0;i < length;i++) {
|
||||
uint16_t send = out_buf ? out_buf[i] : 0xFFFF;
|
||||
uint16_t recieve = 0;
|
||||
|
||||
for (int bit = 0;bit < 16;bit++) {
|
||||
//palWritePad(HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, send >> 15);
|
||||
send <<= 1;
|
||||
|
||||
spi_delay();
|
||||
palSetPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
|
||||
spi_delay();
|
||||
|
||||
recieve <<= 1;
|
||||
if (palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN)) {
|
||||
recieve |= 1;
|
||||
}
|
||||
|
||||
palClearPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
|
||||
spi_delay();
|
||||
}
|
||||
|
||||
if (in_buf) {
|
||||
in_buf[i] = recieve;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void spi_begin(void) {
|
||||
palClearPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN);
|
||||
}
|
||||
|
||||
static void spi_end(void) {
|
||||
palSetPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN);
|
||||
}
|
||||
|
||||
static void spi_delay(void) {
|
||||
__NOP();
|
||||
__NOP();
|
||||
__NOP();
|
||||
__NOP();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -28,9 +28,13 @@
|
|||
#include "conf_general.h"
|
||||
|
||||
// Functions
|
||||
void encoder_init(uint32_t counts);
|
||||
void encoder_deinit(void);
|
||||
void encoder_init_abi(uint32_t counts);
|
||||
void encoder_init_as5047p_spi(void);
|
||||
bool encoder_is_configured(void);
|
||||
float encoder_read_deg(void);
|
||||
void encoder_reset(void);
|
||||
void encoder_tim_isr(void);
|
||||
void encoder_set_counts(uint32_t counts);
|
||||
bool encoder_index_found(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -146,7 +146,7 @@
|
|||
* @brief Enables the SPI subsystem.
|
||||
*/
|
||||
#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__)
|
||||
#define HAL_USE_SPI FALSE
|
||||
#define HAL_USE_SPI TRUE
|
||||
#endif
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -146,6 +146,8 @@
|
|||
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
|
||||
#define HW_ENC_EXTI_LINE EXTI_Line8
|
||||
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
|
||||
#define HW_ENC_TIM_ISR_CH TIM4_IRQn
|
||||
#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
|
||||
|
||||
// NRF pins
|
||||
#define NRF_PORT_CSN HW_ICU_GPIO
|
||||
|
|
|
|||
|
|
@ -148,6 +148,8 @@
|
|||
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
|
||||
#define HW_ENC_EXTI_LINE EXTI_Line11
|
||||
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
|
||||
#define HW_ENC_TIM_ISR_CH TIM4_IRQn
|
||||
#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
|
||||
|
||||
// NRF pins
|
||||
#define NRF_PORT_CSN GPIOA
|
||||
|
|
|
|||
|
|
@ -146,6 +146,8 @@
|
|||
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
|
||||
#define HW_ENC_EXTI_LINE EXTI_Line11
|
||||
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
|
||||
#define HW_ENC_TIM_ISR_CH TIM4_IRQn
|
||||
#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
|
||||
|
||||
// NRF pins
|
||||
#define NRF_PORT_CSN HW_ICU_GPIO
|
||||
|
|
|
|||
|
|
@ -154,6 +154,8 @@
|
|||
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
|
||||
#define HW_ENC_EXTI_LINE EXTI_Line11
|
||||
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
|
||||
#define HW_ENC_TIM_ISR_CH TIM4_IRQn
|
||||
#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
|
||||
|
||||
// NRF pins
|
||||
#define NRF_PORT_CSN HW_ICU_GPIO
|
||||
|
|
|
|||
|
|
@ -148,6 +148,8 @@
|
|||
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
|
||||
#define HW_ENC_EXTI_LINE EXTI_Line11
|
||||
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
|
||||
#define HW_ENC_TIM_ISR_CH TIM4_IRQn
|
||||
#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
|
||||
|
||||
// NRF pins
|
||||
#define NRF_PORT_CSN GPIOB
|
||||
|
|
|
|||
|
|
@ -148,6 +148,8 @@
|
|||
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
|
||||
#define HW_ENC_EXTI_LINE EXTI_Line11
|
||||
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
|
||||
#define HW_ENC_TIM_ISR_CH TIM4_IRQn
|
||||
#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
|
||||
|
||||
// NRF pins
|
||||
#define NRF_PORT_CSN GPIOB
|
||||
|
|
|
|||
|
|
@ -156,6 +156,8 @@
|
|||
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
|
||||
#define HW_ENC_EXTI_LINE EXTI_Line8
|
||||
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
|
||||
#define HW_ENC_TIM_ISR_CH TIM3_IRQn
|
||||
#define HW_ENC_TIM_ISR_VEC TIM3_IRQHandler
|
||||
|
||||
// NRF pins
|
||||
#define NRF_PORT_CSN HW_ICU_GPIO
|
||||
|
|
|
|||
|
|
@ -146,6 +146,8 @@
|
|||
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
|
||||
#define HW_ENC_EXTI_LINE EXTI_Line11
|
||||
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
|
||||
#define HW_ENC_TIM_ISR_CH TIM4_IRQn
|
||||
#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
|
||||
|
||||
// NRF pins
|
||||
#define NRF_PORT_CSN HW_ICU_GPIO
|
||||
|
|
|
|||
|
|
@ -48,3 +48,12 @@ CH_IRQ_HANDLER(HW_ENC_EXTI_ISR_VEC) {
|
|||
EXTI_ClearITPendingBit(HW_ENC_EXTI_LINE);
|
||||
}
|
||||
}
|
||||
|
||||
CH_IRQ_HANDLER(HW_ENC_TIM_ISR_VEC) {
|
||||
if (TIM_GetITStatus(HW_ENC_TIM, TIM_IT_Update) != RESET) {
|
||||
encoder_tim_isr();
|
||||
|
||||
// Clear the IT pending bit
|
||||
TIM_ClearITPendingBit(HW_ENC_TIM, TIM_IT_Update);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
9
main.c
9
main.c
|
|
@ -167,11 +167,6 @@ int main(void) {
|
|||
|
||||
mc_configuration mcconf;
|
||||
conf_general_read_mc_configuration(&mcconf);
|
||||
|
||||
#if ENCODER_ENABLE
|
||||
encoder_init(mcconf.m_encoder_counts);
|
||||
#endif
|
||||
|
||||
mc_interface_init(&mcconf);
|
||||
|
||||
commands_init();
|
||||
|
|
@ -208,8 +203,8 @@ int main(void) {
|
|||
for(;;) {
|
||||
chThdSleepMilliseconds(10);
|
||||
|
||||
#if ENCODER_ENABLE
|
||||
if (encoder_is_configured()) {
|
||||
// comm_can_set_pos(0, encoder_read_deg());
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -118,6 +118,22 @@ void mc_interface_init(mc_configuration *configuration) {
|
|||
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 encoder
|
||||
#if !WS2811_ENABLE
|
||||
switch (m_conf.m_sensor_port_mode) {
|
||||
case SENSOR_PORT_MODE_ABI:
|
||||
encoder_init_abi(m_conf.m_encoder_counts);
|
||||
break;
|
||||
|
||||
case SENSOR_PORT_MODE_AS5047_SPI:
|
||||
encoder_init_as5047p_spi();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
// Initialize selected implementation
|
||||
switch (m_conf.motor_type) {
|
||||
case MOTOR_TYPE_BLDC:
|
||||
|
|
@ -139,6 +155,28 @@ const volatile mc_configuration* mc_interface_get_configuration(void) {
|
|||
}
|
||||
|
||||
void mc_interface_set_configuration(mc_configuration *configuration) {
|
||||
#if !WS2811_ENABLE
|
||||
if (m_conf.m_sensor_port_mode != configuration->m_sensor_port_mode) {
|
||||
encoder_deinit();
|
||||
switch (configuration->m_sensor_port_mode) {
|
||||
case SENSOR_PORT_MODE_ABI:
|
||||
encoder_init_abi(configuration->m_encoder_counts);
|
||||
break;
|
||||
|
||||
case SENSOR_PORT_MODE_AS5047_SPI:
|
||||
encoder_init_as5047p_spi();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (configuration->m_sensor_port_mode == SENSOR_PORT_MODE_ABI) {
|
||||
encoder_set_counts(configuration->m_encoder_counts);
|
||||
}
|
||||
#endif
|
||||
|
||||
if (m_conf.motor_type == MOTOR_TYPE_FOC
|
||||
&& configuration->motor_type != MOTOR_TYPE_FOC) {
|
||||
mcpwm_foc_deinit();
|
||||
|
|
@ -432,7 +470,7 @@ float mc_interface_get_duty_cycle_now(void) {
|
|||
return ret;
|
||||
}
|
||||
|
||||
float mc_interface_get_switching_frequency_now(void) {
|
||||
float mc_interface_get_sampling_frequency_now(void) {
|
||||
float ret = 0.0;
|
||||
|
||||
switch (m_conf.motor_type) {
|
||||
|
|
@ -442,7 +480,7 @@ float mc_interface_get_switching_frequency_now(void) {
|
|||
break;
|
||||
|
||||
case MOTOR_TYPE_FOC:
|
||||
ret = mcpwm_foc_get_switching_frequency_now();
|
||||
ret = mcpwm_foc_get_switching_frequency_now() / 2.0;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -918,14 +956,14 @@ void mc_interface_mc_timer_isr(void) {
|
|||
}
|
||||
|
||||
// Watt and ah counters
|
||||
const float f_sw = mc_interface_get_switching_frequency_now();
|
||||
const float f_samp = mc_interface_get_sampling_frequency_now();
|
||||
if (fabsf(current) > 1.0) {
|
||||
// Some extra filtering
|
||||
static float curr_diff_sum = 0.0;
|
||||
static float curr_diff_samples = 0;
|
||||
|
||||
curr_diff_sum += current_in / f_sw;
|
||||
curr_diff_samples += 1.0 / f_sw;
|
||||
curr_diff_sum += current_in / f_samp;
|
||||
curr_diff_samples += 1.0 / f_samp;
|
||||
|
||||
if (curr_diff_samples >= 0.01) {
|
||||
if (curr_diff_sum > 0.0) {
|
||||
|
|
@ -974,7 +1012,7 @@ void mc_interface_mc_timer_isr(void) {
|
|||
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_f_sw_samples[m_sample_now] = (int16_t)(f_samp / 10.0);
|
||||
|
||||
m_status_samples[m_sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
|
||||
|
||||
|
|
@ -1016,7 +1054,7 @@ void mc_interface_adc_inj_int_handler(void) {
|
|||
* The configaration to update.
|
||||
*/
|
||||
static void update_override_limits(volatile mc_configuration *conf) {
|
||||
const float temp = NTC_TEMP(ADC_IND_TEMP_MOS1);
|
||||
const float temp = NTC_TEMP(ADC_IND_TEMP_MOS2);
|
||||
const float v_in = GET_INPUT_VOLTAGE();
|
||||
|
||||
// Temperature
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ void mc_interface_brake_now(void);
|
|||
void mc_interface_release_motor(void);
|
||||
float mc_interface_get_duty_cycle_set(void);
|
||||
float mc_interface_get_duty_cycle_now(void);
|
||||
float mc_interface_get_switching_frequency_now(void);
|
||||
float mc_interface_get_sampling_frequency_now(void);
|
||||
float mc_interface_get_rpm(void);
|
||||
float mc_interface_get_amp_hours(bool reset);
|
||||
float mc_interface_get_amp_hours_charged(bool reset);
|
||||
|
|
|
|||
|
|
@ -279,8 +279,8 @@
|
|||
#ifndef MCCONF_FOC_HALL_TAB_7
|
||||
#define MCCONF_FOC_HALL_TAB_7 255
|
||||
#endif
|
||||
#ifndef MCCONF_FOC_HALL_ERPM
|
||||
#define MCCONF_FOC_HALL_ERPM 2000.0 // ERPM above which only the observer is used
|
||||
#ifndef MCCONF_FOC_SL_ERPM
|
||||
#define MCCONF_FOC_SL_ERPM 2500.0 // ERPM above which only the observer is used
|
||||
#endif
|
||||
|
||||
// Misc
|
||||
|
|
@ -299,5 +299,8 @@
|
|||
#ifndef MCCONF_M_ENCODER_COUNTS
|
||||
#define MCCONF_M_ENCODER_COUNTS 8192 // The number of encoder counts
|
||||
#endif
|
||||
#ifndef MCCONF_M_SENSOR_PORT_MODE
|
||||
#define MCCONF_M_SENSOR_PORT_MODE SENSOR_PORT_MODE_HALL // The mode of the hall_encoder port
|
||||
#endif
|
||||
|
||||
#endif /* MCCONF_DEFAULT_H_ */
|
||||
|
|
|
|||
|
|
@ -25,44 +25,55 @@
|
|||
#ifndef MCCONF_STEN_H_
|
||||
#define MCCONF_STEN_H_
|
||||
|
||||
/*
|
||||
* HW: 4.10
|
||||
*/
|
||||
|
||||
/*
|
||||
* Parameters
|
||||
*/
|
||||
#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_BLDC
|
||||
#define MCCONF_L_CURRENT_MAX 35.0 // Current limit in Amperes (Upper)
|
||||
#define MCCONF_L_CURRENT_MIN -30.0 // Current limit in Amperes (Lower)
|
||||
#define MCCONF_L_MAX_ABS_CURRENT 100.0 // The maximum absolute current above which a fault is generated
|
||||
#define MCCONF_L_MAX_ABS_CURRENT 130.0 // The maximum absolute current above which a fault is generated
|
||||
#define MCCONF_L_SLOW_ABS_OVERCURRENT 1 // Use the filtered (and hence slower) current for the overcurrent fault detection
|
||||
#define MCCONF_L_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper)
|
||||
#define MCCONF_L_IN_CURRENT_MIN -20.0 // Input current limit in Amperes (Lower)
|
||||
#define MCCONF_L_RPM_MAX 80000.0 // The motor speed limit (Upper)
|
||||
#define MCCONF_L_RPM_MIN -80000.0 // The motor speed limit (Lower)
|
||||
#define MCCONF_L_MIN_VOLTAGE 20.0 // Minimum input voltage
|
||||
#define MCCONF_L_MAX_VOLTAGE 50.0 // Maximum input voltage
|
||||
#define MCCONF_CC_STARTUP_BOOST_DUTY 0.02 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
|
||||
#define MCCONF_L_MIN_VOLTAGE 10.0 // Minimum input voltage
|
||||
#define MCCONF_L_MAX_VOLTAGE 57.0 // Maximum input voltage
|
||||
#define MCCONF_CC_STARTUP_BOOST_DUTY 0.03 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
|
||||
#define MCCONF_L_RPM_LIMIT_NEG_TORQUE 0 // Use negative torque to limit the RPM
|
||||
#define MCCONF_L_CURR_MAX_RPM_FBRAKE 1500 // Maximum electrical RPM to use full brake at
|
||||
|
||||
#define MCCONF_L_BATTERY_CUT_START 41.0 // Start limiting the positive current at this voltage
|
||||
#define MCCONF_L_BATTERY_CUT_END 39.0 // Limit the positive current completely at this voltage
|
||||
|
||||
// Sensorless settings
|
||||
#define MCCONF_SENSOR_MODE SENSOR_MODE_SENSORLESS // Sensor mode
|
||||
#define MCCONF_SL_MIN_RPM 250 // Auto-commutate below this RPM
|
||||
#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 500.0 // Minimum RPM to calculate the BEMF coupling from
|
||||
#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 1100.0 // Minimum RPM to calculate the BEMF coupling from
|
||||
#define MCCONF_SL_CYCLE_INT_LIMIT 80.0 // Flux integrator limit 0 ERPM
|
||||
#define MCCONF_SL_PHASE_ADVANCE_AT_BR 0.8 // Flux integrator limit percentage at MCPWM_CYCLE_INT_START_RPM_BR ERPM
|
||||
#define MCCONF_SL_BEMF_COUPLING_K 750.0 // Input voltage to bemf coupling constant
|
||||
|
||||
// FOC settings
|
||||
#define MCCONF_FOC_CURRENT_KP 0.03
|
||||
#define MCCONF_FOC_CURRENT_KI 50.0
|
||||
#define MCCONF_FOC_F_SW 20000.0
|
||||
#define MCCONF_FOC_MOTOR_L 0.000007
|
||||
#define MCCONF_FOC_MOTOR_R 0.015
|
||||
#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00245
|
||||
#define MCCONF_FOC_OBSERVER_GAIN 9e7
|
||||
#define MCCONF_FOC_OPENLOOP_RPM 600.0
|
||||
#define MCCONF_FOC_SL_OPENLOOP_HYST 0.5
|
||||
#define MCCONF_FOC_SL_OPENLOOP_TIME 0.5
|
||||
|
||||
// Speed PID parameters
|
||||
#define MCCONF_S_PID_KP 0.0001 // Proportional gain
|
||||
#define MCCONF_S_PID_KI 0.002 // Integral gain
|
||||
#define MCCONF_S_PID_KD 0.0 // Derivative gain
|
||||
#define MCCONF_S_PID_MIN_RPM 1200.0 // Minimum allowed RPM
|
||||
|
||||
// Position PID parameters
|
||||
#define MCCONF_P_PID_KP 0.0001 // Proportional gain
|
||||
#define MCCONF_P_PID_KI 0.002 // Integral gain
|
||||
#define MCCONF_P_PID_KD 0.0 // Derivative gain
|
||||
|
||||
// Current control parameters
|
||||
#define MCCONF_CC_GAIN 0.0046 // Current controller error gain
|
||||
#define MCCONF_CC_MIN_CURRENT 0.05 // Minimum allowed current
|
||||
|
||||
#endif /* MCCONF_STEN_H_ */
|
||||
|
|
|
|||
4
mcpwm.c
4
mcpwm.c
|
|
@ -1916,7 +1916,7 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
|
|||
|
||||
mc_interface_mc_timer_isr();
|
||||
|
||||
if (ENCODER_ENABLE) {
|
||||
if (encoder_is_configured()) {
|
||||
run_pid_control_pos(1.0 / switching_frequency_now);
|
||||
}
|
||||
|
||||
|
|
@ -2045,7 +2045,7 @@ void mcpwm_reset_hall_detect_table(void) {
|
|||
int mcpwm_get_hall_detect_result(int8_t *table) {
|
||||
if (WS2811_ENABLE) {
|
||||
return -2;
|
||||
} else if (ENCODER_ENABLE) {
|
||||
} else if (conf->m_sensor_port_mode != SENSOR_PORT_MODE_HALL) {
|
||||
return -3;
|
||||
}
|
||||
|
||||
|
|
|
|||
146
mcpwm_foc.c
146
mcpwm_foc.c
|
|
@ -120,6 +120,7 @@ static void run_pid_control_speed(float dt);
|
|||
static void stop_pwm_hw(void);
|
||||
static void start_pwm_hw(void);
|
||||
static int read_hall(void);
|
||||
static float correct_encoder(float obs_angle, float enc_angle, float speed);
|
||||
static float correct_hall(float angle, float speed, float dt);
|
||||
|
||||
// Threads
|
||||
|
|
@ -829,7 +830,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
|||
// Disable timeout
|
||||
systime_t tout = timeout_get_timeout_msec();
|
||||
float tout_c = timeout_get_brake_current();
|
||||
timeout_configure(60000, 0.0);
|
||||
timeout_configure(600000, 0.0);
|
||||
|
||||
// Save configuration
|
||||
float offset_old = m_conf->foc_encoder_offset;
|
||||
|
|
@ -872,34 +873,68 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
|||
// Inverted and ratio
|
||||
chThdSleepMilliseconds(1000);
|
||||
|
||||
const int it_rat = 20;
|
||||
float s_sum = 0.0;
|
||||
float c_sum = 0.0;
|
||||
for (int i = 0; i < 10; i++) {
|
||||
float first = m_phase_now_encoder;
|
||||
|
||||
for (int i = 0; i < it_rat; i++) {
|
||||
float phase_old = m_phase_now_encoder;
|
||||
float phase_ovr_tmp = m_phase_now_override;
|
||||
for (float i = phase_ovr_tmp; i < phase_ovr_tmp + 0.8 * M_PI;
|
||||
for (float i = phase_ovr_tmp; i < phase_ovr_tmp + (2.0 / 3.0) * M_PI;
|
||||
i += (2.0 * M_PI) / 500.0) {
|
||||
m_phase_now_override = i;
|
||||
chThdSleepMilliseconds(2);
|
||||
chThdSleepMilliseconds(1);
|
||||
}
|
||||
chThdSleepMilliseconds(1000);
|
||||
float diff = utils_angle_difference(
|
||||
m_phase_now_encoder * (180.0 / M_PI),
|
||||
phase_old * (180.0 / M_PI));
|
||||
utils_norm_angle_rad((float*)&m_phase_now_override);
|
||||
chThdSleepMilliseconds(300);
|
||||
float diff = utils_angle_difference_rad(m_phase_now_encoder, phase_old);
|
||||
|
||||
float s, c;
|
||||
sincosf(diff * M_PI / 180.0, &s, &c);
|
||||
sincosf(diff, &s, &c);
|
||||
s_sum += s;
|
||||
c_sum += c;
|
||||
|
||||
if (print) {
|
||||
commands_printf("%.2f", (double)diff);
|
||||
commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
|
||||
}
|
||||
|
||||
if (i > 3 && fabsf(utils_angle_difference_rad(m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
first = m_phase_now_encoder;
|
||||
|
||||
for (int i = 0; i < it_rat; i++) {
|
||||
float phase_old = m_phase_now_encoder;
|
||||
float phase_ovr_tmp = m_phase_now_override;
|
||||
for (float i = phase_ovr_tmp; i > phase_ovr_tmp - (2.0 / 3.0) * M_PI;
|
||||
i -= (2.0 * M_PI) / 500.0) {
|
||||
m_phase_now_override = i;
|
||||
chThdSleepMilliseconds(1);
|
||||
}
|
||||
utils_norm_angle_rad((float*)&m_phase_now_override);
|
||||
chThdSleepMilliseconds(300);
|
||||
float diff = utils_angle_difference_rad(phase_old, m_phase_now_encoder);
|
||||
|
||||
float s, c;
|
||||
sincosf(diff, &s, &c);
|
||||
s_sum += s;
|
||||
c_sum += c;
|
||||
|
||||
if (print) {
|
||||
commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
|
||||
}
|
||||
|
||||
if (i > 3 && fabsf(utils_angle_difference_rad(m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
float diff = atan2f(s_sum, c_sum) * 180.0 / M_PI;
|
||||
*inverted = diff < 0.0;
|
||||
*ratio = roundf((0.8 * 180.0) /
|
||||
*ratio = roundf(((2.0 / 3.0) * 180.0) /
|
||||
fabsf(diff));
|
||||
|
||||
m_conf->foc_encoder_inverted = *inverted;
|
||||
|
|
@ -920,33 +955,37 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
|||
commands_printf("Enc: %.2f", (double)encoder_read_deg());
|
||||
}
|
||||
|
||||
const int it_ofs = m_conf->foc_encoder_ratio * 3.0;
|
||||
s_sum = 0.0;
|
||||
c_sum = 0.0;
|
||||
for (int i = 0;i < 5;i++) {
|
||||
m_phase_now_override = (float)i * M_PI / 5.0;
|
||||
chThdSleepMilliseconds(1000);
|
||||
|
||||
for (int i = 0;i < it_ofs;i++) {
|
||||
m_phase_now_override = ((float)i * 2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
|
||||
chThdSleepMilliseconds(500);
|
||||
|
||||
float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
|
||||
float s, c;
|
||||
sincosf(m_phase_now_encoder - m_phase_now_override, &s, &c);
|
||||
sincosf(diff, &s, &c);
|
||||
s_sum += s;
|
||||
c_sum += c;
|
||||
|
||||
if (print) {
|
||||
commands_printf("%.2f", (double)((m_phase_now_encoder - m_phase_now_override) * 180.0 / M_PI));
|
||||
commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 3;i >= -1;i--) {
|
||||
m_phase_now_override = (float)i * M_PI / 5.0;
|
||||
chThdSleepMilliseconds(1000);
|
||||
for (int i = it_ofs;i > 0;i--) {
|
||||
m_phase_now_override = ((float)i * 2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
|
||||
chThdSleepMilliseconds(500);
|
||||
|
||||
float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
|
||||
float s, c;
|
||||
sincosf(m_phase_now_encoder - m_phase_now_override, &s, &c);
|
||||
sincosf(diff, &s, &c);
|
||||
s_sum += s;
|
||||
c_sum += c;
|
||||
|
||||
if (print) {
|
||||
commands_printf("%.2f", (double)((m_phase_now_encoder - m_phase_now_override) * 180.0 / M_PI));
|
||||
commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1370,8 +1409,10 @@ void mcpwm_foc_adc_inj_int_handler(void) {
|
|||
|
||||
m_motor_state.v_bus = GET_INPUT_VOLTAGE();
|
||||
|
||||
#if ENCODER_ENABLE
|
||||
float phase_tmp = encoder_read_deg();
|
||||
float enc_ang = 0;
|
||||
if (encoder_is_configured()) {
|
||||
enc_ang = encoder_read_deg();
|
||||
float phase_tmp = enc_ang;
|
||||
if (m_conf->foc_encoder_inverted) {
|
||||
phase_tmp = 360.0 - phase_tmp;
|
||||
}
|
||||
|
|
@ -1379,7 +1420,7 @@ void mcpwm_foc_adc_inj_int_handler(void) {
|
|||
phase_tmp -= m_conf->foc_encoder_offset;
|
||||
utils_norm_angle((float*)&phase_tmp);
|
||||
m_phase_now_encoder = phase_tmp * (M_PI / 180.0);
|
||||
#endif
|
||||
}
|
||||
|
||||
static float phase_before = 0.0;
|
||||
const float phase_diff = utils_angle_difference_rad(m_motor_state.phase, phase_before);
|
||||
|
|
@ -1410,7 +1451,7 @@ void mcpwm_foc_adc_inj_int_handler(void) {
|
|||
// observer has lost tracking. Use duty cycle control with the lowest duty cycle
|
||||
// to get as smooth braking as possible.
|
||||
if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE
|
||||
&& (m_conf->foc_sensor_mode != FOC_SENSOR_MODE_ENCODER)
|
||||
// && (m_conf->foc_sensor_mode != FOC_SENSOR_MODE_ENCODER) // Don't use this with encoderss
|
||||
&& fabsf(duty_filtered) < 0.03) {
|
||||
control_duty = true;
|
||||
duty_set = 0.0;
|
||||
|
|
@ -1479,7 +1520,7 @@ void mcpwm_foc_adc_inj_int_handler(void) {
|
|||
switch (m_conf->foc_sensor_mode) {
|
||||
case FOC_SENSOR_MODE_ENCODER:
|
||||
if (encoder_index_found()) {
|
||||
m_motor_state.phase = m_phase_now_encoder;
|
||||
m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
|
||||
} else {
|
||||
// Rotate the motor in open loop if the index isn't found.
|
||||
m_motor_state.phase = m_phase_now_encoder_no_index;
|
||||
|
|
@ -1492,6 +1533,10 @@ void mcpwm_foc_adc_inj_int_handler(void) {
|
|||
case FOC_SENSOR_MODE_HALL:
|
||||
m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
|
||||
m_motor_state.phase = m_phase_now_observer;
|
||||
|
||||
if (!m_phase_override) {
|
||||
id_set_tmp = 0.0;
|
||||
}
|
||||
break;
|
||||
case FOC_SENSOR_MODE_SENSORLESS:
|
||||
if (m_phase_observer_override) {
|
||||
|
|
@ -1571,7 +1616,7 @@ void mcpwm_foc_adc_inj_int_handler(void) {
|
|||
|
||||
switch (m_conf->foc_sensor_mode) {
|
||||
case FOC_SENSOR_MODE_ENCODER:
|
||||
m_motor_state.phase = m_phase_now_encoder;
|
||||
m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
|
||||
break;
|
||||
case FOC_SENSOR_MODE_HALL:
|
||||
m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
|
||||
|
|
@ -1602,11 +1647,12 @@ void mcpwm_foc_adc_inj_int_handler(void) {
|
|||
|
||||
// Track position control angle
|
||||
// TODO: Have another look at this.
|
||||
#if ENCODER_ENABLE
|
||||
float angle_now = encoder_read_deg();
|
||||
#else
|
||||
float angle_now = m_motor_state.phase * (180.0 / M_PI);
|
||||
#endif
|
||||
float angle_now = 0.0;
|
||||
if (encoder_is_configured()) {
|
||||
angle_now = enc_ang;
|
||||
} else {
|
||||
angle_now = m_motor_state.phase * (180.0 / M_PI);
|
||||
}
|
||||
|
||||
if (m_conf->p_pid_ang_div > 0.98 && m_conf->p_pid_ang_div < 1.02) {
|
||||
m_pos_pid_now = angle_now;
|
||||
|
|
@ -2005,11 +2051,11 @@ static void run_pid_control_pos(float angle_now, float angle_set, float dt) {
|
|||
// Compute parameters
|
||||
float error = utils_angle_difference(angle_set, angle_now);
|
||||
|
||||
#if ENCODER_ENABLE
|
||||
if (encoder_is_configured()) {
|
||||
if (m_conf->foc_encoder_inverted) {
|
||||
error = -error;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
p_term = error * m_conf->p_pid_kp;
|
||||
i_term += error * (m_conf->p_pid_ki * dt);
|
||||
|
|
@ -2037,16 +2083,16 @@ static void run_pid_control_pos(float angle_now, float angle_set, float dt) {
|
|||
float output = p_term + i_term + d_term;
|
||||
utils_truncate_number(&output, -1.0, 1.0);
|
||||
|
||||
#if ENCODER_ENABLE
|
||||
if (encoder_is_configured()) {
|
||||
if (encoder_index_found()) {
|
||||
m_iq_set = output * m_conf->lo_current_max;
|
||||
} else {
|
||||
// Rotate the motor with 40 % power until the encoder index is found.
|
||||
m_iq_set = 0.4 * m_conf->lo_current_max;
|
||||
}
|
||||
#else
|
||||
} else {
|
||||
m_iq_set = output * m_conf->lo_current_max;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
static void run_pid_control_speed(float dt) {
|
||||
|
|
@ -2133,18 +2179,38 @@ static int read_hall(void) {
|
|||
return READ_HALL1() | (READ_HALL2() << 1) | (READ_HALL3() << 2);
|
||||
}
|
||||
|
||||
static float correct_encoder(float obs_angle, float enc_angle, float speed) {
|
||||
float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
|
||||
static bool using_encoder = true;
|
||||
|
||||
// Hysteresis 5 % of total speed
|
||||
float hyst = m_conf->foc_sl_erpm * 0.05;
|
||||
if (using_encoder) {
|
||||
if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
|
||||
using_encoder = false;
|
||||
}
|
||||
} else {
|
||||
if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
|
||||
using_encoder = true;
|
||||
}
|
||||
}
|
||||
|
||||
return using_encoder ? enc_angle : obs_angle;
|
||||
}
|
||||
|
||||
static float correct_hall(float angle, float speed, float dt) {
|
||||
static int ang_hall_int_prev = -1;
|
||||
float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
|
||||
static bool using_hall = true;
|
||||
|
||||
// Hysteresis of 100 rpm
|
||||
// Hysteresis 5 % of total speed
|
||||
float hyst = m_conf->foc_sl_erpm * 0.05;
|
||||
if (using_hall) {
|
||||
if (rpm_abs > (m_conf->foc_hall_sl_erpm + 100)) {
|
||||
if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
|
||||
using_hall = false;
|
||||
}
|
||||
} else {
|
||||
if (rpm_abs < (m_conf->foc_hall_sl_erpm - 100)) {
|
||||
if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
|
||||
using_hall = true;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -248,7 +248,7 @@
|
|||
/*
|
||||
* SPI driver system settings.
|
||||
*/
|
||||
#define STM32_SPI_USE_SPI1 FALSE
|
||||
#define STM32_SPI_USE_SPI1 TRUE
|
||||
#define STM32_SPI_USE_SPI2 FALSE
|
||||
#define STM32_SPI_USE_SPI3 FALSE
|
||||
#define STM32_SPI_SPI1_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 0)
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@
|
|||
#include "comm_can.h"
|
||||
#include "utils.h"
|
||||
#include "timeout.h"
|
||||
#include "encoder.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
|
@ -221,7 +222,7 @@ void terminal_process_string(char *str) {
|
|||
sscanf(argv[1], "%f", ¤t);
|
||||
|
||||
if (current > 0.0 && current <= mcconf.l_current_max) {
|
||||
#if ENCODER_ENABLE
|
||||
if (encoder_is_configured()) {
|
||||
mc_motor_type type_old = mcconf.motor_type;
|
||||
mcconf.motor_type = MOTOR_TYPE_FOC;
|
||||
mc_interface_set_configuration(&mcconf);
|
||||
|
|
@ -237,9 +238,9 @@ void terminal_process_string(char *str) {
|
|||
commands_printf("Offset : %.2f", (double)offset);
|
||||
commands_printf("Ratio : %.2f", (double)ratio);
|
||||
commands_printf("Inverted : %s\n", inverted ? "true" : "false");
|
||||
#else
|
||||
} else {
|
||||
commands_printf("Encoder not enabled.\n");
|
||||
#endif
|
||||
}
|
||||
} else {
|
||||
commands_printf("Invalid argument(s).\n");
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue