bldc/encoder/enc_sincos.c

82 lines
2.6 KiB
C

/*
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/>.
*/
#include "enc_sincos.h"
#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "mc_interface.h"
#include "utils_math.h"
#include "hw.h"
#include "timer.h"
#include <math.h>
#include <string.h>
#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
bool enc_sincos_init(ENCSINCOS_config_t *cfg) {
memset(&cfg->state, 0, sizeof(ENCSINCOS_state));
return true;
}
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 * 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
++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)) {
++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(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, cfg->filter_constant);
cfg->state.last_enc_angle = angle;
}
}
return angle;
}