SINCOS added

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

encoder/ENC_SINCOS.c

@@ -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;
+}

encoder/ENC_SINCOS.h

@@ -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_ */

encoder/encoder_datatype.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_ */

encoder/encoder_hwconf.c

@@ -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
 };
 

encoder/encoder_hwconf.h

@@ -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

encoder/encoders.c

@@ -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();
 	}
 }
 

encoder/encoders.h

@@ -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);