diff --git a/include/project/inc_encoder.h b/include/project/inc_encoder.h index fabbacf..d910241 100644 --- a/include/project/inc_encoder.h +++ b/include/project/inc_encoder.h @@ -9,14 +9,14 @@ class Encoder public: enum mode { - SINGLE, AB, ABZ, SPI, RESOLVER, INVALID + SINGLE, AB, ABZ, SPI, RESOLVER, SINCOS, INVALID }; static void Reset(); static void SetMode(enum mode encMode); static bool SeenNorthSignal(); static void UpdateRotorAngle(int dir); - static void UpdateRotorFrequency(); + static void UpdateRotorFrequency(int callingFrequency); static void SetPwmFrequency(uint32_t frq); static uint16_t GetRotorAngle(); static uint32_t GetSpeed(); diff --git a/include/project/param_prj.h b/include/project/param_prj.h index 6d4d5f7..0dd9246 100644 --- a/include/project/param_prj.h +++ b/include/project/param_prj.h @@ -26,11 +26,12 @@ #define SNS_M "12=KTY83-110, 13=KTY84-130, 14=Leaf" #define PWMFUNCS "0=tmpm, 1=tmphs, 2=speed, 3=speedfrq" #define BTNSWITCH "0=Button, 1=Switch" +#define DIRMODES "0=Button, 1=Switch, 2=ButtonReversed, 3=SwitchReversed" #define IDLEMODS "0=always, 1=nobrake, 2=cruise" #define ONOFF "0=Off, 1=On, 2=na" #define OKERR "0=Error, 1=Ok, 2=na" #define CHARGEMODS "0=Off, 3=Boost, 4=Buck" -#define ENCMODES "0=Single, 1=AB, 2=ABZ, 3=SPI, 4=Resolver" +#define ENCMODES "0=Single, 1=AB, 2=ABZ, 3=SPI, 4=Resolver, 5=SinCos" #define POTMODES "0=SingleRegen, 1=DualChannel, 2=CAN" #define CANSPEEDS "0=250k, 1=500k, 2=800k, 3=1M" #define CANIOS "1=Cruise, 2=Start, 4=Brake, 8=Fwd, 16=Rev, 32=Bms" @@ -48,7 +49,7 @@ #define CAT_CHARGER "Charger" #define CAT_COMM "Communication" -#define VER 4.78 +#define VER 4.87 #define VERCEIL VER + 0.009 enum _modes @@ -70,6 +71,13 @@ enum _tripmodes TRIP_PRECHARGEON }; +enum _dirmodes +{ + DIR_BUTTON = 0, + DIR_SWITCH = 1, + DIR_REVERSED = 2, //used as a flag +}; + enum _canio { CAN_IO_CRUISE = 1, @@ -113,14 +121,12 @@ enum _canio PARAM_ENTRY(CAT_MOTOR, fslipmax, "Hz", 0, 10, 3, 33 ) \ PARAM_ENTRY(CAT_MOTOR, polepairs, "", 1, 16, 2, 32 ) \ PARAM_ENTRY(CAT_MOTOR, respolepairs,"", 1, 16, 1, 93 ) \ - PARAM_ENTRY(CAT_MOTOR, enckp, "", 0, 40, 20, 6 ) \ - PARAM_ENTRY(CAT_MOTOR, encki, "", 0, 16000, 400, 94 ) \ - PARAM_ENTRY(CAT_MOTOR, encmode, ENCMODES, 0, 4, 0, 75 ) \ + PARAM_ENTRY(CAT_MOTOR, encmode, ENCMODES, 0, 5, 0, 75 ) \ PARAM_ENTRY(CAT_MOTOR, fmin, "Hz", 0, 400, 1, 34 ) \ PARAM_ENTRY(CAT_MOTOR, fmax, "Hz", 0, 1000, 200, 9 ) \ PARAM_ENTRY(CAT_MOTOR, numimp, "ppr", 8, 8192, 60, 15 ) \ PARAM_ENTRY(CAT_MOTOR, dirchrpm, "rpm", 0, 2000, 100, 87 ) \ - PARAM_ENTRY(CAT_MOTOR, dirmode, BTNSWITCH, 0, 1, 1, 95 ) \ + PARAM_ENTRY(CAT_MOTOR, dirmode, DIRMODES, 0, 3, 1, 95 ) \ PARAM_ENTRY(CAT_MOTOR, syncofs, "dig", 0, 65535, 0, 70 ) \ PARAM_ENTRY(CAT_MOTOR, snsm, SNS_M, 12, 14, 12, 46 ) \ PARAM_ENTRY(CAT_INVERTER,pwmfrq, PWMFRQS, 0, 4, 1, 13 ) \ diff --git a/src/project/inc_encoder.cpp b/src/project/inc_encoder.cpp index 7b425e1..e1130f7 100644 --- a/src/project/inc_encoder.cpp +++ b/src/project/inc_encoder.cpp @@ -37,9 +37,11 @@ static void InitTimerSingleChannelMode(); static void InitTimerABZMode(); static void InitSPIMode(); static void InitResolverMode(); +static u32fp TrackFrequency(uint16_t angle); static void DMASetup(); static uint16_t GetAngleSPI(); static uint16_t GetAngleResolver(); +static uint16_t GetAngleSinCos(); static int GetPulseTimeFiltered(); static void GetMinMaxTime(uint32_t& min, uint32_t& max); @@ -56,6 +58,7 @@ static u32fp lastFrequency = 0; static bool ignore = true; static enum Encoder::mode encMode = Encoder::INVALID; static bool seenNorthSignal = false; +static uint32_t turnsSinceLastSample = 0; void Encoder::Reset() { @@ -92,6 +95,7 @@ void Encoder::SetMode(Encoder::mode mode) InitSPIMode(); break; case RESOLVER: + case SINCOS: InitResolverMode(); break; default: @@ -133,6 +137,7 @@ void Encoder::UpdateRotorAngle(int dir) int16_t numPulses; uint32_t timeSinceLastPulse; uint16_t interpolatedAngle; + uint16_t angleDiff; switch (encMode) { @@ -142,6 +147,9 @@ void Encoder::UpdateRotorAngle(int dir) cntVal *= TWO_PI; cntVal /= pulsesPerTurn * 4; angle = (uint16_t)cntVal; + angleDiff = ((TIM_CR1(REV_CNT_TIMER) & TIM_CR1_DIR_DOWN) ? + (lastAngle - angle) : (angle - lastAngle)) & 0xFFFF; + turnsSinceLastSample += angleDiff; break; case SINGLE: numPulses = GetPulseTimeFiltered(); @@ -151,11 +159,16 @@ void Encoder::UpdateRotorAngle(int dir) angle = accumulatedAngle + dir * interpolatedAngle; break; case SPI: - angle = GetAngleSPI(); //Gets 12-bit - angle <<= 4; + angle = GetAngleSPI(); + lastFrequency = TrackFrequency(angle); break; case RESOLVER: angle = GetAngleResolver(); + lastFrequency = TrackFrequency(angle); + break; + case SINCOS: + angle = GetAngleSinCos(); + lastFrequency = TrackFrequency(angle); break; default: break; @@ -163,32 +176,31 @@ void Encoder::UpdateRotorAngle(int dir) if (lastAngle <= (TWO_PI / 2) && angle > (TWO_PI / 2)) { - poleCounter--; if (poleCounter == 0) { fullTurns++; poleCounter = Param::GetInt(Param::respolepairs); } + else + { + poleCounter--; + } } lastAngle = angle; } /** Update rotor frequency. - * @ref https://www.embeddedrelated.com/showarticle/530.php + * @param callingFrequency Frequency at which this function is called in Hz */ -void Encoder::UpdateRotorFrequency() +void Encoder::UpdateRotorFrequency(int callingFrequency) { - static int velest = 0, velint = 0; - static uint16_t posest = 0; - const int kp = Param::GetInt(Param::enckp); - const int ki = Param::GetInt(Param::encki); - - posest += velest / (int)pwmFrq; - int16_t poserr = angle - posest; - velint += (poserr * ki) / (int)pwmFrq; - velest = poserr * kp + velint; - lastFrequency = ABS(velint) / FP_TOINT(TWO_PI); + if ((encMode == AB) || (encMode == ABZ)) + { + //65536 is one turn + lastFrequency = (callingFrequency * turnsSinceLastSample) / FP_TOINT(TWO_PI); + turnsSinceLastSample = 0; + } } /** Returns current angle of motor shaft to some arbitrary 0-axis @@ -223,7 +235,7 @@ uint32_t Encoder::GetSpeed() if (ignore) return 0; return 60000000 / (lastPulseTimespan * pulsesPerTurn); } - else if (encMode == RESOLVER) + else if (encMode == RESOLVER || encMode == SINCOS) { return FP_TOINT(60 * lastFrequency) / Param::GetInt(Param::respolepairs); } @@ -241,7 +253,24 @@ uint32_t Encoder::GetFullTurns() bool Encoder::IsSyncMode() { - return encMode == Encoder::ABZ || encMode == Encoder::SPI || encMode == Encoder::RESOLVER; + return encMode == ABZ || encMode == SPI || encMode == RESOLVER || encMode == SINCOS; +} + +/** Track rotor frequency. + * @ref https://www.embeddedrelated.com/showarticle/530.php + */ +static u32fp TrackFrequency(uint16_t angle) +{ + static int velest = 0, velint = 0; + static uint16_t posest = 0; + const int kp = 20; + const int ki = 400; + + posest += velest / (int)pwmFrq; + int16_t poserr = angle - posest; + velint += (poserr * ki) / (int)pwmFrq; + velest = poserr * kp + velint; + return ABS(velint) / FP_TOINT(TWO_PI); } static void DMASetup() @@ -289,7 +318,7 @@ static void InitTimerSingleChannelMode() timer_generate_event(REV_CNT_TIMER, TIM_EGR_UG); timer_enable_counter(REV_CNT_TIMER); gpio_set_mode(GPIOD, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO2); - gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO7); + gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO6 | GPIO7); DMASetup(); exti_disable_request(EXTI2); } @@ -300,17 +329,18 @@ static void InitSPIMode() exti_disable_request(EXTI2); gpio_set_mode(GPIOD, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO2); gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO7); + gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO6); seenNorthSignal = true; } static void InitTimerABZMode() { rcc_periph_reset_pulse(REV_CNT_TIMRST); - timer_set_period(REV_CNT_TIMER, 4 * pulsesPerTurn); //2 channels and 2 edges -> 4 times the number of base pulses + timer_set_period(REV_CNT_TIMER, 4 * pulsesPerTurn); //2 channels and 2 edges -> 4 times the number of base pulses - //In sync mode start out in reset mode and switch to encoder - //mode once the north marker has been detected - if (encMode == Encoder::ABZ) + //In sync mode start out in reset mode and switch to encoder + //mode once the north marker has been detected + if (encMode == Encoder::ABZ) { exti_select_source(EXTI2, GPIOD); exti_set_trigger(EXTI2, EXTI_TRIGGER_RISING); @@ -322,15 +352,15 @@ static void InitTimerABZMode() } timer_slave_set_mode(REV_CNT_TIMER, TIM_SMCR_SMS_EM3); // encoder mode - timer_ic_set_input(REV_CNT_TIMER, TIM_IC1, TIM_IC_IN_TI1); - timer_ic_set_input(REV_CNT_TIMER, TIM_IC2, TIM_IC_IN_TI2); - timer_ic_set_filter(REV_CNT_TIMER, TIM_IC1, TIM_IC_DTF_DIV_32_N_8); - timer_ic_set_filter(REV_CNT_TIMER, TIM_IC2, TIM_IC_DTF_DIV_32_N_8); + timer_ic_set_input(REV_CNT_TIMER, TIM_IC1, TIM_IC_IN_TI1); + timer_ic_set_input(REV_CNT_TIMER, TIM_IC2, TIM_IC_IN_TI2); + timer_ic_set_filter(REV_CNT_TIMER, TIM_IC1, TIM_IC_DTF_DIV_32_N_8); + timer_ic_set_filter(REV_CNT_TIMER, TIM_IC2, TIM_IC_DTF_DIV_32_N_8); timer_ic_enable(REV_CNT_TIMER, TIM_IC1); timer_ic_enable(REV_CNT_TIMER, TIM_IC2); - timer_enable_counter(REV_CNT_TIMER); + timer_enable_counter(REV_CNT_TIMER); gpio_set_mode(GPIOD, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO2); - gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO7); + gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO6 | GPIO7); seenNorthSignal = false; } @@ -342,33 +372,44 @@ static void InitResolverMode() adc_enable_external_trigger_injected(ADC1, ADC_CR2_JEXTSEL_JSWSTART); adc_set_sample_time(ADC1, 6, ADC_SMPR_SMP_1DOT5CYC); adc_set_sample_time(ADC1, 7, ADC_SMPR_SMP_1DOT5CYC); - rcc_periph_reset_pulse(REV_CNT_TIMRST); - timer_set_prescaler(REV_CNT_TIMER, 71); //run at 1MHz - timer_set_period(REV_CNT_TIMER, 65535); - timer_one_shot_mode(REV_CNT_TIMER); - timer_set_oc_mode(REV_CNT_TIMER, TIM_OC4, TIM_OCM_PWM2); - timer_enable_oc_output(REV_CNT_TIMER, TIM_OC4); //OC4 is connected to ADC trigger - timer_enable_preload(REV_CNT_TIMER); - timer_direction_up(REV_CNT_TIMER); - timer_generate_event(REV_CNT_TIMER, TIM_EGR_UG); - gpio_set_mode(GPIOD, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO2); - gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, GPIO6); - gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, GPIO7); + gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, GPIO6 | GPIO7); exti_disable_request(EXTI2); - adc_start_conversion_injected(ADC1); + if (encMode == Encoder::RESOLVER) + { + rcc_periph_reset_pulse(REV_CNT_TIMRST); + timer_set_prescaler(REV_CNT_TIMER, 71); //run at 1MHz + timer_set_period(REV_CNT_TIMER, 65535); + timer_one_shot_mode(REV_CNT_TIMER); + timer_set_oc_mode(REV_CNT_TIMER, TIM_OC4, TIM_OCM_PWM2); + timer_enable_oc_output(REV_CNT_TIMER, TIM_OC4); //OC4 is connected to ADC trigger + timer_enable_preload(REV_CNT_TIMER); + timer_direction_up(REV_CNT_TIMER); + timer_generate_event(REV_CNT_TIMER, TIM_EGR_UG); + gpio_set_mode(GPIOD, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO2); - while (!adc_eoc_injected(ADC1)); + adc_start_conversion_injected(ADC1); //Determine offset + + while (!adc_eoc_injected(ADC1)); + + int sin = adc_read_injected(ADC1, 2); + int cos = adc_read_injected(ADC1, 3); + adc_set_injected_offset(ADC1, 2, sin); + adc_set_injected_offset(ADC1, 3, cos); + adc_enable_external_trigger_injected(ADC1, ADC_CR2_JEXTSEL_TIM3_CC4); + } + else //SINCOS + { + //Offset assumed 3.3V/2 + adc_set_injected_offset(ADC1, 2, 2048); + adc_set_injected_offset(ADC1, 3, 2048); + } - int sin = adc_read_injected(ADC1, 2); - int cos = adc_read_injected(ADC1, 3); - adc_set_injected_offset(ADC1, 2, sin); - adc_set_injected_offset(ADC1, 3, cos); - adc_enable_external_trigger_injected(ADC1, ADC_CR2_JEXTSEL_TIM3_CC4); seenNorthSignal = true; } +/** Gets angle from an AD2S chip */ static uint16_t GetAngleSPI() { uint32_t d = 0; @@ -385,10 +426,12 @@ static uint16_t GetAngleSPI() GPIO_BSRR(GPIOD) |= GPIO2; //Read high d >>= 10; //6 because of GPIO6, 4 because of encoder format //Encoder format is ANGLE[11:0], RDVEL, Parity, DOS, LOT - return d; + return d << 4; //we want 16-bit representation } /** Calculates current angle and velocity from resolver feedback + * and generates square wave that is filtered + * into a sine wave for resolver excitation * * For different PWM frequencies you need to populate different * resistor values on the 3-pole filter. @@ -407,7 +450,6 @@ static uint16_t GetAngleResolver() timer_set_oc_value(REV_CNT_TIMER, TIM_OC4, resolverSampleDelay); timer_set_counter(REV_CNT_TIMER, 0); timer_enable_counter(REV_CNT_TIMER); - state = false; } else { @@ -415,17 +457,30 @@ static uint16_t GetAngleResolver() int sin = adc_read_injected(ADC1, 2); int cos = adc_read_injected(ADC1, 3); angle = SineCore::Atan2(cos, sin); - state = true; } + state = !state; + return angle; } +/** Calculates angle from a Hall sin/cos encoder like MLX90380 */ +static uint16_t GetAngleSinCos() +{ + int sin = adc_read_injected(ADC1, 2); + int cos = adc_read_injected(ADC1, 3); + uint16_t calcAngle = SineCore::Atan2(cos, sin); + + adc_start_conversion_injected(ADC1); + + return calcAngle; +} + extern "C" void exti2_isr(void) { - exti_reset_request(EXTI2); + exti_reset_request(EXTI2); timer_set_counter(REV_CNT_TIMER, 0); - seenNorthSignal = true; + seenNorthSignal = true; } static int GetPulseTimeFiltered() diff --git a/src/project/pwmgeneration.cpp b/src/project/pwmgeneration.cpp index 26707a5..88fb1b3 100644 --- a/src/project/pwmgeneration.cpp +++ b/src/project/pwmgeneration.cpp @@ -32,6 +32,7 @@ #define SHIFT_180DEG (uint16_t)32768 #define SHIFT_90DEG (uint16_t)16384 #define FRQ_TO_ANGLE(frq) FP_TOINT((frq << SineCore::BITS) / pwmfrq) +#define DIGIT_TO_DEGREE(a) FP_FROMINT(angle) / (65536 / 360) static uint8_t pwmdigits; static uint16_t pwmfrq; @@ -151,7 +152,6 @@ extern "C" void pwm_timer_isr(void) s32fp ampNomLimited = LimitCurrent(); Encoder::UpdateRotorAngle(dir); - Encoder::UpdateRotorFrequency(); if (opmode == MOD_SINE) CalcNextAngleConstant(dir); @@ -165,7 +165,7 @@ extern "C" void pwm_timer_isr(void) SineCore::SetAmp(amp); Param::SetInt(Param::amp, amp); Param::SetFlt(Param::fstat, frq); - Param::SetFlt(Param::angle, FP_FROMINT(angle) / 182); + Param::SetFlt(Param::angle, DIGIT_TO_DEGREE(angle)); SineCore::Calc(angle); /* Match to PWM resolution */ diff --git a/src/project/stm32_can.cpp b/src/project/stm32_can.cpp index e0053c5..8e7ae96 100644 --- a/src/project/stm32_can.cpp +++ b/src/project/stm32_can.cpp @@ -240,26 +240,7 @@ void Init(enum baudrates baudrate) // Reset CAN can_reset(CAN1); - // CAN cell init. - // Setting the bitrate to 250KBit. APB1 = 36MHz, - // prescaler = 9 -> 4MHz time quanta frequency. - // 1tq sync + 9tq bit segment1 (TS1) + 6tq bit segment2 (TS2) = - // 16time quanto per bit period, therefor 4MHz/16 = 250kHz - // - can_init(CAN1, - false, // TTCM: Time triggered comm mode? - true, // ABOM: Automatic bus-off management? - false, // AWUM: Automatic wakeup mode? - true, // NART: No automatic retransmission? - false, // RFLM: Receive FIFO locked mode? - false, // TXFP: Transmit FIFO priority? - CAN_BTR_SJW_1TQ, - canSpeed[baudrate].ts1, - canSpeed[baudrate].ts2, - canSpeed[baudrate].prescaler, // BRP+1: Baud rate prescaler - false, - false); - + SetBaudrate(baudrate); ConfigureFilters(); // Enable CAN RX interrupt. can_enable_irq(CAN1, CAN_IER_FMPIE0); @@ -267,6 +248,12 @@ void Init(enum baudrates baudrate) void SetBaudrate(enum baudrates baudrate) { + // CAN cell init. + // Setting the bitrate to 250KBit. APB1 = 36MHz, + // prescaler = 9 -> 4MHz time quanta frequency. + // 1tq sync + 9tq bit segment1 (TS1) + 6tq bit segment2 (TS2) = + // 16time quanto per bit period, therefor 4MHz/16 = 250kHz + // can_init(CAN1, false, // TTCM: Time triggered comm mode? true, // ABOM: Automatic bus-off management? diff --git a/src/project/stm32_sine.cpp b/src/project/stm32_sine.cpp index 0684659..1d38ba1 100644 --- a/src/project/stm32_sine.cpp +++ b/src/project/stm32_sine.cpp @@ -95,16 +95,17 @@ static void SelectDirection() { int selectedDir = Param::GetInt(Param::dir); int userDirSelection = 0; + int dirSign = (Param::GetInt(Param::dirmode) & DIR_REVERSED) ? -1 : 1; - if (Param::GetInt(Param::dirmode) == BUTTON) + if ((Param::GetInt(Param::dirmode) & 1) == DIR_BUTTON) { /* if forward AND reverse selected, force neutral, because it's charge mode */ if (Param::GetBool(Param::din_forward) && Param::GetBool(Param::din_reverse)) selectedDir = 0; else if (Param::GetBool(Param::din_forward)) - userDirSelection = 1; + userDirSelection = 1 * dirSign; else if (Param::GetBool(Param::din_reverse)) - userDirSelection = -1; + userDirSelection = -1 * dirSign; else userDirSelection = selectedDir; } @@ -114,9 +115,9 @@ static void SelectDirection() if (!(Param::GetBool(Param::din_forward) ^ Param::GetBool(Param::din_reverse))) selectedDir = 0; else if (Param::GetBool(Param::din_forward)) - userDirSelection = 1; + userDirSelection = 1 * dirSign; else if (Param::GetBool(Param::din_reverse)) - userDirSelection = -1; + userDirSelection = -1 * dirSign; } /* Only change direction when below certain motor speed */ @@ -625,6 +626,7 @@ static void Ms10Task(void) int newMode = MOD_OFF; s32fp udc = ProcessUdc(); + Encoder::UpdateRotorFrequency(100); GetDigInputs(); ProcessThrottle(); CalcAndOutputTemp(); @@ -789,7 +791,6 @@ extern void parm_Change(Param::PARAM_NUM paramNum) { PwmGeneration::SetCurrentLimitThreshold(Param::Get(Param::ocurlim)); - Encoder::SetFilterConst(Param::GetInt(Param::enckp)); Encoder::SetMode((enum Encoder::mode)Param::GetInt(Param::encmode)); Encoder::SetImpulsesPerTurn(Param::GetInt(Param::numimp));