mirror of https://github.com/rusefi/bldc.git
Moved state out of ad2s1205
This commit is contained in:
Benjamin Vedder
parent
8bd3057dd7
commit
f40cd090d0
|
|
@ -27,44 +27,16 @@
|
|||
#include "hw.h"
|
||||
#include "mc_interface.h"
|
||||
#include "utils.h"
|
||||
#include <math.h>
|
||||
#include "spi_bb.h"
|
||||
#include "timer.h"
|
||||
|
||||
static AD2S1205_config_t AD2S1205_config_now = { 0 };
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
static uint16_t spi_val = 0;
|
||||
static float resolver_loss_of_tracking_error_rate = 0.0;
|
||||
static float resolver_degradation_of_signal_error_rate = 0.0;
|
||||
static float resolver_loss_of_signal_error_rate = 0.0;
|
||||
static uint32_t resolver_loss_of_tracking_error_cnt = 0;
|
||||
static uint32_t resolver_degradation_of_signal_error_cnt = 0;
|
||||
static uint32_t resolver_loss_of_signal_error_cnt = 0;
|
||||
static uint32_t spi_error_cnt = 0;
|
||||
static float spi_error_rate = 0.0;
|
||||
static float last_enc_angle = 0.0;
|
||||
encoder_ret_t enc_ad2s1205_init(AD2S1205_config_t *cfg) {
|
||||
spi_bb_init(&(cfg->sw_spi));
|
||||
|
||||
void enc_ad2s1205_deinit(void) {
|
||||
spi_bb_deinit(&(AD2S1205_config_now.sw_spi));
|
||||
|
||||
// TODO: (TO BE TESTED!!) DEINITIALIZE ALSO SAMPLE AND RDVEL
|
||||
#if defined(AD2S1205_SAMPLE_GPIO)
|
||||
palSetPadMode(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN, PAL_MODE_INPUT_PULLUP); // Prepare for a falling edge SAMPLE assertion
|
||||
#endif
|
||||
#if defined(AD2S1205_RDVEL_GPIO)
|
||||
palSetPadMode(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN, PAL_MODE_INPUT_PULLUP); // Will always read position
|
||||
#endif
|
||||
}
|
||||
|
||||
encoder_ret_t enc_ad2s1205_init(AD2S1205_config_t *AD2S1205_config) {
|
||||
AD2S1205_config_now = *AD2S1205_config;
|
||||
|
||||
spi_bb_init(&(AD2S1205_config_now.sw_spi));
|
||||
|
||||
resolver_loss_of_tracking_error_rate = 0.0;
|
||||
resolver_degradation_of_signal_error_rate = 0.0;
|
||||
resolver_loss_of_signal_error_rate = 0.0;
|
||||
resolver_loss_of_tracking_error_cnt = 0;
|
||||
resolver_loss_of_signal_error_cnt = 0;
|
||||
memset(&cfg->state, 0, sizeof(AD2S1205_state));
|
||||
|
||||
// TODO: Choose pins on comm port when these are not defined
|
||||
#if defined(AD2S1205_SAMPLE_GPIO)
|
||||
|
|
@ -77,36 +49,46 @@ encoder_ret_t enc_ad2s1205_init(AD2S1205_config_t *AD2S1205_config) {
|
|||
palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN); // Will always read position
|
||||
#endif
|
||||
|
||||
AD2S1205_config_now = *AD2S1205_config;
|
||||
|
||||
nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
|
||||
|
||||
return ENCODER_OK;
|
||||
}
|
||||
|
||||
float enc_ad2s1205_read_deg(void) {
|
||||
return last_enc_angle;
|
||||
void enc_ad2s1205_deinit(AD2S1205_config_t *cfg) {
|
||||
spi_bb_deinit(&(cfg->sw_spi));
|
||||
|
||||
#if defined(AD2S1205_SAMPLE_GPIO)
|
||||
palSetPadMode(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN, PAL_MODE_INPUT_PULLUP); // Prepare for a falling edge SAMPLE assertion
|
||||
#endif
|
||||
#if defined(AD2S1205_RDVEL_GPIO)
|
||||
palSetPadMode(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN, PAL_MODE_INPUT_PULLUP); // Will always read position
|
||||
#endif
|
||||
}
|
||||
|
||||
void enc_ad2s1205_routine(float rate) {
|
||||
void enc_ad2s1205_routine(AD2S1205_config_t *cfg) {
|
||||
uint16_t pos;
|
||||
|
||||
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();
|
||||
|
||||
// SAMPLE signal should have been be asserted in sync with ADC sampling
|
||||
#ifdef AD2S1205_RDVEL_GPIO
|
||||
palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN); // Always read position
|
||||
#endif
|
||||
|
||||
palSetPad(AD2S1205_config_now.sw_spi.sck_gpio, AD2S1205_config_now.sw_spi.sck_pin);
|
||||
palSetPad(cfg->sw_spi.sck_gpio, cfg->sw_spi.sck_pin);
|
||||
|
||||
spi_bb_delay();
|
||||
spi_bb_begin(&(AD2S1205_config_now.sw_spi)); // CS uses the same mcu pin as AS5047
|
||||
spi_bb_begin(&(cfg->sw_spi));
|
||||
spi_bb_delay();
|
||||
|
||||
spi_bb_transfer_16(&(AD2S1205_config_now.sw_spi), &pos, 0, 1);
|
||||
spi_bb_end(&(AD2S1205_config_now.sw_spi));
|
||||
spi_bb_transfer_16(&(cfg->sw_spi), &pos, 0, 1);
|
||||
spi_bb_end(&(cfg->sw_spi));
|
||||
|
||||
spi_val = pos;
|
||||
cfg->state.spi_val = pos;
|
||||
|
||||
uint16_t RDVEL = pos & 0x0008; // 1 means a position read
|
||||
uint16_t RDVEL = pos & 0x0008;
|
||||
|
||||
if ((RDVEL != 0)) {
|
||||
|
||||
|
|
@ -121,11 +103,11 @@ void enc_ad2s1205_routine(float rate) {
|
|||
}
|
||||
|
||||
if (!parity_error) {
|
||||
UTILS_LP_FAST(spi_error_rate, 0.0, 1.0 / rate);
|
||||
UTILS_LP_FAST(cfg->state.spi_error_rate, 0.0, timestep);
|
||||
} else {
|
||||
angle_is_correct = false;
|
||||
++spi_error_cnt;
|
||||
UTILS_LP_FAST(spi_error_rate, 1.0, 1.0 / rate);
|
||||
++cfg->state.spi_error_cnt;
|
||||
UTILS_LP_FAST(cfg->state.spi_error_rate, 1.0, timestep);
|
||||
}
|
||||
|
||||
pos &= 0xFFF0;
|
||||
|
|
@ -134,58 +116,30 @@ void enc_ad2s1205_routine(float rate) {
|
|||
|
||||
if (LOT) {
|
||||
angle_is_correct = false;
|
||||
++resolver_loss_of_tracking_error_cnt;
|
||||
UTILS_LP_FAST(resolver_loss_of_tracking_error_rate, 1.0, 1.0 / rate);
|
||||
++cfg->state.resolver_loss_of_tracking_error_cnt;
|
||||
UTILS_LP_FAST(cfg->state.resolver_loss_of_tracking_error_rate, 1.0, timestep);
|
||||
} else {
|
||||
UTILS_LP_FAST(resolver_loss_of_tracking_error_rate, 0.0, 1.0 / rate);
|
||||
UTILS_LP_FAST(cfg->state.resolver_loss_of_tracking_error_rate, 0.0, timestep);
|
||||
}
|
||||
|
||||
if (DOS) {
|
||||
angle_is_correct = false;
|
||||
++resolver_degradation_of_signal_error_cnt;
|
||||
UTILS_LP_FAST(resolver_degradation_of_signal_error_rate, 1.0, 1.0 / rate);
|
||||
++cfg->state.resolver_degradation_of_signal_error_cnt;
|
||||
UTILS_LP_FAST(cfg->state.resolver_degradation_of_signal_error_rate, 1.0, timestep);
|
||||
} else {
|
||||
UTILS_LP_FAST(resolver_degradation_of_signal_error_rate, 0.0, 1.0 / rate);
|
||||
UTILS_LP_FAST(cfg->state.resolver_degradation_of_signal_error_rate, 0.0, timestep);
|
||||
}
|
||||
|
||||
if (LOS) {
|
||||
angle_is_correct = false;
|
||||
++resolver_loss_of_signal_error_cnt;
|
||||
UTILS_LP_FAST(resolver_loss_of_signal_error_rate, 1.0, 1.0 / rate);
|
||||
++cfg->state.resolver_loss_of_signal_error_cnt;
|
||||
UTILS_LP_FAST(cfg->state.resolver_loss_of_signal_error_rate, 1.0, timestep);
|
||||
} else {
|
||||
UTILS_LP_FAST(resolver_loss_of_signal_error_rate, 0.0, 1.0 / rate);
|
||||
UTILS_LP_FAST(cfg->state.resolver_loss_of_signal_error_rate, 0.0, timestep);
|
||||
}
|
||||
|
||||
if (angle_is_correct) {
|
||||
last_enc_angle = ((float) pos * 360.0) / 4096.0;
|
||||
cfg->state.last_enc_angle = ((float) pos * 360.0) / 4096.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float enc_ad2s1205_resolver_loss_of_tracking_error_rate(void) {
|
||||
return resolver_loss_of_tracking_error_rate;
|
||||
}
|
||||
|
||||
float enc_ad2s1205_resolver_degradation_of_signal_error_rate(void) {
|
||||
return resolver_degradation_of_signal_error_rate;
|
||||
}
|
||||
|
||||
float enc_ad2s1205_resolver_loss_of_signal_error_rate(void) {
|
||||
return resolver_loss_of_signal_error_rate;
|
||||
}
|
||||
|
||||
uint32_t enc_ad2s1205_resolver_loss_of_tracking_error_cnt(void) {
|
||||
return resolver_loss_of_tracking_error_cnt;
|
||||
}
|
||||
|
||||
uint32_t enc_ad2s1205_resolver_degradation_of_signal_error_cnt(void) {
|
||||
return resolver_degradation_of_signal_error_cnt;
|
||||
}
|
||||
|
||||
uint32_t enc_ad2s1205_resolver_loss_of_signal_error_cnt(void) {
|
||||
return resolver_loss_of_signal_error_cnt;
|
||||
}
|
||||
|
||||
uint32_t AD2S1205_spi_get_error_cnt(void) {
|
||||
return spi_error_cnt;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,17 +25,12 @@
|
|||
#include "datatypes.h"
|
||||
#include "encoder/encoder_datatype.h"
|
||||
|
||||
void enc_ad2s1205_deinit(void);
|
||||
// Functions
|
||||
encoder_ret_t enc_ad2s1205_init(AD2S1205_config_t *AD2S1205_config);
|
||||
void enc_ad2s1205_deinit(AD2S1205_config_t *cfg);
|
||||
void enc_ad2s1205_routine(AD2S1205_config_t *cfg);
|
||||
|
||||
float enc_ad2s1205_read_deg(void);
|
||||
void enc_ad2s1205_routine(float rate);
|
||||
|
||||
float enc_ad2s1205_resolver_loss_of_tracking_error_rate(void);
|
||||
float enc_ad2s1205_resolver_degradation_of_signal_error_rate(void);
|
||||
float enc_ad2s1205_resolver_loss_of_signal_error_rate(void);
|
||||
uint32_t enc_ad2s1205_resolver_loss_of_tracking_error_cnt(void);
|
||||
uint32_t enc_ad2s1205_resolver_degradation_of_signal_error_cnt(void);
|
||||
uint32_t enc_ad2s1205_resolver_loss_of_signal_error_cnt(void);
|
||||
// Macros
|
||||
#define AD2S1205_LAST_ANGLE(cfg) ((cfg)->state.last_enc_angle)
|
||||
|
||||
#endif /* ENC_AD2S1205_H_ */
|
||||
|
|
|
|||
|
|
@ -74,6 +74,7 @@ AD2S1205_config_t ad2s1205_cfg = {
|
|||
0, // has_error
|
||||
CH_MUTEX_INIT_ZERO
|
||||
},
|
||||
{0},
|
||||
};
|
||||
|
||||
MT6816_config_t mt6816_cfg = {
|
||||
|
|
@ -152,7 +153,7 @@ void encoder_deinit(void) {
|
|||
} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
||||
enc_mt6816_deinit(&mt6816_cfg);
|
||||
} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
|
||||
enc_ad2s1205_deinit();
|
||||
enc_ad2s1205_deinit(&ad2s1205_cfg);
|
||||
} else if (encoder_type_now == ENCODER_TYPE_ABI) {
|
||||
enc_abi_deinit();
|
||||
} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
|
||||
|
|
@ -323,7 +324,7 @@ float encoder_read_deg(void) {
|
|||
} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
||||
return MT6816_LAST_ANGLE(&mt6816_cfg);
|
||||
} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
|
||||
return enc_ad2s1205_read_deg();
|
||||
return AD2S1205_LAST_ANGLE(&ad2s1205_cfg);
|
||||
} else if (encoder_type_now == ENCODER_TYPE_ABI) {
|
||||
return enc_abi_read_deg();
|
||||
} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
|
||||
|
|
@ -373,35 +374,31 @@ void encoder_reset_errors(void) {
|
|||
void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_motor) {
|
||||
// Trigger encoder error rate fault, using 5% errors as threshold.
|
||||
// Relevant only in FOC mode with encoder enabled
|
||||
if(m_conf->motor_type == MOTOR_TYPE_FOC &&
|
||||
|
||||
bool is_foc_encoder = m_conf->motor_type == MOTOR_TYPE_FOC &&
|
||||
m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
|
||||
mcpwm_foc_is_using_encoder();
|
||||
|
||||
if (is_foc_encoder &&
|
||||
m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI &&
|
||||
mcpwm_foc_is_using_encoder() &&
|
||||
as504x_cfg.state.spi_error_rate > 0.05) {
|
||||
mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
|
||||
}
|
||||
|
||||
if(m_conf->motor_type == MOTOR_TYPE_FOC &&
|
||||
m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
|
||||
if (is_foc_encoder &&
|
||||
m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_MT6816_SPI &&
|
||||
mcpwm_foc_is_using_encoder() &&
|
||||
mt6816_cfg.state.encoder_no_magnet_error_rate > 0.05) {
|
||||
mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false);
|
||||
}
|
||||
|
||||
if(m_conf->motor_type == MOTOR_TYPE_FOC &&
|
||||
m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
|
||||
m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
|
||||
|
||||
if (is_foc_encoder && m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
|
||||
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 (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);
|
||||
}
|
||||
|
||||
if (m_conf->motor_type == MOTOR_TYPE_FOC &&
|
||||
m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
|
||||
m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI) {
|
||||
if (is_foc_encoder && m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI) {
|
||||
AS504x_diag diag = as504x_cfg.state.sensor_diag;
|
||||
|
||||
if (!diag.is_connected) {
|
||||
|
|
@ -415,17 +412,18 @@ void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_moto
|
|||
}
|
||||
}
|
||||
|
||||
if(m_conf->motor_type == MOTOR_TYPE_FOC &&
|
||||
m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
|
||||
m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {
|
||||
if (enc_ad2s1205_resolver_loss_of_tracking_error_rate() > 0.05)
|
||||
if (is_foc_encoder && m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {
|
||||
if (ad2s1205_cfg.state.resolver_loss_of_tracking_error_rate > 0.05) {
|
||||
mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOT, is_second_motor, false);
|
||||
if (enc_ad2s1205_resolver_degradation_of_signal_error_rate() > 0.05)
|
||||
}
|
||||
if (ad2s1205_cfg.state.resolver_degradation_of_signal_error_rate > 0.05) {
|
||||
mc_interface_fault_stop(FAULT_CODE_RESOLVER_DOS, is_second_motor, false);
|
||||
if (enc_ad2s1205_resolver_loss_of_signal_error_rate() > 0.04)
|
||||
}
|
||||
if (ad2s1205_cfg.state.resolver_loss_of_signal_error_rate > 0.04) {
|
||||
mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOS, is_second_motor, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void encoder_pin_isr(void) {
|
||||
// Only reset if the pin is still high to avoid too short pulses, which
|
||||
|
|
@ -465,7 +463,7 @@ void encoder_tim_isr(void) {
|
|||
} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
||||
enc_mt6816_routine(&mt6816_cfg);
|
||||
} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
|
||||
enc_ad2s1205_routine(timer_rate_now);
|
||||
enc_ad2s1205_routine(&ad2s1205_cfg);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -550,14 +548,14 @@ static void terminal_encoder(int argc, const char **argv) {
|
|||
|
||||
if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {
|
||||
commands_printf("Resolver Loss Of Tracking (>5%c error): errors: %d, error rate: %.3f %%", 0xB0,
|
||||
enc_ad2s1205_resolver_loss_of_tracking_error_cnt(),
|
||||
(double)(enc_ad2s1205_resolver_loss_of_tracking_error_rate() * 100.0));
|
||||
ad2s1205_cfg.state.resolver_loss_of_signal_error_cnt,
|
||||
(double)(ad2s1205_cfg.state.resolver_loss_of_signal_error_rate * 100.0));
|
||||
commands_printf("Resolver Degradation Of Signal (>33%c error): errors: %d, error rate: %.3f %%", 0xB0,
|
||||
enc_ad2s1205_resolver_degradation_of_signal_error_cnt(),
|
||||
(double)(enc_ad2s1205_resolver_degradation_of_signal_error_rate() * 100.0));
|
||||
ad2s1205_cfg.state.resolver_degradation_of_signal_error_cnt,
|
||||
(double)(ad2s1205_cfg.state.resolver_degradation_of_signal_error_rate * 100.0));
|
||||
commands_printf("Resolver Loss Of Signal (>57%c error): errors: %d, error rate: %.3f %%", 0xB0,
|
||||
enc_ad2s1205_resolver_loss_of_signal_error_cnt(),
|
||||
(double)(enc_ad2s1205_resolver_loss_of_signal_error_rate() * 100.0));
|
||||
ad2s1205_cfg.state.resolver_loss_of_signal_error_cnt,
|
||||
(double)(ad2s1205_cfg.state.resolver_loss_of_signal_error_rate * 100.0));
|
||||
}
|
||||
|
||||
if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_ABI) {
|
||||
|
|
|
|||
|
|
@ -41,8 +41,23 @@ typedef enum {
|
|||
ENCODER_TYPE_ABI
|
||||
} encoder_type_t;
|
||||
|
||||
typedef struct {
|
||||
uint16_t spi_val;
|
||||
float resolver_loss_of_tracking_error_rate;
|
||||
float resolver_degradation_of_signal_error_rate;
|
||||
float resolver_loss_of_signal_error_rate;
|
||||
uint32_t resolver_loss_of_tracking_error_cnt;
|
||||
uint32_t resolver_degradation_of_signal_error_cnt;
|
||||
uint32_t resolver_loss_of_signal_error_cnt;
|
||||
uint32_t spi_error_cnt;
|
||||
float spi_error_rate;
|
||||
float last_enc_angle;
|
||||
uint32_t last_update_time;
|
||||
} AD2S1205_state;
|
||||
|
||||
typedef struct {
|
||||
spi_bb_state sw_spi;
|
||||
AD2S1205_state state;
|
||||
} AD2S1205_config_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
|
|||
Loading…
Reference in New Issue