mirror of https://github.com/rusefi/bldc.git
SINCOS added
This commit is contained in:
parent
d0acc03f86
commit
6507a47bf1
1
Makefile
1
Makefile
|
@ -175,6 +175,7 @@ CSRC = $(STARTUPSRC) \
|
|||
encoder/MT6816.c \
|
||||
encoder/AD2S1205.c \
|
||||
encoder/ABI.c \
|
||||
encoder/ENC_SINCOS.c \
|
||||
encoder/encoder_hwconf.c
|
||||
|
||||
# C++ sources that can be compiled in ARM or THUMB mode depending on the global
|
||||
|
|
|
@ -0,0 +1,91 @@
|
|||
/*
|
||||
* ENC_SINCOS.c
|
||||
*
|
||||
* Created on: Dec 15, 2021
|
||||
* Author: jtuxv
|
||||
*/
|
||||
#include "encoder/ENC_SINCOS.h"
|
||||
|
||||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
#include "stm32f4xx_conf.h"
|
||||
#include "mc_interface.h"
|
||||
#include "utils.h"
|
||||
#include <math.h>
|
||||
|
||||
ENC_SINCOS_config_t enc_sincos_config_now = { 0 };
|
||||
|
||||
static float sin_gain = 0.0;
|
||||
static float sin_offset = 0.0;
|
||||
static float cos_gain = 0.0;
|
||||
static float cos_offset = 0.0;
|
||||
static float sincos_filter_constant = 0.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;
|
||||
enc_sincos_config_now.is_init = 0;
|
||||
}
|
||||
|
||||
encoders_ret_t ENC_SINCOS_init(ENC_SINCOS_config_t *enc_sincos_config) {
|
||||
//ADC inputs are already initialized in hw_init_gpio()
|
||||
sin_gain = enc_sincos_config->s_gain;
|
||||
sin_offset = enc_sincos_config->s_offset;
|
||||
cos_gain = enc_sincos_config->c_gain;
|
||||
cos_offset = enc_sincos_config->c_offset;
|
||||
sincos_filter_constant = enc_sincos_config->filter_constant;
|
||||
|
||||
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;
|
||||
|
||||
// ADC measurements needs to be in sync with motor PWM
|
||||
#ifdef HW_HAS_SIN_COS_ENCODER
|
||||
enc_sincos_config->is_init = 1;
|
||||
return ENCODERS_OK;
|
||||
#else
|
||||
enc_sincos_config->is_init = 0;
|
||||
return ENCODERS_ERROR;
|
||||
#endif
|
||||
}
|
||||
|
||||
float ENC_SINCOS_read_deg(void) {
|
||||
#ifdef HW_HAS_SIN_COS_ENCODER
|
||||
float angle;
|
||||
float sin = ENCODER_SIN_VOLTS * sin_gain - sin_offset;
|
||||
float cos = ENCODER_COS_VOLTS * cos_gain - cos_offset;
|
||||
|
||||
float module = SQ(sin) + SQ(cos);
|
||||
|
||||
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;
|
||||
} 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;
|
||||
} 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);
|
||||
|
||||
float angle_tmp = RAD2DEG_f(utils_fast_atan2(sin, cos));
|
||||
UTILS_LP_FAST(angle, angle_tmp, sincos_filter_constant);
|
||||
last_enc_angle = angle;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return last_enc_angle;
|
||||
}
|
|
@ -0,0 +1,19 @@
|
|||
/*
|
||||
* ENC_SINCOS.h
|
||||
*
|
||||
* Created on: Dec 15, 2021
|
||||
* Author: jtuxv
|
||||
*/
|
||||
|
||||
#ifndef ENCODER_ENC_SINCOS_H_
|
||||
#define ENCODER_ENC_SINCOS_H_
|
||||
|
||||
#include "datatypes.h"
|
||||
#include "encoder/encoder_datatype.h"
|
||||
|
||||
void ENC_SINCOS_deinit(void);
|
||||
encoders_ret_t ENC_SINCOS_init(ENC_SINCOS_config_t *enc_sincos_config);
|
||||
|
||||
float ENC_SINCOS_read_deg(void);
|
||||
|
||||
#endif /* ENCODER_ENC_SINCOS_H_ */
|
|
@ -64,12 +64,27 @@ typedef struct {
|
|||
encoders_incremental_config_t incremental_config;
|
||||
} ABI_config_t;
|
||||
|
||||
typedef struct {
|
||||
bool is_init;
|
||||
encoders_refresh_rate_hz_t refresh_rate_hz;
|
||||
float s_gain;
|
||||
float s_offset;
|
||||
float c_gain;
|
||||
float c_offset;
|
||||
float filter_constant;
|
||||
} ENC_SINCOS_config_t;
|
||||
|
||||
typedef struct {
|
||||
encoders_type_t encoder_type;
|
||||
encoders_refresh_rate_hz_t refresh_rate_hz;
|
||||
encoders_spi_config_t spi_config;
|
||||
uint32_t counts; // FOR INCREMENTAL INTERFACE
|
||||
encoders_incremental_config_t incremental_config; // FOR INCREMENTAL INTERFACE
|
||||
float s_gain;
|
||||
float s_offset;
|
||||
float c_gain;
|
||||
float c_offset;
|
||||
float filter_constant;
|
||||
} encoders_config_t;
|
||||
|
||||
#endif /* ENCODER_ENCODER_DATATYPE_H_ */
|
||||
|
|
|
@ -24,7 +24,12 @@ AS5047_SAMPLE_RATE_HZ,
|
|||
{/*SCK*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 }
|
||||
},
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_INCREMENTAL_UNUSED
|
||||
ENCODERS_INCREMENTAL_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED
|
||||
};
|
||||
|
||||
encoders_config_t conf_MT6816 = { ENCODERS_TYPE_MT6816,
|
||||
|
@ -36,7 +41,12 @@ MT6816_SAMPLE_RATE_HZ,
|
|||
{/*SCK*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 }
|
||||
},
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_INCREMENTAL_UNUSED
|
||||
ENCODERS_INCREMENTAL_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED
|
||||
};
|
||||
|
||||
encoders_config_t conf_AD2S1205 = { ENCODERS_TYPE_AD2S1205_SPI,
|
||||
|
@ -48,7 +58,12 @@ AD2S1205_SAMPLE_RATE_HZ,
|
|||
{/*SCK*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 }
|
||||
},
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_INCREMENTAL_UNUSED
|
||||
ENCODERS_INCREMENTAL_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED
|
||||
};
|
||||
|
||||
encoders_config_t conf_ABI = { ENCODERS_TYPE_ABI,
|
||||
|
@ -58,6 +73,23 @@ ENCODERS_ABI_COUNTER_DEFAULT_VALUE,
|
|||
{/*INCREMENTAL PROTOCOL*/
|
||||
{/*A*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 },
|
||||
{/*B*/HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2 }
|
||||
}
|
||||
},
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED
|
||||
};
|
||||
|
||||
encoders_config_t conf_SINCOS = { ENCODERS_TYPE_SINCOS,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_SPI_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_INCREMENTAL_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED,
|
||||
ENCODERS_CONFIG_UNUSED
|
||||
};
|
||||
|
||||
|
|
|
@ -3,6 +3,7 @@
|
|||
#define ENCODER_ENCODER_HWCONF_H_
|
||||
|
||||
#include "encoder/encoder_datatype.h"
|
||||
#include "hw.h"
|
||||
|
||||
#define AS5047_SAMPLE_RATE_HZ 20000
|
||||
#define AD2S1205_SAMPLE_RATE_HZ 20000 //25MHz max spi clk
|
||||
|
|
|
@ -16,6 +16,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
|
|||
AS504x_config_t as504x_config;
|
||||
encoders_ret_t encoder_ret;
|
||||
|
||||
as504x_config.is_init = 0;
|
||||
as504x_config.spi_config = encoder_config->spi_config;
|
||||
as504x_config.refresh_rate_hz = encoder_config->refresh_rate_hz;
|
||||
encoder_ret = AS504x_init(&as504x_config);
|
||||
|
@ -32,6 +33,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
|
|||
MT6816_config_t mt6816_config;
|
||||
encoders_ret_t encoder_ret;
|
||||
|
||||
mt6816_config.is_init = 0;
|
||||
mt6816_config.spi_config = encoder_config->spi_config;
|
||||
mt6816_config.refresh_rate_hz = encoder_config->refresh_rate_hz;
|
||||
|
||||
|
@ -49,6 +51,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
|
|||
AD2S1205_config_t AD2S1205_config;
|
||||
encoders_ret_t encoder_ret;
|
||||
|
||||
AD2S1205_config.is_init = 0;
|
||||
AD2S1205_config.spi_config = encoder_config->spi_config;
|
||||
AD2S1205_config.refresh_rate_hz = encoder_config->refresh_rate_hz;
|
||||
|
||||
|
@ -66,6 +69,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
|
|||
ABI_config_t abi_config;
|
||||
encoders_ret_t encoder_ret;
|
||||
|
||||
abi_config.is_init = 0;
|
||||
abi_config.incremental_config = encoder_config->incremental_config;
|
||||
abi_config.counts = encoder_config->counts;
|
||||
|
||||
|
@ -79,6 +83,28 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
|
|||
encoder_type_now = ENCODERS_TYPE_ABI;
|
||||
index_found = true;
|
||||
return ENCODERS_OK;
|
||||
} else if (encoder_type_now == ENCODERS_TYPE_SINCOS) {
|
||||
ENC_SINCOS_config_t enc_sincos_config;
|
||||
encoders_ret_t encoder_ret;
|
||||
|
||||
enc_sincos_config.is_init = 0;
|
||||
enc_sincos_config.s_gain = encoder_config->s_gain;
|
||||
enc_sincos_config.s_offset = encoder_config->s_offset;
|
||||
enc_sincos_config.c_gain = encoder_config->c_gain;
|
||||
enc_sincos_config.s_offset = encoder_config->s_offset;
|
||||
enc_sincos_config.filter_constant = encoder_config->filter_constant;
|
||||
enc_sincos_config.refresh_rate_hz = encoder_config->refresh_rate_hz;
|
||||
|
||||
encoder_ret = ENC_SINCOS_init(&enc_sincos_config);
|
||||
|
||||
if (ENCODERS_OK != encoder_ret || !enc_sincos_config.is_init) {
|
||||
encoder_type_now = ENCODERS_TYPE_SINCOS;
|
||||
index_found = false;
|
||||
return ENCODERS_ERROR;
|
||||
}
|
||||
encoder_type_now = ENCODERS_TYPE_SINCOS;
|
||||
index_found = true;
|
||||
return ENCODERS_OK;
|
||||
} else {
|
||||
encoder_type_now = ENCODERS_TYPE_NONE;
|
||||
index_found = false;
|
||||
|
@ -95,6 +121,8 @@ float encoders_read_deg(void) {
|
|||
return AD2S1205_read_deg();
|
||||
} else if (encoder_type_now == ENCODERS_TYPE_ABI) {
|
||||
return ABI_read_deg();
|
||||
} else if (encoder_type_now == ENCODERS_TYPE_SINCOS) {
|
||||
return ENC_SINCOS_read_deg();
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
|
@ -116,7 +144,11 @@ void encoders_deinit(void) {
|
|||
AD2S1205_deinit();
|
||||
} else if (encoder_type_now == ENCODERS_TYPE_ABI) {
|
||||
ABI_deinit();
|
||||
} else if (encoder_type_now == ENCODERS_TYPE_SINCOS) {
|
||||
ENC_SINCOS_deinit();
|
||||
}
|
||||
|
||||
encoder_type_now = ENCODERS_TYPE_NONE;
|
||||
}
|
||||
|
||||
float encoders_spi_get_error_rate(void) {
|
||||
|
@ -217,9 +249,9 @@ void encoders_tim_isr(void) {
|
|||
if (encoder_type_now == ENCODERS_TYPE_AS504x) {
|
||||
AS504x_routine();
|
||||
} else if (encoder_type_now == ENCODERS_TYPE_MT6816) {
|
||||
return MT6816_routine();
|
||||
MT6816_routine();
|
||||
} else if (encoder_type_now == ENCODERS_TYPE_AD2S1205_SPI) {
|
||||
return AD2S1205_routine();
|
||||
AD2S1205_routine();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
|
||||
#ifndef ENCODER_ENCODERS_H_
|
||||
#define ENCODER_ENCODERS_H_
|
||||
|
||||
|
@ -8,6 +7,7 @@
|
|||
#include "encoder/AD2S1205.h"
|
||||
#include "encoder/encoder_hwconf.h"
|
||||
#include "encoder/ABI.h"
|
||||
#include "encoder/ENC_SINCOS.h"
|
||||
#include "hal.h"
|
||||
// GENERIC GLOBAL
|
||||
void encoders_deinit(void);
|
||||
|
|
Loading…
Reference in New Issue