main.ino

Go to the documentation of this file.

/* This file has been prepared for Doxygen automatic documentation generation.*/
 
// Include motor control related headers
#include "config.h"
#include "tables.h"
#include "fault.h"
#include "filter.h"
#include "scpi.h"
 
// Include PID control algorithm if closed-loop speed control is enabled
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
#include "pid.h"
#endif

volatile motorflags_t motorFlags FAST_ACCESS(0x4A);

volatile faultflags_t faultFlags FAST_ACCESS(0x3E);

volatile motorconfigs_t motorConfigs;

volatile uint16_t commutationTicks = 0;
volatile uint16_t lastCommutationTicks = 0xffff;

volatile uint8_t speedInput = 0;

volatile uint8_t speedOutput = 0;

volatile uint16_t ibus = 0;

volatile int16_t iphaseU = 0;
volatile int16_t iphaseV = 0;

volatile int16_t iphaseW = 0;

volatile uint16_t vbusVref = 0;
 
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
pidData_t pidParameters;
#endif

void setup(void)
{
  // Load motor configs
  ConfigsInit();
 
  // Initialize flags.
  FlagsInit();
 
  // Check if remote mode requested.
  RemoteUpdate();
 
  // Initialize peripherals.
  PortsInit(); // depends on motorFlags.remote
  ADCInit();   // include self-test + loop until board detected must be before TimersInit
  PLLInit();
  TimersInit();
 
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
  PIDInit(PID_K_P, PID_K_I, PID_K_D, &pidParameters);
#endif
 
  if (motorFlags.remote == TRUE)
  {
    // Start serial interface with 115200 bauds.
    Serial.begin(115200);
    // while (!Serial); // wait for serial to finish initializing
 
    // Initialise SCPI subsystem if remote mode.
    ScpiInit();
  }
  else
  {
    // Update direction before enable flag.
    DesiredDirectionUpdate();
    // Do not update enable flag until everything is ready.
    EnableUpdate();
  }
 
  // Set up pin change interrupts.
  PinChangeIntInit();
 
  // Enable Timer4 overflow event interrupt.
  TIMSK4 |= (1 << TOIE4);
 
  // Enable interrupts globally and let motor driver take over.
  sei();
}

void loop()
{
  if (motorFlags.remote == TRUE)
  {
    ScpiInput(Serial);
  }
  if (motorFlags.speedControllerRun)
  {
    SpeedController();
    motorFlags.speedControllerRun = FALSE;
  }
}

static void FlagsInit(void)
{
  // Initialize motorFlags with default values.
  motorFlags.speedControllerRun = FALSE;
  motorFlags.remote = FALSE;
  motorFlags.enable = FALSE;
  motorFlags.actualDirection = DIRECTION_UNKNOWN;
  motorFlags.desiredDirection = DIRECTION_FORWARD;
  motorFlags.driveWaveform = WAVEFORM_UNDEFINED;
  motorFlags.fatalFault = FALSE;
 
  // Initialize faultFlags with default values. Set motorStopped to FALSE at
  // startup. This will make sure that the motor is not started if it is not
  // really stopped. If it is stopped, this variable will quickly be updated.
  faultFlags.motorStopped = FALSE;
  faultFlags.reverseDirection = FALSE;
  faultFlags.overCurrent = FALSE;
  faultFlags.noHallConnections = FALSE;
  faultFlags.userFlag1 = FALSE;
  faultFlags.userFlag2 = FALSE;
  faultFlags.userFlag3 = FALSE;
}

static void ConfigsInit(void)
{
  motorConfigs.tim4Freq = (uint32_t)F_MOSFET;
  motorConfigs.tim4Top = (uint16_t)TIM4_TOP(motorConfigs.tim4Freq);
  motorConfigs.tim4DeadTime = (uint16_t)DEAD_TIME;
  motorConfigs.speedInputSource = (uint8_t)SPEED_INPUT_SOURCE_LOCAL;
}

static void PLLInit(void)
{
  // Configure PLL Frequency Control Register to output 48MHz for USB Module and
  // 64MHz for the High-Speed Clock Timer with a base speed of 96MHz for PLL
  // Output Frequency.
 
  PLLFRQ = (0 << PINMUX) | (1 << PLLUSB) | PLL_POSTSCALER_DIV_1_5 | (1 << PDIV3) | (0 << PDIV2) | (1 << PDIV1) | (0 << PDIV0);
 
  // Enable PLL.
  PLLCSR = (1 << PINDIV) | (1 << PLLE);
 
  // Wait until PLOCK bit is set, indicating PLL is locked and stable.
  while ((PLLCSR & (1 << PLOCK)) == 0)
  {
    // Wait for PLL lock
    ;
  }
}

static void PortsInit(void)
{
#if (EMULATE_HALL == TRUE)
  // Configure and set hall sensor pins for motor emulation
  PORTB &= ~((1 << H1_PIN) | (1 << H2_PIN) | (1 << H3_PIN));
  PORTB |= (0x07 & pgm_read_byte_near(&expectedHallSequenceForward[1]));
  // Set hall sensor pins as outputs.
  DDRB |= (1 << H1_PIN) | (1 << H2_PIN) | (1 << H3_PIN);
#endif
 
#if ((HALL_PULLUP_ENABLE == TRUE) && (EMULATE_HALL != TRUE))
  // Configure and set hall sensor pins as input with pull-ups enabled
  PORTB |= (1 << H1_PIN) | (1 << H2_PIN) | (1 << H3_PIN);
#endif
 
  // Configure and set pins FAULT_PIN_3, FAULT_PIN_2, and FAULT_PIN_1 as outputs
  PORTB &= ~((1 << FAULT_PIN_3) | (1 << FAULT_PIN_2));
  DDRB |= (1 << FAULT_PIN_3) | (1 << FAULT_PIN_2);
  PORTD &= ~(1 << FAULT_PIN_1);
  DDRD |= (1 << FAULT_PIN_1);
 
  // If remote mode, set enable and direction pin as output to allow software
  // triggered interrupts
  if (motorFlags.remote == TRUE)
  {
    PORTD &= ~((1 << ENABLE_PIN) | (1 << DIRECTION_COMMAND_PIN));
    DDRD |= (1 << ENABLE_PIN) | (1 << DIRECTION_COMMAND_PIN);
  }
}

void TimersInit(void)
{
  // Set Timer1 accordingly.
  TCCR1A = (1 << WGM11) | (0 << WGM10);
  TCCR1B = (0 << WGM13) | (1 << WGM12);
  TIMSK1 = (1 << TOIE1);
 
  // Start Timer1.
  TCCR1B |= TIM1_CLOCK_DIV_64;
 
#if (EMULATE_HALL == TRUE)
  // Set Timer3 accordingly.
  TCCR3A = (1 << WGM31) | (1 << WGM30);
  TCCR3B = (1 << WGM33) | (1 << WGM32);
  TIMSK3 = (1 << TOIE3);
 
  // Set top value of Timer/counter3.
  OCR3AH = (uint8_t)(TIM3_TOP >> 8);
  OCR3AL = (uint8_t)(0xff & TIM3_TOP);
 
  // Start Timer3.
  TCCR3B |= TIM1_CLOCK_DIV_8;
#endif
  // Set Timer4 in "PWM6 / Dual-slope" mode. Does not enable outputs yet.
  TCCR4A = (0 << COM4A1) | (1 << COM4A0) | (0 << COM4B1) | (1 << COM4B0) | (1 << PWM4A) | (1 << PWM4B);
  TCCR4B = DT_PRESCALER_DIV_PATTERN(CHOOSE_DT_PRESCALER(motorConfigs.tim4DeadTime));
  TCCR4C |= (0 << COM4D1) | (1 << COM4D0) | (1 << PWM4D);
  TCCR4E = (1 << ENHC4);
 
  // Set top value of Timer/counter4.
  TC4H = (uint8_t)(motorConfigs.tim4Top >> 8);
  OCR4C = (uint8_t)(0xff & motorConfigs.tim4Top);
 
  // Set the dead time.
  DT4 = (DEAD_TIME_HALF(motorConfigs.tim4DeadTime) << 4) | DEAD_TIME_HALF(motorConfigs.tim4DeadTime);
 
  // Start Timer4.
  TCCR4B |= TIM4_PRESCALER_DIV_PATTERN(CHOOSE_TIM4_PRESCALER(F_MOSFET));
}

static void PinChangeIntInit(void)
{
  // Initialize external interrupt on shutdown pin (INT0) and direction input
  // (INT2) pin.
  EICRA = (0 << ISC21) | (1 << ISC20) | (0 << ISC01) | (1 << ISC00);
  EIMSK = (1 << INT2) | (1 << INT0);
 
  // Initialize pin change interrupt on hall sensor inputs (PCINT1..3).
  PCMSK0 = (1 << PCINT3) | (1 << PCINT2) | (1 << PCINT1);
 
  // Enable pin change interrupt on ports with pin change signals
  PCICR = (1 << PCIE0);
}

static void ADCInit(void)
{
#if (WAIT_FOR_BOARD == TRUE)
  // Select initial AD conversion channel [IBUS] to check if board is connected.
  ADMUX = (ADC_REFERENCE_VOLTAGE | (1 << ADLAR) | ADC_MUX_L_IBUS);
  ADCSRB = ADC_MUX_H_IBUS;
  _delay_ms(1);
 
  // Enable ADC
  ADCSRA = (1 << ADEN);
 
  // Start ADC single conversion and discard first measurement.
  uint16_t adc_reading = ADCSingleConversion();
 
  // Enable pull up resistor
  PORTF |= (1 << IBUS_PIN);
 
  _delay_ms(10);
  SweepLEDsBlocking();
 
  // Start ADC single conversion to measure BREF.
  adc_reading = ADCSingleConversion();
 
  // Wait to check if any board is connected. Should be less than
  // 0x3C0 if the inverter board is connected
  while (adc_reading > 0x3C0)
  {
    SweepLEDsBlocking();
 
    // Start ADC single conversion to measure BREF.
    adc_reading = ADCSingleConversion();
 
    _delay_ms(10);
  }
 
  // Disable pull up resistor
  PORTF &= ~(1 << IBUS_PIN);
 
  _delay_ms(10);
 
#else
  // Select AD reference voltage and left adjust result.
  ADMUX = (ADC_REFERENCE_VOLTAGE | (1 << ADLAR));
 
  SweepLEDsBlocking();
#endif
 
  // Re-initialize ADC mux channel select.
  ADMUX &= ~ADC_MUX_L_BITS;
  ADMUX |= ADC_MUX_L_SPEED;
  // Set trigger source to ADC_TRIGGER.
  ADCSRB = ADC_MUX_H_SPEED | ADC_TRIGGER;
 
  // Re-initialize ADC to work with interrupts.
  ADCSRA = (1 << ADEN) | (1 << ADSC) | (1 << ADATE) | (1 << ADIF) | (1 << ADIE) | ADC_PRESCALER;
}

static uint16_t ADCSingleConversion(void)
{
  // Initialize ADC for one-time conversion.
  ADCSRA |= (1 << ADSC);
 
  // Wait for the conversion to complete.
  while (ADCSRA & (1 << ADSC))
  {
    // Wait until the conversion is finished.
  }
 
  // Read the ADC result and combine ADCH and ADCL into a 16-bit value.
  uint16_t value = (ADCL >> 6);
  value |= 0x3ff & (ADCH << 2);
 
  return value;
}

static void EnableUpdate(void)
{
  if ((PIND & (1 << ENABLE_PIN)) != 0)
  {
    motorFlags.enable = TRUE;
  }
  else
  {
    DisableMotor();
  }
}

static void DisableMotor(void)
{
  motorFlags.enable = FALSE;
  SetFaultFlag(FAULT_MOTOR_STOPPED, FALSE);
 
#if (TURN_OFF_MODE == TURN_OFF_MODE_COAST)
  // Disable driver signals to let the motor coast.
  motorFlags.driveWaveform = WAVEFORM_UNDEFINED;
  DisablePWMOutputs();
  ClearPWMPorts();
#endif
}

static void RemoteUpdate(void)
{
  if ((PIND & (1 << REMOTE_PIN)) != 0)
  {
    motorFlags.remote = TRUE;
  }
  else
  {
    motorFlags.remote = FALSE;
  }
}

static void SpeedController(void)
{
  if (motorFlags.enable == TRUE)
  {
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
    // Calculate an increment set point from the analog speed input.
    int16_t incrementSetpoint = ((int32_t)speedInput * SPEED_CONTROLLER_MAX_SPEED) / SPEED_CONTROLLER_MAX_INPUT;
 
    // PID regulator with feed forward from speed input.
    uint16_t outputValue;
 
    outputValue = PIDController(incrementSetpoint, (motorConfigs.tim4Freq / (lastCommutationTicks * 3)) >> 1, &pidParameters);
 
    if (outputValue > 255)
    {
      outputValue = 255;
    }
 
    speedOutput = outputValue;
 
    // Without the delay PID does not reset when needed
    _delay_us(1);
#else
    // Calculate the delta in speedInput
    int16_t delta = speedInput - speedOutput;
    // If delta exceeds the maximum allowed change, limit it and update
    // speedOutput
    if (delta > SPEED_CONTROLLER_MAX_DELTA)
    {
      speedOutput += SPEED_CONTROLLER_MAX_DELTA;
    }
    else if (delta < -SPEED_CONTROLLER_MAX_DELTA)
    {
      speedOutput -= SPEED_CONTROLLER_MAX_DELTA;
    }
    else
    {
      speedOutput = speedInput;
    }
#endif
  }
  else
  {
    if (speedOutput > 0)
    {
      if (speedOutput > SPEED_CONTROLLER_MAX_DELTA)
      {
        speedOutput -= SPEED_CONTROLLER_MAX_DELTA;
      }
      else
      {
        speedOutput = 0;
      }
    }
  }
}

static void FatalError()
{
  // Stop the motor.
  DisableMotor();
 
  // Once this is set no more faults will be registered creating a snapshot
  // of the failure point.
  motorFlags.fatalFault = TRUE;
}

static FORCE_INLINE void SetDuty(const uint16_t duty)
{
  TC4H = duty >> 8;
  OCR4A = 0xFF & duty;
}

static FORCE_INLINE void TimersSetModeBlockCommutation(void)
{
  // Set PWM pins to input (High-Z) while changing modes.
  DisablePWMOutputs();
 
  // Sets up timers.
  TCCR4A = (0 << COM4A1) | (1 << COM4A0) | (0 << COM4B1) | (1 << COM4B0) | (1 << PWM4A) | (1 << PWM4B);
  TCCR4C |= (0 << COM4D1) | (1 << COM4D0) | (1 << PWM4D);
  TCCR4D = (1 << WGM41) | (1 << WGM40);
 
  // Set output duty cycle to zero for now.
  SetDuty(0);
 
  // Wait for the next PWM cycle to ensure that all outputs are updated.
  TimersWaitForNextPWMCycle();
 
  motorFlags.driveWaveform = WAVEFORM_BLOCK_COMMUTATION;
 
  // Change PWM pins to output again to allow PWM control.
  EnablePWMOutputs();
}

static FORCE_INLINE void TimersWaitForNextPWMCycle(void)
{
  // Clear Timer1 Capture event flag.
  TIFR4 = (1 << TOV4);
 
  // Wait for new Timer1 Capture event flag.
  while (!(TIFR4 & (1 << TOV4)))
  {
  }
}

static FORCE_INLINE void BlockCommutate(const uint8_t direction, uint8_t hall)
{
  const uint8_t *tableAddress;
 
  if (direction == DIRECTION_FORWARD)
  {
    tableAddress = blockCommutationTableForward;
  }
  else
  {
    tableAddress = blockCommutationTableReverse;
  }
  tableAddress += (hall * 4);
 
  ClearPWMPorts();
  TCCR4E &= ~0b00111111;
 
  DisablePWMOutputs();
 
  PORTB |= (uint8_t)pgm_read_byte_near(tableAddress++);
  PORTC |= (uint8_t)pgm_read_byte_near(tableAddress++);
  PORTD |= (uint8_t)pgm_read_byte_near(tableAddress++);
  TCCR4E |= (uint8_t)pgm_read_byte_near(tableAddress);
 
  EnablePWMOutputs();
}

static FORCE_INLINE uint8_t GetHall(void)
{
  uint8_t hall;
 
  hall = HALL_PIN & ((1 << H3_PIN) | (1 << H2_PIN) | (1 << H1_PIN));
  hall >>= H1_PIN;
 
  return hall;
}

static FORCE_INLINE void DesiredDirectionUpdate(void)
{
  if (motorFlags.enable == TRUE)
  {
    return;
  }
  else if ((PIND & (1 << DIRECTION_COMMAND_PIN)) != 0)
  {
    motorFlags.desiredDirection = DIRECTION_REVERSE;
  }
  else
  {
    motorFlags.desiredDirection = DIRECTION_FORWARD;
  }
}

static FORCE_INLINE void ActualDirectionUpdate(uint8_t lastHall, const uint8_t newHall)
{
  // Ensure that lastHall is within the bounds of the table. If not, set it to
  // 0, which is also an illegal hall value but a legal table index.
  if (lastHall > 6)
  {
    lastHall = 0;
  }
 
  if (pgm_read_byte_near(&expectedHallSequenceForward[lastHall]) == newHall)
  {
    motorFlags.actualDirection = DIRECTION_FORWARD;
  }
  else if (pgm_read_byte_near(&expectedHallSequenceReverse[lastHall]) == newHall)
  {
    motorFlags.actualDirection = DIRECTION_REVERSE;
  }
  else
  {
    motorFlags.actualDirection = DIRECTION_UNKNOWN;
  }
}

static FORCE_INLINE void ReverseRotationSignalUpdate(void)
{
  if (motorFlags.actualDirection == motorFlags.desiredDirection)
  {
    faultFlags.reverseDirection = FALSE;
  }
  else
  {
    faultFlags.reverseDirection = TRUE;
  }
}

static FORCE_INLINE void EnablePWMOutputs(void)
{
  DDRB |= PWM_PATTERN_PORTB;
  DDRC |= PWM_PATTERN_PORTC;
  DDRD |= PWM_PATTERN_PORTD;
}

static FORCE_INLINE void DisablePWMOutputs(void)
{
  DDRB &= ~PWM_PATTERN_PORTB;
  DDRC &= ~PWM_PATTERN_PORTC;
  DDRD &= ~PWM_PATTERN_PORTD;
}

static FORCE_INLINE void ClearPWMPorts(void)
{
  PORTB &= ~PWM_PATTERN_PORTB;
  PORTC &= ~PWM_PATTERN_PORTC;
  PORTD &= ~PWM_PATTERN_PORTD;
}

static FORCE_INLINE void CommutationTicksUpdate(void)
{
  // If the motor is not stopped, increment the tick counter.
  if (commutationTicks < COMMUTATION_TICKS_STOPPED)
  {
    commutationTicks++;
  }
  // If motor is stopped, set the stopped flag and clear the tick counter.
  else
  {
    // Set flags to notify that the motor is stopped.
    SetFaultFlag(FAULT_MOTOR_STOPPED, TRUE);
    lastCommutationTicks = 0xffff;
 
    // Get the current hall value.
    uint8_t hall = GetHall();
    if ((hall == 0) || (hall == 0b111))
    {
      SetFaultFlag(FAULT_NO_HALL_CONNECTIONS, TRUE);
    }
 
    // If the motor is in a fatal fault, motor is now stopped so loop forever.
    if (motorFlags.fatalFault == TRUE)
    {
      while (1)
      {
        faultSequentialStateMachine(&faultFlags, &motorFlags);
        _delay_ms(2);
      }
    }
    // If the motor is supposed to be enabled, and the drive method is not block commutation,
    // reset the speed output and set the drive waveform.
    else if (motorFlags.driveWaveform != WAVEFORM_BLOCK_COMMUTATION && motorFlags.enable == TRUE)
    {
      speedOutput = 0;
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
      PIDResetIntegrator(&pidParameters);
#endif
      TimersSetModeBlockCommutation();
      BlockCommutate(motorFlags.desiredDirection, GetHall());
    }
    // If the motor is supposed to be stopped, (and it has stopped now) ...
    else if (motorFlags.enable == FALSE)
    {
#if (TURN_OFF_MODE == TURN_OFF_MODE_RAMP)
      // ... unset the drive waveform and disable PWM outputs.
      motorFlags.driveWaveform = WAVEFORM_UNDEFINED;
      DisablePWMOutputs();
      ClearPWMPorts();
#endif
      // ... update the desired direction flag as this would not have been
      // updated if the direction was changed while the motor was running.
      DesiredDirectionUpdate();
    }
  }
}

ISR(INT0_vect)
{
  EnableUpdate();
}

ISR(PCINT0_vect)
{
  static uint8_t lastHall = 0xff;
  uint8_t hall;
 
  hall = GetHall();
 
  if (motorFlags.driveWaveform == WAVEFORM_BLOCK_COMMUTATION)
  {
    BlockCommutate(motorFlags.desiredDirection, hall);
  }
 
  // Update flags that depend on hall sensor value.
  ActualDirectionUpdate(lastHall, hall);
  ReverseRotationSignalUpdate();
 
  lastHall = hall;
 
  // Reset commutation timer.
  lastCommutationTicks = calculateEMA(commutationTicks, lastCommutationTicks, 2);
  commutationTicks = 0;
 
  // Since the hall sensors are changing, the motor can not be stopped.
  // For fast access, SetFaultFlag() is not used to update this flag.
  // Instead, the flag is updated directly.
  faultFlags.motorStopped = FALSE;
  faultFlags.noHallConnections = FALSE;
}

ISR(INT2_vect)
{
  // Update desired direction flag.
  DesiredDirectionUpdate();
 
  // Stop the motor.
  DisableMotor();
}

ISR(TIMER4_OVF_vect)
{
  if (motorFlags.driveWaveform == WAVEFORM_BLOCK_COMMUTATION)
  {
    uint16_t dutyCycle = ((uint32_t)speedOutput * motorConfigs.tim4Top) >> 7;
 
    if (dutyCycle > (uint16_t)(motorConfigs.tim4Top << 1))
    {
      dutyCycle = (uint16_t)(motorConfigs.tim4Top << 1);
    }
 
    SetDuty(dutyCycle);
  }
 
  CommutationTicksUpdate();
 
  {
    // Run the speed regulation loop with constant intervals.
    static uint8_t speedRegTicks = 0;
    speedRegTicks++;
    if (speedRegTicks >= SPEED_CONTROLLER_TIME_BASE)
    {
      motorFlags.speedControllerRun = TRUE;
      speedRegTicks -= SPEED_CONTROLLER_TIME_BASE;
    }
  }
}
 
#if (EMULATE_HALL == TRUE)
ISR(TIMER3_OVF_vect)
{
  if (motorFlags.enable == TRUE)
  {
    uint8_t hall = GetHall();
 
    if (motorFlags.desiredDirection == DIRECTION_FORWARD)
    {
      PORTB = (PORTB & ~((1 << H1_PIN) | (1 << H2_PIN) | (1 << H3_PIN))) | ((0x07 & pgm_read_byte_near(&expectedHallSequenceForward[hall])) << H1_PIN);
    }
    else
    {
      PORTB = (PORTB & ~((1 << H1_PIN) | (1 << H2_PIN) | (1 << H3_PIN))) | ((0x07 & pgm_read_byte_near(&expectedHallSequenceReverse[hall])) << H1_PIN);
    }
  }
}
#endif

ISR(TIMER1_OVF_vect)
{
  faultSequentialStateMachine(&faultFlags, &motorFlags);
}

ISR(ADC_vect)
{
  switch ((ADMUX & ADC_MUX_L_BITS) | (ADCSRB & ADC_MUX_H_BITS))
  {
  case (ADC_MUX_H_SPEED | ADC_MUX_L_SPEED):
    // Handle ADC conversion result for speed measurement.
    if (motorConfigs.speedInputSource == SPEED_INPUT_SOURCE_LOCAL)
    {
      speedInput = ADCH;
    }
    ADMUX &= ~ADC_MUX_L_BITS;
    ADMUX |= ADC_MUX_L_IBUS;
    ADCSRB &= ~ADC_MUX_H_BITS;
    ADCSRB |= ADC_MUX_H_IBUS;
    break;
  case (ADC_MUX_H_IBUS | ADC_MUX_L_IBUS):
    // Handle ADC conversion result for current measurement.
    ibus = ADCL >> 6;
    ibus |= (ADCH << 2);
    ADMUX &= ~ADC_MUX_L_BITS;
    ADMUX |= ADC_MUX_L_IPHASE_U;
    ADCSRB &= ~ADC_MUX_H_BITS;
    ADCSRB |= ADC_MUX_H_IPHASE_U;
 
#if (IBUS_FAULT_ENABLE == TRUE)
    // Debounce current error flags.
    static uint8_t currentErrorCount = 0;
    if (ibus > IBUS_ERROR_THRESHOLD)
    {
      if (currentErrorCount < 3)
      {
        currentErrorCount++;
      }
      else
      {
        SetFaultFlag(FAULT_OVER_CURRENT, TRUE);
        SetFaultFlag(FAULT_USER_FLAG1, TRUE);
        SetFaultFlag(FAULT_USER_FLAG2, TRUE);
        SetFaultFlag(FAULT_USER_FLAG3, TRUE);
        FatalError();
      }
    }
    else
#endif
        if (ibus > IBUS_WARNING_THRESHOLD)
    {
      SetFaultFlag(FAULT_OVER_CURRENT, TRUE);
#if (IBUS_FAULT_ENABLE == TRUE)
      currentErrorCount = 0;
#endif
    }
    else
    {
      SetFaultFlag(FAULT_OVER_CURRENT, FALSE);
#if (IBUS_FAULT_ENABLE == TRUE)
      currentErrorCount = 0;
#endif
    }
    break;
  case (ADC_MUX_H_IPHASE_U | ADC_MUX_L_IPHASE_U):
    // Handle ADC conversion result for phase current measurement.
    iphaseU = ADCL >> 6;
    iphaseU |= (ADCH << 2);
    ADMUX &= ~ADC_MUX_L_BITS;
    ADMUX |= ADC_MUX_L_IPHASE_V;
    ADCSRB &= ~ADC_MUX_H_BITS;
    ADCSRB |= ADC_MUX_H_IPHASE_V;
    break;
  case (ADC_MUX_H_IPHASE_V | ADC_MUX_L_IPHASE_V):
    // Handle ADC conversion result for phase current measurement.
    iphaseV = ADCL >> 6;
    iphaseV |= (ADCH << 2);
    ADMUX &= ~ADC_MUX_L_BITS;
    ADMUX |= ADC_MUX_L_IPHASE_W;
    ADCSRB &= ~ADC_MUX_H_BITS;
    ADCSRB |= ADC_MUX_H_IPHASE_W;
    break;
  case (ADC_MUX_H_IPHASE_W | ADC_MUX_L_IPHASE_W):
    // Handle ADC conversion result for phase current measurement.
    iphaseW = ADCL >> 6;
    iphaseW |= (ADCH << 2);
    ADMUX &= ~ADC_MUX_L_BITS;
    ADMUX |= ADC_MUX_L_VBUSVREF;
    ADCSRB &= ~ADC_MUX_H_BITS;
    ADCSRB |= ADC_MUX_H_VBUSVREF;
    break;
  case (ADC_MUX_H_VBUSVREF | ADC_MUX_L_VBUSVREF):
    // Handle ADC conversion result for gate voltage reference measurement.
    vbusVref = ADCL >> 6;
    vbusVref |= (ADCH << 2);
    ADMUX &= ~ADC_MUX_L_BITS;
    ADMUX |= ADC_MUX_L_SPEED;
    ADCSRB &= ~ADC_MUX_H_BITS;
    ADCSRB |= ADC_MUX_H_SPEED;
    break;
  default:
    // This is probably an error and should be handled.
    SetFaultFlag(FAULT_USER_FLAG1, TRUE);
    SetFaultFlag(FAULT_USER_FLAG2, TRUE);
    SetFaultFlag(FAULT_USER_FLAG3, TRUE);
    FatalError();
    break;
  }
 
  // Clear Timer/Counter0 overflow flag.
  TIFR0 = (1 << TOV0);
}

static void SetFaultFlag(fault_flag_t flag, uint8_t value)
{
  if (motorFlags.fatalFault)
  {
    // Fatal fault active, cannot set any more flags.
    return;
  }
 
  switch (flag)
  {
  case FAULT_RESERVED:
    faultFlags.reserved = value;
    break;
  case FAULT_REVERSE_DIRECTION:
    faultFlags.reverseDirection = value;
    break;
  case FAULT_MOTOR_STOPPED:
    faultFlags.motorStopped = value;
    break;
  case FAULT_OVER_CURRENT:
    faultFlags.overCurrent = value;
    break;
  case FAULT_NO_HALL_CONNECTIONS:
    faultFlags.noHallConnections = value;
    break;
  case FAULT_USER_FLAG1:
    faultFlags.userFlag1 = value;
    break;
  case FAULT_USER_FLAG2:
    faultFlags.userFlag2 = value;
    break;
  case FAULT_USER_FLAG3:
    faultFlags.userFlag3 = value;
    break;
  default:
    return; // Invalid flag
  }
 
  return; // Success
}