Hello everyone,
I am working on shifting sensored FOC to sensorless FOC for a BLDC motor using STM32F103C8T6. I have successfully implemented all necessary transformations (Clark, Park, and Inverse Park), and the waveforms appear correct.
To estimate the rotor angle, I am using a Sliding Mode Observer (SMO). However, since the STM32F103 lacks an FPU, So function like atan2f() increase execution time. Instead, I implemented an alternate method for angle calculation. I am using position detected by hall sensor as reference, but output of my sensorless algorithm is having some differences from hall sensor signal.
This is output waveform
Here is the code I am using for angle calculation:
void SMOcalcalculation(int32_t Valpa, int32_t Vbeta, int32_t Ialpa, int32_t Ibeta){
// this is for approximated or predicted values of motor phase currents
// this is need to reconstant bemf :The error between the actual and estimated currents (error_alpa and error_beta) is used to compute the Sliding Mode Observer (SMO) error, which is crucial for estimating back-EMF.
//The estimated back-EMF is used for sensorless rotor position estimation in FOC (Field-Oriented Control).
Ialpa_hat += (Valpa - Rs *Ialpa_hat) >> 2;
Ibeta_hat += (Vbeta - Rs *Ibeta_hat) >> 2;
//Error Calculation
error_alpa = Ialpa - Ialpa_hat;
error_beta = Ibeta - Ibeta_hat;
// Apply Low-Pass Filter
error_alpha_filtered = LPF_ALPHA * error_alpha_filtered + (1 - LPF_ALPHA) * error_alpa;
error_beta_filtered = LPF_ALPHA * error_beta_filtered + (1 - LPF_ALPHA) * error_beta;
//Calculated bemf using Sliding Mode
if(Measured.phaseADCValues[0] == 0 && Measured.phaseADCValues[1] == 0){
Measured.foc.error_alpha = 0;
Measured.foc.error_beta = 0;
}else{
// e_alpha[k] = u_alpha[k] - R_s * i_alpha[k] - L_s * (i_alpha[k] - i_alpha[k-1]) / T_s
// e_beta[k] = u_beta[k] - R_s * i_beta[k] - L_s * (i_beta[k] - i_beta[k-1]) / T_s
//Ts sample time 1s 10e6
Measured.foc.error_alpha = ((((Valpa - ((Rs * Ialpa)) - (Ls* (error_alpha_filtered)))) + 9) - 4 ) *100 ;
Measured.foc.error_beta = ((Vbeta - (Rs * Ibeta)) - (Ls *(error_beta_filtered)) + 4) * 100;
}
}
#define FILTER_SIZE 5
int16_t angle_buffer[FILTER_SIZE] = {0};
int index = 0;
// Integer-based atan2 approximation using fixed-point math
int32_t fast_atan2_fixed(int32_t y, int32_t x) {
int32_t abs_y = (y < 0) ? -y : y;
int32_t angle;
if (x >= 0) {
//180/3 = 60 == 14043
//30 == 5461
int32_t r = ((x - abs_y) * 8191) / (x + abs_y + 1);
angle = 8191 - r; // π/4 - r
} else {
int32_t r = ((x + abs_y) *8191) / (abs_y - x + 1);
angle = 16383 - r; // π/2 - r
}
return (y < 0) ? -angle : angle;
}
void CalculateRotorAngle(int32_t ealpha, int32_t ebeta) {
int16_t raw_angle = (int16_t) fast_atan2_fixed(ebeta, ealpha); // Get raw rotor angle
// Apply moving average filter
angle_buffer[index] = raw_angle;
index = (index + 1) % FILTER_SIZE; // Circular buffer index
int32_t sum = 0;
for (int i = 0; i < FILTER_SIZE; i++) {
sum += angle_buffer[i];
}
Measured.foc.rotorAngle =(int16_t) (sum / FILTER_SIZE) ; // Smoothed angle
Measured.foc.rotorAngle += 10000;
}
Thanks in advance!
