rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sincos cleanup

This commit is contained in:
Benjamin Vedder 2022-02-20 12:12:40 +01:00
parent 77e75f4898
commit a446b355af
5 changed files with 69 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
encoder/enc_sincos.c
View File

@ -26,82 +26,56 @@
#include "stm32f4xx_conf.h"
#include "mc_interface.h"
#include "utils.h"
#include <math.h>
#include "hw.h"
#include "timer.h"
#include <math.h>
#include <string.h>
#define SINCOS_SAMPLE_RATE_HZ 20000
#define SINCOS_MIN_AMPLITUDE 1.0 // sqrt(sin^2 + cos^2) has to be larger than this
#define SINCOS_MAX_AMPLITUDE 1.65 // sqrt(sin^2 + cos^2) has to be smaller than this
ENCSINCOS_config_t enc_sincos_config_now = { 0 };
static uint32_t sincos_signal_below_min_error_cnt = 0;
static uint32_t sincos_signal_above_max_error_cnt = 0;
static float sincos_signal_low_error_rate = 0.0;
static float sincos_signal_above_max_error_rate = 0.0;
static float last_enc_angle = 0.0;
void enc_sincos_deinit(void) {
last_enc_angle = 0.0;
sincos_signal_low_error_rate = 0.0;
sincos_signal_above_max_error_rate = 0.0;
}
encoder_ret_t enc_sincos_init(ENCSINCOS_config_t *enc_sincos_config) {
enc_sincos_config_now = *enc_sincos_config;
sincos_signal_below_min_error_cnt = 0;
sincos_signal_above_max_error_cnt = 0;
sincos_signal_low_error_rate = 0.0;
sincos_signal_above_max_error_rate = 0.0;
last_enc_angle = 0.0;
enc_sincos_config_now = *enc_sincos_config;
encoder_ret_t enc_sincos_init(ENCSINCOS_config_t *cfg) {
memset(&cfg->state, 0, sizeof(ENCSINCOS_state));
return ENCODER_OK;
}
float enc_sincos_read_deg(void) {
void enc_sincos_deinit(ENCSINCOS_config_t *cfg) {
memset(&cfg->state, 0, sizeof(ENCSINCOS_state));
}
float enc_sincos_read_deg(ENCSINCOS_config_t *cfg) {
float angle = 0.0;
float sin = ENCODER_SIN_VOLTS * enc_sincos_config_now.s_gain - enc_sincos_config_now.s_offset;
float cos = ENCODER_COS_VOLTS * enc_sincos_config_now.c_gain - enc_sincos_config_now.c_offset;
float sin = ENCODER_SIN_VOLTS * cfg->s_gain - cfg->s_offset;
float cos = ENCODER_COS_VOLTS * cfg->c_gain - cfg->c_offset;
float module = SQ(sin) + SQ(cos);
float timestep = timer_seconds_elapsed_since(cfg->state.last_update_time);
if (timestep > 1.0) {
timestep = 1.0;
}
cfg->state.last_update_time = timer_time_now();
if (module > SQ(SINCOS_MAX_AMPLITUDE) ) {
// signals vector outside of the valid area. Increase error count and discard measurement
++sincos_signal_above_max_error_cnt;
UTILS_LP_FAST(sincos_signal_above_max_error_rate, 1.0, 1./SINCOS_SAMPLE_RATE_HZ);
angle = last_enc_angle;
++cfg->state.signal_above_max_error_cnt;
UTILS_LP_FAST(cfg->state.signal_above_max_error_rate, 1.0, timestep);
angle = cfg->state.last_enc_angle;
} else {
if (module < SQ(SINCOS_MIN_AMPLITUDE)) {
++sincos_signal_below_min_error_cnt;
UTILS_LP_FAST(sincos_signal_low_error_rate, 1.0, 1./SINCOS_SAMPLE_RATE_HZ);
angle = last_enc_angle;
++cfg->state.signal_below_min_error_cnt;
UTILS_LP_FAST(cfg->state.signal_low_error_rate, 1.0, timestep);
angle = cfg->state.last_enc_angle;
} else {
UTILS_LP_FAST(sincos_signal_above_max_error_rate, 0.0, 1./SINCOS_SAMPLE_RATE_HZ);
UTILS_LP_FAST(sincos_signal_low_error_rate, 0.0, 1./SINCOS_SAMPLE_RATE_HZ);
UTILS_LP_FAST(cfg->state.signal_above_max_error_rate, 0.0, timestep);
UTILS_LP_FAST(cfg->state.signal_low_error_rate, 0.0, timestep);
float angle_tmp = RAD2DEG_f(utils_fast_atan2(sin, cos));
UTILS_LP_FAST(angle, angle_tmp, enc_sincos_config_now.filter_constant);
last_enc_angle = angle;
UTILS_LP_FAST(angle, angle_tmp, cfg->filter_constant);
cfg->state.last_enc_angle = angle;
}
}
return last_enc_angle;
}
uint32_t enc_sincos_get_signal_below_min_error_cnt(void) {
return sincos_signal_below_min_error_cnt;
}
uint32_t enc_sincos_get_signal_above_max_error_cnt(void) {
return sincos_signal_above_max_error_cnt;
}
float enc_sincos_get_signal_below_min_error_rate(void) {
return sincos_signal_low_error_rate;
}
float enc_sincos_get_signal_above_max_error_rate(void) {
return sincos_signal_above_max_error_rate;
return angle;
}

13
encoder/enc_sincos.h
View File

@ -25,14 +25,9 @@
#include "datatypes.h"
#include "encoder/encoder_datatype.h"
void enc_sincos_deinit(void);
encoder_ret_t enc_sincos_init(ENCSINCOS_config_t *enc_sincos_config);
float enc_sincos_read_deg(void);
uint32_t enc_sincos_get_signal_below_min_error_cnt(void);
uint32_t enc_sincos_get_signal_above_max_error_cnt(void);
float enc_sincos_get_signal_below_min_error_rate(void);
float enc_sincos_get_signal_above_max_error_rate(void);
// Functions
encoder_ret_t enc_sincos_init(ENCSINCOS_config_t *cfg);
void enc_sincos_deinit(ENCSINCOS_config_t *cfg);
float enc_sincos_read_deg(ENCSINCOS_config_t *cfg);
#endif /* ENC_SINCOS_H_ */

30
encoder/encoder.c
View File

@ -108,7 +108,7 @@ ABI_config_t encoder_conf_ABI =
HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3
};
ENCSINCOS_config_t encoder_conf_ENCSINCOS = {0};
ENCSINCOS_config_t sincos_cfg = {0};
TS5700N8501_config_t encoder_conf_TS5700N8501 = {
#if defined(HW_ADC_EXT_GPIO) && defined(HW_ADC_EXT_GPIO)
@ -156,7 +156,7 @@ void encoder_deinit(void) {
} else if (encoder_type_now == ENCODER_TYPE_ABI) {
enc_abi_deinit();
} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
enc_sincos_deinit();
enc_sincos_deinit(&sincos_cfg);
} else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
enc_ts5700n8501_deinit();
}
@ -246,13 +246,13 @@ encoder_ret_t encoder_init(volatile mc_configuration *conf) {
case SENSOR_PORT_MODE_SINCOS: {
SENSOR_PORT_5V();
encoder_conf_ENCSINCOS.s_gain = conf->foc_encoder_sin_gain;
encoder_conf_ENCSINCOS.s_offset = conf->foc_encoder_sin_offset;
encoder_conf_ENCSINCOS.c_gain = conf->foc_encoder_cos_gain;
encoder_conf_ENCSINCOS.c_offset = conf->foc_encoder_cos_offset;
encoder_conf_ENCSINCOS.filter_constant = conf->foc_encoder_sincos_filter_constant;
sincos_cfg.s_gain = conf->foc_encoder_sin_gain;
sincos_cfg.s_offset = conf->foc_encoder_sin_offset;
sincos_cfg.c_gain = conf->foc_encoder_cos_gain;
sincos_cfg.c_offset = conf->foc_encoder_cos_offset;
sincos_cfg.filter_constant = conf->foc_encoder_sincos_filter_constant;
encoder_ret_t encoder_ret = enc_sincos_init(&encoder_conf_ENCSINCOS);
encoder_ret_t encoder_ret = enc_sincos_init(&sincos_cfg);
if (ENCODER_OK != encoder_ret) {
encoder_type_now = ENCODER_TYPE_NONE;
@ -327,7 +327,7 @@ float encoder_read_deg(void) {
} else if (encoder_type_now == ENCODER_TYPE_ABI) {
return enc_abi_read_deg();
} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
return enc_sincos_read_deg();
return enc_sincos_read_deg(&sincos_cfg);
} else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
return enc_ts5700n8501_read_deg();
}
@ -393,9 +393,9 @@ void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_moto
m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
if (enc_sincos_get_signal_below_min_error_rate() > 0.05)
if (sincos_cfg.state.signal_low_error_rate > 0.05)
mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, is_second_motor, false);
if (enc_sincos_get_signal_above_max_error_rate() > 0.05)
if (sincos_cfg.state.signal_above_max_error_rate > 0.05)
mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, is_second_motor, false);
}
@ -540,12 +540,12 @@ static void terminal_encoder(int argc, const char **argv) {
if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
commands_printf("Sin/Cos encoder signal below minimum amplitude: errors: %d, error rate: %.3f %%",
enc_sincos_get_signal_below_min_error_cnt(),
(double)(enc_sincos_get_signal_below_min_error_rate() * 100.0));
sincos_cfg.state.signal_below_min_error_cnt,
(double)(sincos_cfg.state.signal_low_error_rate * 100.0));
commands_printf("Sin/Cos encoder signal above maximum amplitude: errors: %d, error rate: %.3f %%",
enc_sincos_get_signal_above_max_error_cnt(),
(double)(enc_sincos_get_signal_above_max_error_rate() * 100.0));
sincos_cfg.state.signal_above_max_error_cnt,
(double)(sincos_cfg.state.signal_above_max_error_rate * 100.0));
}
if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {

11
encoder/encoder_datatype.h
View File

@ -79,6 +79,15 @@ typedef struct {
uint8_t I_pin;
} ABI_config_t;
typedef struct {
uint32_t signal_below_min_error_cnt;
uint32_t signal_above_max_error_cnt;
float signal_low_error_rate;
float signal_above_max_error_rate;
float last_enc_angle;
uint32_t last_update_time;
} ENCSINCOS_state;
typedef struct {
uint32_t refresh_rate_hz;
float s_gain;
@ -86,6 +95,8 @@ typedef struct {
float c_gain;
float c_offset;
float filter_constant;
ENCSINCOS_state state;
} ENCSINCOS_config_t;
typedef struct {

9
hwconf/hw.h
View File

@ -283,11 +283,20 @@
// Sin/Cos Encoder Signals. Override if available
#ifndef ENCODER_SIN_VOLTS
#if defined(ADC_IND_EXT) && defined(ADC_VOLTS)
#define ENCODER_SIN_VOLTS ADC_VOLTS(ADC_IND_EXT)
#else
#define ENCODER_SIN_VOLTS 0.0
#endif
#endif
#ifndef ENCODER_COS_VOLTS
#if defined(ADC_IND_EXT2) && defined(ADC_VOLTS)
#define ENCODER_COS_VOLTS ADC_VOLTS(ADC_IND_EXT2)
#else
#define ENCODER_COS_VOLTS 0.0
#endif
#endif
// Current ADC macros. Override them for custom current measurement functions.
#ifndef GET_CURRENT1