|
@@ -302,9 +302,10 @@ float encoder_zero_phase_detect(float *enc_off) {
|
|
|
total_ph += phase;
|
|
total_ph += phase;
|
|
|
total_enc_off += g_encoder.pwm_count;
|
|
total_enc_off += g_encoder.pwm_count;
|
|
|
}
|
|
}
|
|
|
- float offset_now = total_ph/(float)count;
|
|
|
|
|
|
|
+ sys_debug("count = %d\n", count);
|
|
|
|
|
+ float offset_now = total_ph/(float)(count + 1);
|
|
|
g_encoder.enc_offset = offset_now;
|
|
g_encoder.enc_offset = offset_now;
|
|
|
- g_encoder.enc_count_off = (u32)(total_enc_off/(float)count);
|
|
|
|
|
|
|
+ g_encoder.enc_count_off = (u32)(total_enc_off/(float)(count + 1));
|
|
|
if (enc_off) {
|
|
if (enc_off) {
|
|
|
*enc_off = (float)g_encoder.enc_count_off;
|
|
*enc_off = (float)g_encoder.enc_count_off;
|
|
|
sys_debug("encoder off %f\n", *enc_off);
|
|
sys_debug("encoder off %f\n", *enc_off);
|