diff --git a/.gitignore b/.gitignore index 61ac400..45abd8c 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +*.o obj libopencm3 linker.map diff --git a/include/project/inc_encoder.h b/include/project/inc_encoder.h index 740b9e2..63ab9e3 100644 --- a/include/project/inc_encoder.h +++ b/include/project/inc_encoder.h @@ -34,6 +34,7 @@ class Encoder static uint16_t GetAngleSPI(); static uint16_t GetAngleResolver(); static uint16_t GetAngleSinCos(); + static uint16_t DecodeAngle(); static int GetPulseTimeFiltered(); static void GetMinMaxTime(uint32_t& min, uint32_t& max); static void DMASetup(); diff --git a/include/project/param_prj.h b/include/project/param_prj.h index 65d0a3b..b51a624 100644 --- a/include/project/param_prj.h +++ b/include/project/param_prj.h @@ -66,7 +66,7 @@ #define POTMODE_DUALCHANNEL 1 #define POTMODE_CAN 2 -#define VER 4.10.R +#define VER 4.12.R #define VERSTR STRINGIFY(4=VER) enum _modes diff --git a/src/project/inc_encoder.cpp b/src/project/inc_encoder.cpp index 62561df..e679ede 100644 --- a/src/project/inc_encoder.cpp +++ b/src/project/inc_encoder.cpp @@ -32,6 +32,7 @@ #include "printf.h" #define TWO_PI 65536 +#define TEN_DEGREE ((10 * TWO_PI) / 360) #define MAX_CNT TWO_PI - 1 #define MAX_REVCNT_VALUES 5 @@ -69,11 +70,14 @@ static bool ignore = true; static enum Encoder::mode encMode = Encoder::INVALID; static bool seenNorthSignal = false; static uint32_t turnsSinceLastSample = 0; +static int32_t minSin = 0, maxSin = 0; void Encoder::Reset() { ignore = true; lastPulseTimespan = MAX_CNT; + minSin = 0; + maxSin = 0; for (uint32_t i = 0; i < MAX_REVCNT_VALUES; i++) timdata[i] = MAX_CNT; } @@ -252,7 +256,7 @@ bool Encoder::IsSyncMode() u32fp Encoder::CalcFrequencyFromAngleDifference(uint16_t angle) { - static int samples = 0; + static uint32_t samples = 0; static int lastAngle = 0; u32fp frq = lastFrequency; uint16_t diffPos = (lastAngle - angle) & 0xFFFF; @@ -261,7 +265,7 @@ u32fp Encoder::CalcFrequencyFromAngleDifference(uint16_t angle) samples++; - if (angleDiff > 1820) + if (angleDiff > TEN_DEGREE) { //We don't make assumption about the rotation direction but we //do assume less than 180° in one PWM cycle @@ -270,6 +274,10 @@ u32fp Encoder::CalcFrequencyFromAngleDifference(uint16_t angle) lastAngle = angle; samples = 0; } + else if (samples > (pwmFrq >> 5)) + { + frq = 0; + } frq = IIRFILTER(lastFrequency, frq, 1); return frq; @@ -472,9 +480,7 @@ uint16_t Encoder::GetAngleResolver() else { gpio_set(GPIOD, GPIO2); - int sin = adc_read_injected(ADC1, 2); - int cos = adc_read_injected(ADC1, 3); - angle = SineCore::Atan2(cos, sin); + angle = DecodeAngle(); } return angle; @@ -483,16 +489,32 @@ uint16_t Encoder::GetAngleResolver() /** Calculates angle from a Hall sin/cos encoder like MLX90380 */ uint16_t Encoder::GetAngleSinCos() { - int sin = adc_read_injected(ADC1, 2); - int cos = adc_read_injected(ADC1, 3); - uint16_t calcAngle = SineCore::Atan2(cos, sin); + uint16_t calcAngle = DecodeAngle(); adc_start_conversion_injected(ADC1); + return calcAngle; +} + +uint16_t Encoder::DecodeAngle() +{ + int sin = adc_read_injected(ADC1, 2); + int cos = adc_read_injected(ADC1, 3); //Param::SetInt(Param::sin, sin); //Param::SetInt(Param::cos, cos); - return calcAngle; + minSin = MIN(sin, minSin); + maxSin = MAX(sin, maxSin); + + //Wait for signal to reach usable amplitude + if ((maxSin - minSin) > 1000) + { + return SineCore::Atan2(cos, sin); + } + else + { + return 0; + } } int Encoder::GetPulseTimeFiltered()