2022-02-19 06:37:12 -08:00
|
|
|
/*
|
|
|
|
Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se
|
|
|
|
Copyright 2022 Jakub Tomczak
|
|
|
|
|
|
|
|
This file is part of the VESC firmware.
|
|
|
|
|
|
|
|
The VESC firmware 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.
|
|
|
|
|
|
|
|
The VESC firmware 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/>.
|
|
|
|
*/
|
2022-01-20 12:23:37 -08:00
|
|
|
|
|
|
|
#include "encoder.h"
|
2022-02-20 04:44:01 -08:00
|
|
|
#include "encoder_datatype.h"
|
|
|
|
#include "encoder_cfg.h"
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-03-16 10:40:51 -07:00
|
|
|
#include "utils_math.h"
|
2022-02-19 09:51:34 -08:00
|
|
|
#include "commands.h"
|
|
|
|
#include "mcpwm_foc.h"
|
|
|
|
#include "mc_interface.h"
|
|
|
|
#include "mempools.h"
|
|
|
|
#include "terminal.h"
|
2022-02-21 17:36:14 -08:00
|
|
|
#include "app.h"
|
2022-02-19 09:51:34 -08:00
|
|
|
|
|
|
|
#include <math.h>
|
2022-01-09 08:10:40 -08:00
|
|
|
|
|
|
|
static encoder_type_t encoder_type_now = ENCODER_TYPE_NONE;
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
// Private functions
|
|
|
|
static void terminal_encoder(int argc, const char **argv);
|
|
|
|
static void terminal_encoder_clear_errors(int argc, const char **argv);
|
|
|
|
static void terminal_encoder_clear_multiturn(int argc, const char **argv);
|
2022-02-19 12:00:52 -08:00
|
|
|
static void timer_start(float rate);
|
2022-02-19 09:51:34 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
bool encoder_init(volatile mc_configuration *conf) {
|
|
|
|
bool res = false;
|
2022-01-09 08:10:40 -08:00
|
|
|
|
|
|
|
if (encoder_type_now != ENCODER_TYPE_NONE) {
|
2022-02-20 07:22:38 -08:00
|
|
|
encoder_deinit();
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
nvicDisableVector(HW_ENC_EXTI_CH);
|
|
|
|
nvicDisableVector(HW_ENC_TIM_ISR_CH);
|
|
|
|
TIM_DeInit(HW_ENC_TIM);
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
switch (conf->m_sensor_port_mode) {
|
|
|
|
case SENSOR_PORT_MODE_ABI: {
|
|
|
|
SENSOR_PORT_5V();
|
|
|
|
|
2022-02-20 04:44:01 -08:00
|
|
|
encoder_cfg_ABI.counts = conf->m_encoder_counts;
|
2022-02-19 09:51:34 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
if (!enc_abi_init(&encoder_cfg_ABI)) {
|
2022-02-19 09:51:34 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
2022-02-20 07:22:38 -08:00
|
|
|
return false;
|
2022-02-19 09:51:34 -08:00
|
|
|
}
|
2022-02-20 07:22:38 -08:00
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_ABI;
|
2022-02-20 07:22:38 -08:00
|
|
|
res = true;
|
2022-02-19 09:51:34 -08:00
|
|
|
} break;
|
|
|
|
|
|
|
|
case SENSOR_PORT_MODE_AS5047_SPI: {
|
|
|
|
SENSOR_PORT_3V3();
|
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
if (!enc_as504x_init(&encoder_cfg_as504x)) {
|
|
|
|
return false;
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
2022-02-19 14:28:58 -08:00
|
|
|
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_AS504x;
|
2022-02-20 04:44:01 -08:00
|
|
|
timer_start(10000);
|
2022-02-19 12:00:52 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
res = true;
|
2022-02-19 09:51:34 -08:00
|
|
|
} break;
|
|
|
|
|
2022-11-15 11:34:30 -08:00
|
|
|
case SENSOR_PORT_MODE_MT6816_SPI_HW: {
|
2022-02-19 09:51:34 -08:00
|
|
|
SENSOR_PORT_5V();
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
if (!enc_mt6816_init(&encoder_cfg_mt6816)) {
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
2022-02-20 07:22:38 -08:00
|
|
|
return false;
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
2022-02-19 14:28:58 -08:00
|
|
|
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_MT6816;
|
2022-02-20 04:44:01 -08:00
|
|
|
timer_start(10000);
|
2022-02-19 12:00:52 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
res = true;
|
2022-02-19 09:51:34 -08:00
|
|
|
} break;
|
|
|
|
|
2022-11-15 11:34:30 -08:00
|
|
|
// ssc (3 wire) sw spi on hall pins
|
|
|
|
case SENSOR_PORT_MODE_TLE5014_SSC_SW: {
|
|
|
|
SENSOR_PORT_5V();
|
|
|
|
|
|
|
|
// reuse global config, so must set up complete ssc config
|
|
|
|
spi_bb_state sw_ssc = {
|
|
|
|
HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, // nss
|
|
|
|
HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, // sck
|
|
|
|
HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, // mosi
|
|
|
|
HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, // miso
|
|
|
|
ssc_type_sw,
|
|
|
|
0, // has_started
|
|
|
|
0, // has_error
|
|
|
|
{{NULL, NULL}, NULL, NULL} // Mutex
|
|
|
|
};
|
|
|
|
encoder_cfg_tle5012.sw_spi = sw_ssc;
|
|
|
|
|
|
|
|
if (!enc_tle5012_init_sw_ssc(&encoder_cfg_tle5012)) {
|
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
encoder_type_now = ENCODER_TYPE_TLE5012;
|
|
|
|
timer_start(4000); // slow down sw spi as transactions long
|
|
|
|
|
|
|
|
res = true;
|
|
|
|
} break;
|
|
|
|
|
|
|
|
// ssc (3 wire) hw spi w dma (sw spi using hw spi pins for now)
|
|
|
|
case SENSOR_PORT_MODE_TLE5014_SSC_HW: {
|
|
|
|
SENSOR_PORT_5V();
|
|
|
|
|
|
|
|
// reuse global config, so must set up complete ssc config
|
|
|
|
spi_bb_state sw_ssc = {
|
|
|
|
HW_SPI_PORT_NSS, HW_SPI_PIN_NSS, // nss
|
|
|
|
HW_SPI_PORT_SCK, HW_SPI_PIN_SCK, // sck
|
|
|
|
HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, // mosi
|
|
|
|
HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, // miso (shared dat line)
|
|
|
|
ssc_type_sw,
|
|
|
|
0, // has_started
|
|
|
|
0, // has_error
|
|
|
|
{{NULL, NULL}, NULL, NULL} // Mutex
|
|
|
|
};
|
|
|
|
encoder_cfg_tle5012.sw_spi = sw_ssc;
|
|
|
|
|
|
|
|
if (!enc_tle5012_init_hw_ssc(&encoder_cfg_tle5012)) {
|
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
encoder_type_now = ENCODER_TYPE_TLE5012;
|
|
|
|
// timer_start(10000);
|
|
|
|
timer_start(4000);
|
|
|
|
|
|
|
|
res = true;
|
|
|
|
} break;
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
case SENSOR_PORT_MODE_AD2S1205: {
|
|
|
|
SENSOR_PORT_5V();
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
if (!enc_ad2s1205_init(&encoder_cfg_ad2s1205)) {
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
2022-02-20 07:22:38 -08:00
|
|
|
return false;
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
2022-02-19 12:00:52 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_AD2S1205_SPI;
|
2022-02-20 04:44:01 -08:00
|
|
|
timer_start(10000);
|
2022-02-19 12:00:52 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
res = true;
|
2022-02-19 09:51:34 -08:00
|
|
|
} break;
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
case SENSOR_PORT_MODE_SINCOS: {
|
|
|
|
SENSOR_PORT_5V();
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-20 04:44:01 -08:00
|
|
|
encoder_cfg_sincos.s_gain = conf->foc_encoder_sin_gain;
|
|
|
|
encoder_cfg_sincos.s_offset = conf->foc_encoder_sin_offset;
|
|
|
|
encoder_cfg_sincos.c_gain = conf->foc_encoder_cos_gain;
|
|
|
|
encoder_cfg_sincos.c_offset = conf->foc_encoder_cos_offset;
|
|
|
|
encoder_cfg_sincos.filter_constant = conf->foc_encoder_sincos_filter_constant;
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
if (!enc_sincos_init(&encoder_cfg_sincos)) {
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
2022-02-20 07:22:38 -08:00
|
|
|
return false;
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
2022-02-20 07:22:38 -08:00
|
|
|
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_SINCOS;
|
2022-02-20 07:22:38 -08:00
|
|
|
res = true;
|
2022-02-19 09:51:34 -08:00
|
|
|
} break;
|
|
|
|
|
|
|
|
case SENSOR_PORT_MODE_TS5700N8501:
|
|
|
|
case SENSOR_PORT_MODE_TS5700N8501_MULTITURN: {
|
|
|
|
SENSOR_PORT_5V();
|
|
|
|
app_configuration *appconf = mempools_alloc_appconf();
|
|
|
|
conf_general_read_app_configuration(appconf);
|
|
|
|
if (appconf->app_to_use == APP_ADC ||
|
|
|
|
appconf->app_to_use == APP_UART ||
|
|
|
|
appconf->app_to_use == APP_PPM_UART ||
|
|
|
|
appconf->app_to_use == APP_ADC_UART) {
|
|
|
|
appconf->app_to_use = APP_NONE;
|
2022-02-21 17:36:14 -08:00
|
|
|
app_set_configuration(appconf);
|
2022-02-19 09:51:34 -08:00
|
|
|
conf_general_store_app_configuration(appconf);
|
|
|
|
}
|
|
|
|
mempools_free_appconf(appconf);
|
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
if (!enc_ts5700n8501_init(&encoder_cfg_TS5700N8501)) {
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
2022-02-20 07:22:38 -08:00
|
|
|
return false;
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
2022-02-20 07:22:38 -08:00
|
|
|
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_TS5700N8501;
|
2022-02-20 07:22:38 -08:00
|
|
|
res = true;
|
2022-02-19 09:51:34 -08:00
|
|
|
} break;
|
|
|
|
|
2022-07-18 13:25:03 -07:00
|
|
|
case SENSOR_PORT_MODE_AS5x47U_SPI: {
|
|
|
|
SENSOR_PORT_3V3();
|
|
|
|
|
|
|
|
if (!enc_as5x47u_init(&encoder_cfg_as5x47u)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
encoder_type_now = ENCODER_TYPE_AS5x47U;
|
|
|
|
timer_start(10000);
|
|
|
|
|
|
|
|
res = true;
|
|
|
|
} break;
|
|
|
|
|
2022-10-02 07:33:17 -07:00
|
|
|
case SENSOR_PORT_MODE_BISSC: {
|
|
|
|
SENSOR_PORT_5V();
|
|
|
|
|
|
|
|
encoder_cfg_bissc.enc_res = conf->m_encoder_counts;
|
|
|
|
|
|
|
|
if (!enc_bissc_init(&encoder_cfg_bissc)) {
|
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
encoder_type_now = ENCODER_TYPE_BISSC;
|
|
|
|
timer_start(10000);
|
|
|
|
|
|
|
|
res = true;
|
|
|
|
} break;
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
default:
|
|
|
|
SENSOR_PORT_5V();
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
2022-02-19 09:51:34 -08:00
|
|
|
break;
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
2022-02-19 09:51:34 -08:00
|
|
|
|
|
|
|
terminal_register_command_callback(
|
|
|
|
"encoder",
|
2022-11-15 11:34:30 -08:00
|
|
|
"Prints the status of the AS5047, AS5x47U, AD2S1205, TLE5012, MT6816, or TS5700N8501 encoder.",
|
2022-02-19 09:51:34 -08:00
|
|
|
0,
|
|
|
|
terminal_encoder);
|
|
|
|
|
|
|
|
terminal_register_command_callback(
|
|
|
|
"encoder_clear_errors",
|
|
|
|
"Clear error of the TS5700N8501 encoder.",
|
|
|
|
0,
|
|
|
|
terminal_encoder_clear_errors);
|
|
|
|
|
|
|
|
terminal_register_command_callback(
|
|
|
|
"encoder_clear_multiturn",
|
|
|
|
"Clear multiturn counter of the TS5700N8501 encoder.",
|
|
|
|
0,
|
|
|
|
terminal_encoder_clear_multiturn);
|
|
|
|
|
|
|
|
return res;
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
|
2022-02-20 04:44:01 -08:00
|
|
|
void encoder_deinit(void) {
|
|
|
|
nvicDisableVector(HW_ENC_EXTI_CH);
|
|
|
|
nvicDisableVector(HW_ENC_TIM_ISR_CH);
|
|
|
|
TIM_DeInit(HW_ENC_TIM);
|
|
|
|
|
|
|
|
if (encoder_type_now == ENCODER_TYPE_AS504x) {
|
|
|
|
enc_as504x_deinit(&encoder_cfg_as504x);
|
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
|
|
|
enc_mt6816_deinit(&encoder_cfg_mt6816);
|
2022-11-15 11:34:30 -08:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_TLE5012) {
|
|
|
|
enc_tle5012_deinit(&encoder_cfg_tle5012);
|
2022-02-20 04:44:01 -08:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
|
|
|
|
enc_ad2s1205_deinit(&encoder_cfg_ad2s1205);
|
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_ABI) {
|
2022-02-20 07:22:38 -08:00
|
|
|
enc_abi_deinit(&encoder_cfg_ABI);
|
2022-02-20 04:44:01 -08:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
|
|
|
|
enc_sincos_deinit(&encoder_cfg_sincos);
|
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
|
|
|
enc_ts5700n8501_deinit(&encoder_cfg_TS5700N8501);
|
2022-07-18 13:25:03 -07:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_AS5x47U) {
|
|
|
|
enc_as5x47u_deinit(&encoder_cfg_as5x47u);
|
2022-10-02 07:33:17 -07:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_BISSC) {
|
|
|
|
enc_bissc_deinit(&encoder_cfg_bissc);
|
2022-02-20 04:44:01 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
|
|
}
|
|
|
|
|
2022-01-09 08:10:40 -08:00
|
|
|
float encoder_read_deg(void) {
|
|
|
|
if (encoder_type_now == ENCODER_TYPE_AS504x) {
|
2022-02-20 04:44:01 -08:00
|
|
|
return AS504x_LAST_ANGLE(&encoder_cfg_as504x);
|
2022-01-09 08:10:40 -08:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
2022-02-20 04:44:01 -08:00
|
|
|
return MT6816_LAST_ANGLE(&encoder_cfg_mt6816);
|
2022-11-15 11:34:30 -08:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_TLE5012) {
|
|
|
|
return TLE5012_LAST_ANGLE(&encoder_cfg_tle5012);
|
2022-01-09 08:10:40 -08:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
|
2022-02-20 04:44:01 -08:00
|
|
|
return AD2S1205_LAST_ANGLE(&encoder_cfg_ad2s1205);
|
2022-01-09 08:10:40 -08:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_ABI) {
|
2022-02-20 07:22:38 -08:00
|
|
|
return enc_abi_read_deg(&encoder_cfg_ABI);
|
2022-01-09 08:10:40 -08:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
|
2022-02-20 04:44:01 -08:00
|
|
|
return enc_sincos_read_deg(&encoder_cfg_sincos);
|
2022-01-09 08:10:40 -08:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
2022-02-20 04:44:01 -08:00
|
|
|
return enc_ts5700n8501_read_deg(&encoder_cfg_TS5700N8501);
|
2022-07-18 13:25:03 -07:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_AS5x47U) {
|
|
|
|
return AS5x47U_LAST_ANGLE(&encoder_cfg_as5x47u);
|
2022-10-02 07:33:17 -07:00
|
|
|
} else if (encoder_type_now == ENCODER_TYPE_BISSC) {
|
|
|
|
return BISSC_LAST_ANGLE(&encoder_cfg_bissc);
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
return 0.0;
|
|
|
|
}
|
|
|
|
|
|
|
|
float encoder_read_deg_multiturn(void) {
|
|
|
|
if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
2022-02-20 04:44:01 -08:00
|
|
|
float ts_mt = (float)enc_ts5700n8501_get_abm(&encoder_cfg_TS5700N8501);
|
2022-01-09 08:10:40 -08:00
|
|
|
if (fabsf(ts_mt) > 5000.0) {
|
|
|
|
ts_mt = 0;
|
|
|
|
encoder_reset_multiturn();
|
|
|
|
}
|
|
|
|
|
|
|
|
ts_mt += 5000;
|
|
|
|
|
|
|
|
return encoder_read_deg() / 10000.0 + (360 * ts_mt) / 10000.0;
|
|
|
|
} else {
|
|
|
|
return encoder_read_deg();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
encoder_type_t encoder_is_configured(void) {
|
|
|
|
return encoder_type_now;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool encoder_index_found(void) {
|
2022-02-20 07:22:38 -08:00
|
|
|
if (encoder_type_now == ENCODER_TYPE_ABI) {
|
|
|
|
return encoder_cfg_ABI.state.index_found;
|
|
|
|
} else {
|
|
|
|
return true;
|
|
|
|
}
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
void encoder_reset_multiturn(void) {
|
|
|
|
if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
2022-02-20 04:44:01 -08:00
|
|
|
return enc_ts5700n8501_reset_multiturn(&encoder_cfg_TS5700N8501);
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
void encoder_reset_errors(void) {
|
|
|
|
if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
2022-02-20 04:44:01 -08:00
|
|
|
enc_ts5700n8501_reset_errors(&encoder_cfg_TS5700N8501);
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
// Check for encoder faults that should stop the motor with a fault code.
|
2022-02-19 09:51:34 -08:00
|
|
|
void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_motor) {
|
2022-02-20 03:47:34 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
// Only generate fault code when the encoder is being used. Note that encoder faults
|
|
|
|
// that occur above the sensorless ERPM won't stop the motor.
|
2022-02-20 03:47:34 -08:00
|
|
|
bool is_foc_encoder = m_conf->motor_type == MOTOR_TYPE_FOC &&
|
2022-02-19 09:51:34 -08:00
|
|
|
m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
|
2022-02-20 03:47:34 -08:00
|
|
|
mcpwm_foc_is_using_encoder();
|
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
if (is_foc_encoder) {
|
|
|
|
switch (m_conf->m_sensor_port_mode) {
|
|
|
|
case SENSOR_PORT_MODE_AS5047_SPI:
|
|
|
|
if (encoder_cfg_as504x.state.spi_error_rate > 0.05) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
|
|
|
|
}
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
if (encoder_cfg_as504x.sw_spi.mosi_gpio != NULL) {
|
|
|
|
AS504x_diag diag = encoder_cfg_as504x.state.sensor_diag;
|
|
|
|
if (!diag.is_connected) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
|
|
|
|
}
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
if (diag.is_Comp_high) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false);
|
|
|
|
} else if(diag.is_Comp_low) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, is_second_motor, false);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
break;
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-11-15 11:34:30 -08:00
|
|
|
case SENSOR_PORT_MODE_MT6816_SPI_HW:
|
2022-02-20 07:22:38 -08:00
|
|
|
if (encoder_cfg_mt6816.state.encoder_no_magnet_error_rate > 0.05) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false);
|
|
|
|
}
|
|
|
|
break;
|
2022-11-15 11:34:30 -08:00
|
|
|
|
|
|
|
case SENSOR_PORT_MODE_TLE5014_SSC_HW:
|
|
|
|
case SENSOR_PORT_MODE_TLE5014_SSC_SW:
|
|
|
|
if (encoder_cfg_tle5012.state.spi_error_rate > 0.10) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_FAULT, is_second_motor, false);
|
|
|
|
}
|
|
|
|
if (encoder_cfg_tle5012.state.last_status_error != NO_ERROR &&
|
|
|
|
encoder_cfg_tle5012.state.last_status_error != CRC_ERROR) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_FAULT, is_second_motor, false);
|
|
|
|
} // allow some crc errors below 10% error rate
|
|
|
|
break;
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
case SENSOR_PORT_MODE_SINCOS:
|
|
|
|
if (encoder_cfg_sincos.state.signal_low_error_rate > 0.05) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, is_second_motor, false);
|
|
|
|
}
|
|
|
|
if (encoder_cfg_sincos.state.signal_above_max_error_rate > 0.05) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, is_second_motor, false);
|
|
|
|
}
|
|
|
|
break;
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
case SENSOR_PORT_MODE_AD2S1205:
|
|
|
|
if (encoder_cfg_ad2s1205.state.resolver_loss_of_tracking_error_rate > 0.05) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOT, is_second_motor, false);
|
|
|
|
}
|
|
|
|
if (encoder_cfg_ad2s1205.state.resolver_degradation_of_signal_error_rate > 0.05) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_RESOLVER_DOS, is_second_motor, false);
|
|
|
|
}
|
|
|
|
if (encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_rate > 0.04) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOS, is_second_motor, false);
|
|
|
|
}
|
|
|
|
break;
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-07-18 13:25:03 -07:00
|
|
|
case SENSOR_PORT_MODE_AS5x47U_SPI:
|
|
|
|
if (encoder_cfg_as5x47u.state.spi_error_rate > 0.05) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
|
|
|
|
}
|
|
|
|
|
|
|
|
AS5x47U_diag diag = encoder_cfg_as5x47u.state.sensor_diag;
|
|
|
|
if (!diag.is_connected) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (diag.is_Comp_high) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false);
|
2022-07-19 08:35:38 -07:00
|
|
|
} else if (diag.is_Comp_low) {
|
2022-07-18 13:25:03 -07:00
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, is_second_motor, false);
|
2022-07-19 08:35:38 -07:00
|
|
|
} else if (diag.is_broken_hall || diag.is_COF || diag.is_wdtst) {
|
2022-07-18 13:25:03 -07:00
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_FAULT, is_second_motor, false);
|
|
|
|
}
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
2022-10-02 07:33:17 -07:00
|
|
|
case SENSOR_PORT_MODE_BISSC:
|
|
|
|
if (encoder_cfg_bissc.state.spi_comm_error_rate > 0.04) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
|
|
|
|
}
|
|
|
|
if (encoder_cfg_bissc.state.spi_data_error_rate > 0.05) {
|
|
|
|
mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOT, is_second_motor, false);
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
default:
|
|
|
|
break;
|
2022-02-20 03:47:34 -08:00
|
|
|
}
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
void encoder_pin_isr(void) {
|
2022-02-20 07:22:38 -08:00
|
|
|
enc_abi_pin_isr(&encoder_cfg_ABI);
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
void encoder_tim_isr(void) {
|
2022-02-20 07:22:38 -08:00
|
|
|
switch (encoder_type_now) {
|
|
|
|
case ENCODER_TYPE_AS504x:
|
|
|
|
enc_as504x_routine(&encoder_cfg_as504x);
|
|
|
|
break;
|
|
|
|
|
|
|
|
case ENCODER_TYPE_MT6816:
|
|
|
|
enc_mt6816_routine(&encoder_cfg_mt6816);
|
|
|
|
break;
|
|
|
|
|
2022-11-15 11:34:30 -08:00
|
|
|
case ENCODER_TYPE_TLE5012:
|
|
|
|
enc_tle5012_routine(&encoder_cfg_tle5012);
|
|
|
|
break;
|
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
case ENCODER_TYPE_AD2S1205_SPI:
|
|
|
|
enc_ad2s1205_routine(&encoder_cfg_ad2s1205);
|
|
|
|
break;
|
|
|
|
|
2022-07-18 13:25:03 -07:00
|
|
|
case ENCODER_TYPE_AS5x47U:
|
|
|
|
enc_as5x47u_routine(&encoder_cfg_as5x47u);
|
|
|
|
break;
|
|
|
|
|
2022-10-02 07:33:17 -07:00
|
|
|
case ENCODER_TYPE_BISSC:
|
|
|
|
enc_bissc_routine(&encoder_cfg_bissc);
|
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
default:
|
|
|
|
break;
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
static void terminal_encoder(int argc, const char **argv) {
|
|
|
|
(void)argc; (void)argv;
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
const volatile mc_configuration *mcconf = mc_interface_get_configuration();
|
2022-01-09 08:10:40 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
switch (mcconf->m_sensor_port_mode) {
|
|
|
|
case SENSOR_PORT_MODE_AS5047_SPI:
|
|
|
|
commands_printf("SPI encoder value: %d, errors: %d, error rate: %.3f %%",
|
2022-11-15 11:34:30 -08:00
|
|
|
encoder_cfg_as504x.state.spi_val,
|
|
|
|
encoder_cfg_as504x.state.spi_communication_error_count,
|
2022-02-20 07:22:38 -08:00
|
|
|
(double)(encoder_cfg_as504x.state.spi_error_rate * 100.0));
|
2022-02-19 09:51:34 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
if (encoder_cfg_as504x.sw_spi.mosi_gpio != NULL) {
|
2022-02-19 09:51:34 -08:00
|
|
|
commands_printf("\nAS5047 DIAGNOSTICS:\n"
|
2022-02-20 07:22:38 -08:00
|
|
|
"Connected : %u\n"
|
2022-02-19 09:51:34 -08:00
|
|
|
"AGC : %u\n"
|
|
|
|
"Magnitude : %u\n"
|
|
|
|
"COF : %u\n"
|
|
|
|
"OCF : %u\n"
|
|
|
|
"COMP_low : %u\n"
|
2022-02-20 07:22:38 -08:00
|
|
|
"COMP_high : %u",
|
|
|
|
encoder_cfg_as504x.state.sensor_diag.is_connected,
|
|
|
|
encoder_cfg_as504x.state.sensor_diag.AGC_value,
|
|
|
|
encoder_cfg_as504x.state.sensor_diag.magnitude,
|
|
|
|
encoder_cfg_as504x.state.sensor_diag.is_COF,
|
|
|
|
encoder_cfg_as504x.state.sensor_diag.is_OCF,
|
|
|
|
encoder_cfg_as504x.state.sensor_diag.is_Comp_low,
|
|
|
|
encoder_cfg_as504x.state.sensor_diag.is_Comp_high);
|
2022-02-19 09:51:34 -08:00
|
|
|
}
|
2022-02-20 07:22:38 -08:00
|
|
|
break;
|
|
|
|
|
2022-11-15 11:34:30 -08:00
|
|
|
case SENSOR_PORT_MODE_MT6816_SPI_HW:
|
2022-02-20 07:22:38 -08:00
|
|
|
commands_printf("Low flux error (no magnet): errors: %d, error rate: %.3f %%",
|
|
|
|
encoder_cfg_mt6816.state.encoder_no_magnet_error_cnt,
|
|
|
|
(double)(encoder_cfg_mt6816.state.encoder_no_magnet_error_rate * 100.0));
|
|
|
|
break;
|
2022-02-19 09:51:34 -08:00
|
|
|
|
2022-11-15 11:34:30 -08:00
|
|
|
case SENSOR_PORT_MODE_TLE5014_SSC_HW:
|
|
|
|
case SENSOR_PORT_MODE_TLE5014_SSC_SW: ;
|
|
|
|
uint8_t status = encoder_cfg_tle5012.state.last_status_error; // get before other queries
|
|
|
|
double temperature = 0;
|
|
|
|
uint16_t magnet_magnitude = 0;
|
|
|
|
enc_tle5012_get_temperature(&encoder_cfg_tle5012, &temperature);
|
|
|
|
enc_tle5012_get_magnet_magnitude(&encoder_cfg_tle5012, &magnet_magnitude);
|
|
|
|
commands_printf("Last error: %d, ssc error rate: %.3f %%, magnet strength: %d, temp %.2f C",
|
|
|
|
status,
|
|
|
|
(double)(encoder_cfg_tle5012.state.spi_error_rate * 100.0),
|
|
|
|
magnet_magnitude,
|
|
|
|
temperature);
|
|
|
|
// todo, get/report status word (reg 0x00), make "last error" verbose
|
|
|
|
break;
|
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
case SENSOR_PORT_MODE_TS5700N8501:
|
|
|
|
case SENSOR_PORT_MODE_TS5700N8501_MULTITURN: {
|
|
|
|
char sf[9];
|
|
|
|
char almc[9];
|
|
|
|
utils_byte_to_binary(enc_ts5700n8501_get_raw_status(&encoder_cfg_TS5700N8501)[0], sf);
|
|
|
|
utils_byte_to_binary(enc_ts5700n8501_get_raw_status(&encoder_cfg_TS5700N8501)[7], almc);
|
|
|
|
commands_printf("TS5700N8501 ABM: %d, SF: %s, ALMC: %s", enc_ts5700n8501_get_abm(&encoder_cfg_TS5700N8501), sf, almc);
|
|
|
|
} break;
|
|
|
|
|
|
|
|
case SENSOR_PORT_MODE_SINCOS:
|
2022-02-19 09:51:34 -08:00
|
|
|
commands_printf("Sin/Cos encoder signal below minimum amplitude: errors: %d, error rate: %.3f %%",
|
2022-02-20 04:44:01 -08:00
|
|
|
encoder_cfg_sincos.state.signal_below_min_error_cnt,
|
|
|
|
(double)(encoder_cfg_sincos.state.signal_low_error_rate * 100.0));
|
2022-02-19 09:51:34 -08:00
|
|
|
|
|
|
|
commands_printf("Sin/Cos encoder signal above maximum amplitude: errors: %d, error rate: %.3f %%",
|
2022-02-20 04:44:01 -08:00
|
|
|
encoder_cfg_sincos.state.signal_above_max_error_cnt,
|
|
|
|
(double)(encoder_cfg_sincos.state.signal_above_max_error_rate * 100.0));
|
2022-02-20 07:22:38 -08:00
|
|
|
break;
|
2022-02-19 09:51:34 -08:00
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
case SENSOR_PORT_MODE_AD2S1205:
|
2022-02-19 09:51:34 -08:00
|
|
|
commands_printf("Resolver Loss Of Tracking (>5%c error): errors: %d, error rate: %.3f %%", 0xB0,
|
2022-02-20 04:44:01 -08:00
|
|
|
encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_cnt,
|
|
|
|
(double)(encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_rate * 100.0));
|
2022-02-19 09:51:34 -08:00
|
|
|
commands_printf("Resolver Degradation Of Signal (>33%c error): errors: %d, error rate: %.3f %%", 0xB0,
|
2022-02-20 04:44:01 -08:00
|
|
|
encoder_cfg_ad2s1205.state.resolver_degradation_of_signal_error_cnt,
|
|
|
|
(double)(encoder_cfg_ad2s1205.state.resolver_degradation_of_signal_error_rate * 100.0));
|
2022-02-19 09:51:34 -08:00
|
|
|
commands_printf("Resolver Loss Of Signal (>57%c error): errors: %d, error rate: %.3f %%", 0xB0,
|
2022-02-20 04:44:01 -08:00
|
|
|
encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_cnt,
|
|
|
|
(double)(encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_rate * 100.0));
|
2022-02-20 07:22:38 -08:00
|
|
|
break;
|
|
|
|
|
|
|
|
case SENSOR_PORT_MODE_ABI:
|
|
|
|
commands_printf("Index found: %d", encoder_index_found());
|
|
|
|
break;
|
2022-02-19 09:51:34 -08:00
|
|
|
|
2022-07-18 13:25:03 -07:00
|
|
|
case SENSOR_PORT_MODE_AS5x47U_SPI:
|
|
|
|
commands_printf("SPI AS5x47U encoder value: %d, errors: %d, error rate: %.3f %%",
|
|
|
|
encoder_cfg_as5x47u.state.spi_val, encoder_cfg_as5x47u.state.spi_communication_error_count,
|
|
|
|
(double)(encoder_cfg_as5x47u.state.spi_error_rate * 100.0));
|
|
|
|
|
|
|
|
commands_printf("\nAS5x47U DIAGNOSTICS:\n"
|
|
|
|
"Connected : %u\n"
|
|
|
|
"AGC : %u\n"
|
|
|
|
"Magnitude : %u\n"
|
|
|
|
"COF : %u\n"
|
|
|
|
"Hall_Broken : %u\n"
|
|
|
|
"Error : %u\n"
|
|
|
|
"COMP_low : %u\n"
|
|
|
|
"COMP_high : %u\n"
|
|
|
|
"WatchdogTest: %u\n"
|
|
|
|
"CRC Error : %u\n"
|
|
|
|
"MagHalf : %u\n"
|
|
|
|
"Error Flags : %04X\n"
|
|
|
|
"Diag Flags : %04X\n",
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.is_connected,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.AGC_value,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.magnitude,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.is_COF,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.is_broken_hall,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.is_error,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.is_Comp_low,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.is_Comp_high,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.is_wdtst,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.is_crc_error,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.is_mag_half,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.serial_error_flgs,
|
|
|
|
encoder_cfg_as5x47u.state.sensor_diag.serial_diag_flgs);
|
|
|
|
break;
|
|
|
|
|
2022-10-02 07:33:17 -07:00
|
|
|
case SENSOR_PORT_MODE_BISSC:
|
|
|
|
commands_printf("BissC Loss SPI communication (>4%c error): errors: %d, error rate: %.3f %%", 0xB0,
|
|
|
|
encoder_cfg_bissc.state.spi_comm_error_cnt,
|
|
|
|
(double)(encoder_cfg_bissc.state.spi_comm_error_rate * 100.0));
|
|
|
|
commands_printf("BissC Degradation Of Signal (>5%c error): errors: %d, error rate: %.3f %%", 0xB0,
|
|
|
|
encoder_cfg_bissc.state.spi_data_error_cnt,
|
|
|
|
(double)(encoder_cfg_bissc.state.spi_data_error_rate * 100.0));
|
|
|
|
break;
|
|
|
|
|
2022-02-20 07:22:38 -08:00
|
|
|
default:
|
|
|
|
commands_printf("No encoder debug info available.");
|
|
|
|
break;
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
2022-02-20 07:22:38 -08:00
|
|
|
|
|
|
|
commands_printf(" ");
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
static void terminal_encoder_clear_errors(int argc, const char **argv) {
|
|
|
|
(void)argc; (void)argv;
|
|
|
|
encoder_reset_errors();
|
|
|
|
commands_printf("Done!\n");
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
|
2022-02-19 09:51:34 -08:00
|
|
|
static void terminal_encoder_clear_multiturn(int argc, const char **argv) {
|
|
|
|
(void)argc; (void)argv;
|
|
|
|
encoder_reset_multiturn();
|
|
|
|
commands_printf("Done!\n");
|
2022-01-09 08:10:40 -08:00
|
|
|
}
|
|
|
|
|
2022-02-19 12:00:52 -08:00
|
|
|
static void timer_start(float rate) {
|
|
|
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
|
|
|
|
|
|
|
// 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 / rate) - 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);
|
2022-02-19 15:16:51 -08:00
|
|
|
|
|
|
|
nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
|
2022-02-19 12:00:52 -08:00
|
|
|
}
|