/**
* Takes the average of a number of angles.
*
* @param angles
* The angles in radians.
*
* @param angles_num
* The number of angles.
*
* @param weights
* The weight of the summarized angles
*
* @return
* The average angle.
*/
float utils_avg_angles_rad_fast(float *angles, float *weights, int angles_num) {
float s_sum = 0.0;
float c_sum = 0.0;
for (int i = 0; i < angles_num; i++) {
float s, c;
utils_fast_sincos_better(angles[i], &s, &c);
s_sum += s * weights[i];
c_sum += c * weights[i];
}
return utils_fast_atan2(s_sum, c_sum);
}
/**
* Read angle from configured encoder.
*
* @return
* The current encoder angle in degrees.
*/
float encoder_read_deg(void) {
static float angle = 0.0;
switch (mode) {
case ENCODER_MODE_ABI:
angle = ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
break;
case ENCODER_MODE_AS5047P_SPI:
case RESOLVER_MODE_AD2S1205:
angle = last_enc_angle;
break;
#ifdef HW_HAS_SIN_COS_ENCODER
case ENCODER_MODE_SINCOS: {
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 = utils_fast_atan2(sin, cos) * 180.0 / M_PI;
UTILS_LP_FAST(angle, angle_tmp, sincos_filter_constant);
last_enc_angle = angle;
}
}
break;
}
#endif
default:
break;
}
return angle;
}