2022-02-19 06:37:12 -08:00
|
|
|
/*
|
|
|
|
Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se
|
|
|
|
Copyright 2022 Marcos Chaparro mchaparro@powerdesigns.ca
|
|
|
|
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/>.
|
|
|
|
*/
|
2021-12-15 07:48:49 -08:00
|
|
|
|
2021-12-15 07:26:36 -08:00
|
|
|
#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>
|
2022-01-20 12:23:37 -08:00
|
|
|
#include "hw.h"
|
2022-01-02 08:40:30 -08:00
|
|
|
|
|
|
|
//TODO move defines to encoder_hwconf.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
|
2021-12-15 07:26:36 -08:00
|
|
|
|
2021-12-17 05:05:39 -08:00
|
|
|
ENCSINCOS_config_t enc_sincos_config_now = { 0 };
|
2021-12-15 07:26:36 -08:00
|
|
|
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_ret_t ENC_SINCOS_init(ENCSINCOS_config_t *enc_sincos_config) {
|
2022-01-20 12:23:37 -08:00
|
|
|
enc_sincos_config_now = *enc_sincos_config;
|
2021-12-15 07:26:36 -08:00
|
|
|
|
|
|
|
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->is_init = 1;
|
2021-12-20 05:18:51 -08:00
|
|
|
enc_sincos_config_now = *enc_sincos_config;
|
2022-01-20 12:23:37 -08:00
|
|
|
return ENCODER_OK;
|
2021-12-15 07:26:36 -08:00
|
|
|
}
|
2022-02-19 10:40:42 -08:00
|
|
|
|
2021-12-15 07:26:36 -08:00
|
|
|
float ENC_SINCOS_read_deg(void) {
|
2022-01-20 12:23:37 -08:00
|
|
|
float angle = 0.0;
|
2022-02-19 10:40:42 -08:00
|
|
|
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;
|
2021-12-15 07:26:36 -08:00
|
|
|
|
|
|
|
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));
|
2022-01-20 12:23:37 -08:00
|
|
|
UTILS_LP_FAST(angle, angle_tmp, enc_sincos_config_now.filter_constant);
|
2021-12-15 07:26:36 -08:00
|
|
|
last_enc_angle = angle;
|
|
|
|
}
|
|
|
|
}
|
2022-02-19 10:40:42 -08:00
|
|
|
|
2021-12-15 07:26:36 -08:00
|
|
|
return last_enc_angle;
|
|
|
|
}
|
2021-12-20 05:18:51 -08:00
|
|
|
|
|
|
|
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;
|
|
|
|
}
|