/*
* This file is part of the hoverboard-firmware-hack project.
*
* Copyright (C) 2017-2018 Rene Hopf <renehopf@mac.com>
* Copyright (C) 2017-2018 Nico Stute <crinq@crinq.de>
* Copyright (C) 2017-2018 Niklas Fauth <niklas.fauth@kit.fail>
* Copyright (C) 2019-2020 Emanuel FERU <aerdronix@gmail.com>
* Copyright (C) 2019-2020 Daniel Brunner <daniel@brunner.ninja>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <algorithm>
#include <atomic>
#include <bit>
#include <cstring>

#include "stm32f1xx_hal.h"

#include "defines.h"
#include "config.h"
#include "protocol.h"

extern "C" {
#include "BLDC_controller.h"
extern const P rtP_Left; // default settings defined in BLDC_controller_data.c
}

namespace {
TIM_HandleTypeDef htim_right;
TIM_HandleTypeDef htim_left;
ADC_HandleTypeDef hadc1;
ADC_HandleTypeDef hadc2;
#ifdef HUARN2
UART_HandleTypeDef huart2;
#endif
#ifdef HUARN3
UART_HandleTypeDef huart3;
#endif

#ifdef HUARN2
DMA_HandleTypeDef hdma_usart2_rx;
DMA_HandleTypeDef hdma_usart2_tx;
#endif

#ifdef HUARN3
DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart3_tx;
#endif

#ifdef FEATURE_CAN
CAN_HandleTypeDef     CanHandle;

/* Definition for CANx clock resources */
#define CANx                           CAN1
#define CANx_CLK_ENABLE()              __HAL_RCC_CAN1_CLK_ENABLE()
#define CANx_GPIO_CLK_ENABLE()         __HAL_RCC_GPIOB_CLK_ENABLE()

#define CANx_FORCE_RESET()             __HAL_RCC_CAN1_FORCE_RESET()
#define CANx_RELEASE_RESET()           __HAL_RCC_CAN1_RELEASE_RESET()

/* Definition for CANx Pins */
#define CANx_TX_PIN                    GPIO_PIN_9
#define CANx_TX_GPIO_PORT              GPIOB
#define CANx_RX_PIN                    GPIO_PIN_8
#define CANx_RX_GPIO_PORT              GPIOB

/* Definition for CANx AFIO Remap */
#define CANx_AFIO_REMAP_CLK_ENABLE()   __HAL_RCC_AFIO_CLK_ENABLE()
#define CANx_AFIO_REMAP_RX_TX_PIN()    __HAL_AFIO_REMAP_CAN1_2()

/* Definition for CAN's NVIC */
#define CANx_RX_IRQn                   USB_LP_CAN1_RX0_IRQn
#define CANx_RX_IRQHandler             USB_LP_CAN1_RX0_IRQHandler
#define CANx_TX_IRQn                   USB_HP_CAN1_TX_IRQn
#define CANx_TX_IRQHandler             USB_HP_CAN1_TX_IRQHandler
#endif

#ifdef LOG_TO_SERIAL
char logBuffer[512];
#endif


template<std::size_t formatLength, typename ... Targs>
void myPrintf(const char (&format)[formatLength], Targs ... args)
{
#ifdef LOG_TO_SERIAL
#ifdef HUARN2
#define UART_DMA_CHANNEL DMA1_Channel7
#endif
#ifdef HUARN3
#define UART_DMA_CHANNEL DMA1_Channel2
#endif

    while (UART_DMA_CHANNEL->CNDTR != 0);

    char processedFormat[formatLength+2];
    std::copy(std::begin(format), std::end(format), std::begin(processedFormat));
    processedFormat[formatLength-1] = '\r';
    processedFormat[formatLength] = '\n';
    processedFormat[formatLength+1] = '\0';

    const auto size = std::snprintf(logBuffer, sizeof(logBuffer), processedFormat, args ...);
    if (size < 0)
        return;

    UART_DMA_CHANNEL->CCR    &= ~DMA_CCR_EN;
    UART_DMA_CHANNEL->CNDTR   = size;
    UART_DMA_CHANNEL->CMAR    = uint64_t(logBuffer);
    UART_DMA_CHANNEL->CCR    |= DMA_CCR_EN;
#endif
}

volatile struct {
    uint16_t dcr;
    uint16_t dcl;
    uint16_t rl1;
    uint16_t rl2;
    uint16_t rr1;
    uint16_t rr2;
    uint16_t batt1;
    uint16_t l_tx2;
    uint16_t temp;
    uint16_t l_rx2;
} adc_buffer;

// ###############################################################################

std::atomic<uint32_t> timeout;
int16_t timeoutCntSerial   = 0;  // Timeout counter for Rx Serial command

uint32_t main_loop_counter;

uint16_t offsetcount = 0;
int16_t offsetrl1    = 2000;
int16_t offsetrl2    = 2000;
int16_t offsetrr1    = 2000;
int16_t offsetrr2    = 2000;
int16_t offsetdcl    = 2000;
int16_t offsetdcr    = 2000;

int16_t batVoltage       = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE;
int32_t batVoltageFixdt  = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 20;  // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point

int32_t board_temp_adcFixdt = adc_buffer.temp << 20;  // Fixed-point filter output initialized with current ADC converted to fixed-point
int16_t board_temp_adcFilt  = adc_buffer.temp;
int16_t board_temp_deg_c;

struct {
    RT_MODEL rtM; /* Real-time model */
    P     rtP;    /* Block parameters (auto storage) */
    DW    rtDW;   /* Observable states */
    ExtU  rtU;    /* External inputs */
    ExtY  rtY;    /* External outputs */

    MotorState state;

    uint32_t chops = 0;

    uint8_t hallBits() const
    {
        return (rtU.b_hallA ? 0 : 1) | (rtU.b_hallB ? 0 : 2) | (rtU.b_hallC ? 0 : 4);
    }
} left, right;

struct {
    BuzzerState state;

    uint32_t timer = 0;
} buzzer;

Command command;


void filtLowPass32(int16_t u, uint16_t coef, int32_t *y);

void SystemClock_Config();

#ifdef HUARN2
void UART2_Init();
#endif

#ifdef HUARN3
void UART3_Init();
#endif

#ifdef FEATURE_CAN
void CAN_Init();
#endif

void MX_GPIO_Init();

void MX_TIM_Init();

void MX_ADC1_Init();

void MX_ADC2_Init();

void poweroff();

#ifdef FEATURE_SERIAL_CONTROL
void parseCommand();
#endif

#ifdef FEATURE_SERIAL_FEEDBACK
void sendFeedback();
#endif

#ifdef FEATURE_CAN
void sendCanFeedback();
#endif

} // anonymous namespace

int main()
{
    HAL_Init();
    __HAL_RCC_AFIO_CLK_ENABLE();
    HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);
    /* System interrupt init*/
    /* MemoryManagement_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(MemoryManagement_IRQn, 0, 0);
    /* BusFault_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(BusFault_IRQn, 0, 0);
    /* UsageFault_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(UsageFault_IRQn, 0, 0);
    /* SVCall_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(SVCall_IRQn, 0, 0);
    /* DebugMonitor_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(DebugMonitor_IRQn, 0, 0);
    /* PendSV_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(PendSV_IRQn, 0, 0);
    /* SysTick_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);

    SystemClock_Config();

    __HAL_RCC_DMA1_CLK_DISABLE();
    MX_GPIO_Init();
    MX_TIM_Init();
    MX_ADC1_Init();
    MX_ADC2_Init();

    HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET);

    HAL_ADC_Start(&hadc1);
    HAL_ADC_Start(&hadc2);

    enum { CurrentMeasAB, CurrentMeasBC, CurrentMeasAC };

    left.rtP = rtP_Left;
    left.rtP.b_angleMeasEna      = 0;
#ifdef PETERS_PLATINE
    left.rtP.z_selPhaCurMeasABC  = CurrentMeasBC;            // Left motor measured current phases = {iB, iC} -> do NOT change
#else
    left.rtP.z_selPhaCurMeasABC  = CurrentMeasAB;            // Left motor measured current phases = {iA, iB} -> do NOT change
#endif
    left.rtP.z_ctrlTypSel        = uint8_t(left.state.ctrlTyp);
    left.rtP.b_diagEna           = DIAG_ENA;
    left.rtP.i_max               = (left.state.iMotMax * A2BIT_CONV) << 4;        // fixdt(1,16,4)
    left.rtP.n_max               = left.state.nMotMax << 4;                       // fixdt(1,16,4)
    left.rtP.b_fieldWeakEna      = FIELD_WEAK_ENA;
    left.rtP.id_fieldWeakMax     = (left.state.fieldWeakMax * A2BIT_CONV) << 4;   // fixdt(1,16,4)
    left.rtP.a_phaAdvMax         = left.state.phaseAdvMax << 4;                   // fixdt(1,16,4)
    left.rtP.r_fieldWeakHi       = FIELD_WEAK_HI << 4;                   // fixdt(1,16,4)
    left.rtP.r_fieldWeakLo       = FIELD_WEAK_LO << 4;                   // fixdt(1,16,4)

    right.rtP = rtP_Left;
    right.rtP.b_angleMeasEna      = 0;
    right.rtP.z_selPhaCurMeasABC = CurrentMeasBC;            // Right motor measured current phases = {iB, iC} -> do NOT change
    right.rtP.z_ctrlTypSel       = uint8_t(right.state.ctrlTyp);
    right.rtP.b_diagEna          = DIAG_ENA;
    right.rtP.i_max              = (right.state.iMotMax * A2BIT_CONV) << 4;        // fixdt(1,16,4)
    right.rtP.n_max              = right.state.nMotMax << 4;                       // fixdt(1,16,4)
    right.rtP.b_fieldWeakEna     = FIELD_WEAK_ENA;
    right.rtP.id_fieldWeakMax    = (right.state.fieldWeakMax * A2BIT_CONV) << 4;   // fixdt(1,16,4)
    right.rtP.a_phaAdvMax        = right.state.phaseAdvMax << 4;                   // fixdt(1,16,4)
    right.rtP.r_fieldWeakHi      = FIELD_WEAK_HI << 4;                   // fixdt(1,16,4)
    right.rtP.r_fieldWeakLo      = FIELD_WEAK_LO << 4;                   // fixdt(1,16,4)

    left.rtM.defaultParam        = &left.rtP;
    left.rtM.dwork               = &left.rtDW;
    left.rtM.inputs              = &left.rtU;
    left.rtM.outputs             = &left.rtY;

    right.rtM.defaultParam       = &right.rtP;
    right.rtM.dwork              = &right.rtDW;
    right.rtM.inputs             = &right.rtU;
    right.rtM.outputs            = &right.rtY;

    /* Initialize BLDC controllers */
    BLDC_controller_initialize(&left.rtM);
    BLDC_controller_initialize(&right.rtM);

    for (int i = 8; i >= 0; i--)
    {
        buzzer.state.freq = (uint8_t)i;
        HAL_Delay(50);
    }
    buzzer.state.freq = 0;

#ifdef HUARN2
    UART2_Init();
#endif
#ifdef HUARN3
    UART3_Init();
#endif

#ifdef FEATURE_CAN
    CAN_Init();
#endif

#ifdef MOTOR_TEST
    int pwm = 0;
    int8_t dir = 1;
#endif

#ifdef FEATURE_SERIAL_CONTROL
#ifdef HUARN2
    HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, sizeof(command));
#endif
#ifdef HUARN3
    HAL_UART_Receive_DMA(&huart3, (uint8_t *)&command, sizeof(command));
#endif
#endif

    while (true)
    {
        //HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms

#ifdef FEATURE_SERIAL_CONTROL
        parseCommand();
#endif

        timeout = 0;

#ifdef MOTOR_TEST
        left.state.enable = true;
        left.state.ctrlMod = ControlMode::Voltage;
        left.state.ctrlTyp = ControlType::FieldOrientedControl;
        left.state.pwm = pwm;
        left.state.iMotMax = 2;

        right.state.enable = true;
        right.state.ctrlMod = ControlMode::Voltage;
        right.state.ctrlTyp = ControlType::FieldOrientedControl;
        right.state.pwm = pwm;
        right.state.iMotMax = 2;

        constexpr auto pwmMax = 250;

        pwm += dir;
        if (pwm > pwmMax) {
          pwm = pwmMax;
          dir = -1;
        } else if (pwm < -pwmMax) {
          pwm = -pwmMax;
          dir = 1;
        }
#endif

        // ####### CALC BOARD TEMPERATURE #######
        filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt);
        board_temp_adcFilt  = (int16_t)(board_temp_adcFixdt >> 20);  // convert fixed-point to integer
        board_temp_deg_c    = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;

#ifdef FEATURE_SERIAL_FEEDBACK
        if (main_loop_counter % 50 == 0)
            sendFeedback();
#endif

#ifdef FEATURE_CAN
        sendCanFeedback();
#endif

#ifdef FEATURE_BUTTON
        if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN))
        {
            left.state.enable = right.state.enable = 0;           // disable motors

            while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {}    // wait until button is released

            if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) {               // do not power off after software reset (from a programmer/debugger)
                __HAL_RCC_CLEAR_RESET_FLAGS();                      // clear reset flags
            } else {
                poweroff();                                         // release power-latch
            }
        }
#endif

        left.rtP.z_ctrlTypSel         = uint8_t(left.state.ctrlTyp);
        left.rtP.i_max                = (left.state.iMotMax * A2BIT_CONV) << 4;        // fixdt(1,16,4)
        left.rtP.n_max                = left.state.nMotMax << 4;                       // fixdt(1,16,4)
        left.rtP.id_fieldWeakMax      = (left.state.fieldWeakMax * A2BIT_CONV) << 4;   // fixdt(1,16,4)
        left.rtP.a_phaAdvMax          = left.state.phaseAdvMax << 4;                   // fixdt(1,16,4)

        left.rtU.z_ctrlModReq = uint8_t(left.state.ctrlMod);
        left.rtU.r_inpTgt     = left.state.pwm;

        right.rtP.z_ctrlTypSel         = uint8_t(right.state.ctrlTyp);
        right.rtP.i_max                = (right.state.iMotMax * A2BIT_CONV) << 4;        // fixdt(1,16,4)
        right.rtP.n_max                = right.state.nMotMax << 4;                       // fixdt(1,16,4)
        right.rtP.id_fieldWeakMax      = (right.state.fieldWeakMax * A2BIT_CONV) << 4;   // fixdt(1,16,4)
        right.rtP.a_phaAdvMax          = right.state.phaseAdvMax << 4;                   // fixdt(1,16,4)

        right.rtU.z_ctrlModReq  = uint8_t(right.state.ctrlMod);
        right.rtU.r_inpTgt      = right.state.pwm;

        main_loop_counter++;
        timeout++;
    }
}

namespace {
void updateBatVoltage()
{
    if (buzzer.timer % 1000 == 0) // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point
    {
        filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt);
        batVoltage = (int16_t)(batVoltageFixdt >> 20);  // convert fixed-point to integer
    }
}

void updateBuzzer()
{
    if (buzzer.timer % 1000 == 0) // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point
    {
        filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt);
        batVoltage = (int16_t)(batVoltageFixdt >> 20);  // convert fixed-point to integer
    }

    //create square wave for buzzer
    buzzer.timer++;
    if (buzzer.state.freq != 0 && (buzzer.timer / 1000) % (buzzer.state.pattern + 1) == 0)
    {
        if (buzzer.timer % buzzer.state.freq == 0)
        {
            HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN);
        }
    }
    else
    {
        HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, GPIO_PIN_RESET);
    }
}

void updateMotors()
{
    DMA1->IFCR = DMA_IFCR_CTCIF1;

    if (offsetcount < 2000) // calibrate ADC offsets
    {
        offsetcount++;
        offsetrl1 = (adc_buffer.rl1 + offsetrl1) / 2;
        offsetrl2 = (adc_buffer.rl2 + offsetrl2) / 2;
        offsetrr1 = (adc_buffer.rr1 + offsetrr1) / 2;
        offsetrr2 = (adc_buffer.rr2 + offsetrr2) / 2;
        offsetdcl = (adc_buffer.dcl + offsetdcl) / 2;
        offsetdcr = (adc_buffer.dcr + offsetdcr) / 2;
        return;
    }

    // Get Left motor currents
#ifdef PETERS_PLATINE
    int16_t curL_phaB = (int16_t)(offsetrl1 - adc_buffer.rl1)*2;
    int16_t curL_phaA = (int16_t)(offsetrl2 - adc_buffer.rl2)*2;
#else
    int16_t curL_phaA = (int16_t)(offsetrl1 - adc_buffer.rl1);
    int16_t curL_phaB = (int16_t)(offsetrl2 - adc_buffer.rl2);
#endif
    int16_t curL_DC   = (int16_t)(offsetdcl - adc_buffer.dcl);

    // Get Right motor currents
#ifdef PETERS_PLATINE
    int16_t curR_phaB = (int16_t)(offsetrr1 - adc_buffer.rr1)*2;
    int16_t curR_phaC = (int16_t)(offsetrr2 - adc_buffer.rr2)*2;
#else
    int16_t curR_phaB = (int16_t)(offsetrr1 - adc_buffer.rr1);
    int16_t curR_phaC = (int16_t)(offsetrr2 - adc_buffer.rr2);
#endif
    int16_t curR_DC   = (int16_t)(offsetdcr - adc_buffer.dcr);

    const bool chopL = std::abs(curL_DC) > (left.state.iDcMax * A2BIT_CONV);
    if (chopL)
        left.chops++;

    const bool chopR = std::abs(curR_DC) > (right.state.iDcMax * A2BIT_CONV);
    if (chopR)
        right.chops++;

    const auto timeoutVal = timeout.load();

    // Disable PWM when current limit is reached (current chopping)
    // This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX
    if (chopL || timeoutVal > TIMEOUT || !left.state.enable)
        LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
    else
        LEFT_TIM->BDTR |= TIM_BDTR_MOE;

    if (chopR || timeoutVal > TIMEOUT || !right.state.enable)
        RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
    else
        RIGHT_TIM->BDTR |= TIM_BDTR_MOE;

    // ############################### MOTOR CONTROL ###############################

    static boolean_T OverrunFlag = false;

    /* Check for overrun */
    if (OverrunFlag)
      return;
    OverrunFlag = true;

    constexpr int32_t pwm_res  = 64000000 / 2 / PWM_FREQ; // = 2000
    constexpr int32_t pwm_margin = 100;        /* This margin allows to always have a window in the PWM signal for proper Phase currents measurement */

    /* Make sure to stop BOTH motors in case of an error */

    constexpr bool ignoreOtherMotor =
#ifdef FEATURE_IGNORE_OTHER_MOTOR
        false
#else
        true
#endif
    ;

    const bool enableLFin = left.state.enable && left.rtY.z_errCode == 0 && (right.rtY.z_errCode == 0 || ignoreOtherMotor);
    const bool enableRFin = right.state.enable && (left.rtY.z_errCode == 0 || ignoreOtherMotor) && right.rtY.z_errCode == 0;

    // ========================= LEFT MOTOR ============================
    // Get hall sensors values
    bool hall_ul = !(LEFT_HALL_U_PORT->IDR & LEFT_HALL_U_PIN);
    bool hall_vl = !(LEFT_HALL_V_PORT->IDR & LEFT_HALL_V_PIN);
    bool hall_wl = !(LEFT_HALL_W_PORT->IDR & LEFT_HALL_W_PIN);

    /* Set motor inputs here */
    left.rtU.b_motEna     = enableLFin;
    left.rtU.b_hallA      = hall_ul;
    left.rtU.b_hallB      = hall_vl;
    left.rtU.b_hallC      = hall_wl;
    left.rtU.i_phaAB      = curL_phaA;
    left.rtU.i_phaBC      = curL_phaB;
    left.rtU.i_DCLink     = curL_DC;

    /* Step the controller */
    BLDC_controller_step(&left.rtM);

    /* Get motor outputs here */
    int ul            = left.rtY.DC_phaA;
    int vl            = left.rtY.DC_phaB;
    int wl            = left.rtY.DC_phaC;

    /* Apply commands */
    LEFT_TIM->LEFT_TIM_U    = (uint16_t)std::clamp(ul + pwm_res / 2, pwm_margin, pwm_res-pwm_margin);
    LEFT_TIM->LEFT_TIM_V    = (uint16_t)std::clamp(vl + pwm_res / 2, pwm_margin, pwm_res-pwm_margin);
    LEFT_TIM->LEFT_TIM_W    = (uint16_t)std::clamp(wl + pwm_res / 2, pwm_margin, pwm_res-pwm_margin);
    // =================================================================


    // ========================= RIGHT MOTOR ===========================
    // Get hall sensors values
    bool hall_ur = !(RIGHT_HALL_U_PORT->IDR & RIGHT_HALL_U_PIN);
    bool hall_vr = !(RIGHT_HALL_V_PORT->IDR & RIGHT_HALL_V_PIN);
    bool hall_wr = !(RIGHT_HALL_W_PORT->IDR & RIGHT_HALL_W_PIN);

    /* Set motor inputs here */
    right.rtU.b_motEna      = enableRFin;
    right.rtU.b_hallA       = hall_ur;
    right.rtU.b_hallB       = hall_vr;
    right.rtU.b_hallC       = hall_wr;
    right.rtU.i_phaAB       = curR_phaB;
    right.rtU.i_phaBC       = curR_phaC;
    right.rtU.i_DCLink      = curR_DC;

    /* Step the controller */
    BLDC_controller_step(&right.rtM);

    /* Get motor outputs here */
    int ur            = right.rtY.DC_phaA;
    int vr            = right.rtY.DC_phaB;
    int wr            = right.rtY.DC_phaC;

    /* Apply commands */
    RIGHT_TIM->RIGHT_TIM_U  = (uint16_t)std::clamp(ur + pwm_res / 2, pwm_margin, pwm_res-pwm_margin);
    RIGHT_TIM->RIGHT_TIM_V  = (uint16_t)std::clamp(vr + pwm_res / 2, pwm_margin, pwm_res-pwm_margin);
    RIGHT_TIM->RIGHT_TIM_W  = (uint16_t)std::clamp(wr + pwm_res / 2, pwm_margin, pwm_res-pwm_margin);
    // =================================================================

    /* Indicate task complete */
    OverrunFlag = false;
}

// ===========================================================
  /* Low pass filter fixed-point 32 bits: fixdt(1,32,20)
  * Max:  2047.9375
  * Min: -2048
  * Res:  0.0625
  *
  * Inputs:       u     = int16
  * Outputs:      y     = fixdt(1,32,20)
  * Parameters:   coef  = fixdt(0,16,16) = [0,65535U]
  *
  * Example:
  * If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point)
  * filtLowPass16(u, 52429, &y);
  * yint = (int16_t)(y >> 20); // the integer output is the fixed-point ouput shifted by 20 bits
  */
void filtLowPass32(int16_t u, uint16_t coef, int32_t *y)
{
    int tmp;

    tmp = (int16_t)(u << 4) - (*y >> 16);
    tmp = std::clamp(tmp, -32768, 32767);  // Overflow protection
    *y  = coef * tmp + (*y);
}

// ===========================================================

/** System Clock Configuration
*/
void SystemClock_Config()
{
    RCC_OscInitTypeDef RCC_OscInitStruct;
    RCC_ClkInitTypeDef RCC_ClkInitStruct;
    RCC_PeriphCLKInitTypeDef PeriphClkInit;

    /**Initializes the CPU, AHB and APB busses clocks
    */
    RCC_OscInitStruct.OscillatorType      = RCC_OSCILLATORTYPE_HSI;
    RCC_OscInitStruct.HSIState            = RCC_HSI_ON;
    RCC_OscInitStruct.HSICalibrationValue = 16;
    RCC_OscInitStruct.PLL.PLLState        = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource       = RCC_PLLSOURCE_HSI_DIV2;
    RCC_OscInitStruct.PLL.PLLMUL          = RCC_PLL_MUL16;
    HAL_RCC_OscConfig(&RCC_OscInitStruct);

    /**Initializes the CPU, AHB and APB busses clocks
    */
    RCC_ClkInitStruct.ClockType           = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource        = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider       = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider      = RCC_HCLK_DIV2;
    RCC_ClkInitStruct.APB2CLKDivider      = RCC_HCLK_DIV1;

    HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);

    PeriphClkInit.PeriphClockSelection    = RCC_PERIPHCLK_ADC;
    // PeriphClkInit.AdcClockSelection    = RCC_ADCPCLK2_DIV8;  // 8 MHz
    PeriphClkInit.AdcClockSelection       = RCC_ADCPCLK2_DIV4;  // 16 MHz
    HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);

    /**Configure the Systick interrupt time
    */
    HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq() / 1000);

    /**Configure the Systick
    */
    HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

    /* SysTick_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
}

#ifdef HUARN2
void UART2_Init()
{
    /* The code below is commented out - otwerwise Serial Receive does not work */
    // #ifdef CONTROL_SERIAL_USART2
    //   /* DMA1_Channel6_IRQn interrupt configuration */
    //   HAL_NVIC_SetPriority(DMA1_Channel6_IRQn, 5, 6);
    //   HAL_NVIC_EnableIRQ(DMA1_Channel6_IRQn);
    //   /* DMA1_Channel7_IRQn interrupt configuration */
    //   HAL_NVIC_SetPriority(DMA1_Channel7_IRQn, 5, 7);
    //   HAL_NVIC_EnableIRQ(DMA1_Channel7_IRQn);
    // #endif

    // Disable serial interrupt - it is not needed
    HAL_NVIC_DisableIRQ(DMA1_Channel6_IRQn);    // Rx Channel
    HAL_NVIC_DisableIRQ(DMA1_Channel7_IRQn);    // Tx Channel

    __HAL_RCC_DMA1_CLK_ENABLE();
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_USART2_CLK_ENABLE();

    huart2.Instance           = USART2;
    huart2.Init.BaudRate      = USART2_BAUD;
    huart2.Init.WordLength    = USART2_WORDLENGTH;
    huart2.Init.StopBits      = UART_STOPBITS_1;
    huart2.Init.Parity        = UART_PARITY_NONE;
    huart2.Init.HwFlowCtl     = UART_HWCONTROL_NONE;
    huart2.Init.OverSampling  = UART_OVERSAMPLING_16;
    huart2.Init.Mode        = UART_MODE_TX_RX;
    HAL_UART_Init(&huart2);

    USART2->CR3 |= USART_CR3_DMAT;  // | USART_CR3_DMAR | USART_CR3_OVRDIS;

    GPIO_InitTypeDef GPIO_InitStruct;
    GPIO_InitStruct.Pin       = GPIO_PIN_2;
    GPIO_InitStruct.Pull      = GPIO_PULLUP; //GPIO_NOPULL;
    GPIO_InitStruct.Mode      = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Speed     = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    GPIO_InitStruct.Pin     = GPIO_PIN_3;
    GPIO_InitStruct.Mode    = GPIO_MODE_INPUT; //GPIO_MODE_AF_PP;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    /* Peripheral DMA init*/
    hdma_usart2_rx.Instance                 = DMA1_Channel6;
    hdma_usart2_rx.Init.Direction           = DMA_PERIPH_TO_MEMORY;
    hdma_usart2_rx.Init.PeriphInc           = DMA_PINC_DISABLE;
    hdma_usart2_rx.Init.MemInc              = DMA_MINC_ENABLE;
    hdma_usart2_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
    hdma_usart2_rx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
    hdma_usart2_rx.Init.Mode                = DMA_CIRCULAR; //DMA_NORMAL;
    hdma_usart2_rx.Init.Priority            = DMA_PRIORITY_LOW;
    HAL_DMA_Init(&hdma_usart2_rx);
    __HAL_LINKDMA(&huart2, hdmarx, hdma_usart2_rx);

    hdma_usart2_tx.Instance                   = DMA1_Channel7;
    hdma_usart2_tx.Init.Direction             = DMA_MEMORY_TO_PERIPH;
    hdma_usart2_tx.Init.PeriphInc             = DMA_PINC_DISABLE;
    hdma_usart2_tx.Init.MemInc                = DMA_MINC_ENABLE;
    hdma_usart2_tx.Init.PeriphDataAlignment   = DMA_PDATAALIGN_BYTE;
    hdma_usart2_tx.Init.MemDataAlignment      = DMA_MDATAALIGN_BYTE;
    hdma_usart2_tx.Init.Mode                  = DMA_NORMAL;
    hdma_usart2_tx.Init.Priority              = DMA_PRIORITY_LOW;
    HAL_DMA_Init(&hdma_usart2_tx);

    __HAL_LINKDMA(&huart2, hdmatx, hdma_usart2_tx);

    DMA1_Channel7->CPAR     = uint64_t(&(USART2->DR));
    DMA1_Channel7->CNDTR    = 0;
    DMA1->IFCR              = DMA_IFCR_CTCIF7 | DMA_IFCR_CHTIF7 | DMA_IFCR_CGIF7;
}
#endif

#ifdef HUARN3
void UART3_Init()
{
    /* The code below is commented out - otwerwise Serial Receive does not work */
    // #ifdef CONTROL_SERIAL_USART3
    //   /* DMA1_Channel3_IRQn interrupt configuration */
    //   HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 5, 3);
    //   HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn);
    //   /* DMA1_Channel2_IRQn interrupt configuration */
    //   HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 5, 2);
    //   HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
    // #endif

    // Disable serial interrupt - it is not needed
    HAL_NVIC_DisableIRQ(DMA1_Channel3_IRQn);  // Rx Channel
    HAL_NVIC_DisableIRQ(DMA1_Channel2_IRQn);  // Tx Channel

    __HAL_RCC_DMA1_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_USART3_CLK_ENABLE();

    huart3.Instance             = USART3;
    huart3.Init.BaudRate        = USART3_BAUD;
    huart3.Init.WordLength      = USART3_WORDLENGTH;
    huart3.Init.StopBits        = UART_STOPBITS_1;
    huart3.Init.Parity          = UART_PARITY_NONE;
    huart3.Init.HwFlowCtl       = UART_HWCONTROL_NONE;
    huart3.Init.OverSampling    = UART_OVERSAMPLING_16;
    huart3.Init.Mode          = UART_MODE_TX_RX;
    HAL_UART_Init(&huart3);

    USART3->CR3 |= USART_CR3_DMAT;  // | USART_CR3_DMAR | USART_CR3_OVRDIS;

    GPIO_InitTypeDef GPIO_InitStruct;
    GPIO_InitStruct.Pin         = GPIO_PIN_10;
    GPIO_InitStruct.Pull        = GPIO_PULLUP;
    GPIO_InitStruct.Mode        = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Speed       = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    GPIO_InitStruct.Pin       = GPIO_PIN_11;
    GPIO_InitStruct.Mode      = GPIO_MODE_INPUT;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    /* Peripheral DMA init*/
    hdma_usart3_rx.Instance                   = DMA1_Channel3;
    hdma_usart3_rx.Init.Direction             = DMA_PERIPH_TO_MEMORY;
    hdma_usart3_rx.Init.PeriphInc             = DMA_PINC_DISABLE;
    hdma_usart3_rx.Init.MemInc                = DMA_MINC_ENABLE;
    hdma_usart3_rx.Init.PeriphDataAlignment   = DMA_PDATAALIGN_BYTE;
    hdma_usart3_rx.Init.MemDataAlignment      = DMA_MDATAALIGN_BYTE;
    hdma_usart3_rx.Init.Mode                  = DMA_CIRCULAR; //DMA_NORMAL;
    hdma_usart3_rx.Init.Priority              = DMA_PRIORITY_LOW;
    HAL_DMA_Init(&hdma_usart3_rx);
    __HAL_LINKDMA(&huart3, hdmarx, hdma_usart3_rx);

    hdma_usart3_tx.Instance                     = DMA1_Channel2;
    hdma_usart3_tx.Init.Direction               = DMA_MEMORY_TO_PERIPH;
    hdma_usart3_tx.Init.PeriphInc               = DMA_PINC_DISABLE;
    hdma_usart3_tx.Init.MemInc                  = DMA_MINC_ENABLE;
    hdma_usart3_tx.Init.PeriphDataAlignment     = DMA_PDATAALIGN_BYTE;
    hdma_usart3_tx.Init.MemDataAlignment        = DMA_MDATAALIGN_BYTE;
    hdma_usart3_tx.Init.Mode                    = DMA_NORMAL;
    hdma_usart3_tx.Init.Priority                = DMA_PRIORITY_LOW;
    HAL_DMA_Init(&hdma_usart3_tx);

    __HAL_LINKDMA(&huart3, hdmatx, hdma_usart3_tx);

    DMA1_Channel2->CPAR     = (uint32_t) & (USART3->DR);
    DMA1_Channel2->CNDTR    = 0;
    DMA1->IFCR              = DMA_IFCR_CTCIF2 | DMA_IFCR_CHTIF2 | DMA_IFCR_CGIF2;
}
#endif

#ifdef FEATURE_CAN
void CAN_MspInit(CAN_HandleTypeDef *hcan);
void CAN_MspDeInit(CAN_HandleTypeDef *hcan);
void CAN_MspDeInit(CAN_HandleTypeDef *hcan);
void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle);
void CAN_TxMailboxCompleteCallback(CAN_HandleTypeDef *hcan);

void CAN_Init()
{
    myPrintf("CAN_Init() called");

    CAN_FilterTypeDef  sFilterConfig;

    /* Configure the CAN peripheral */
    CanHandle.Instance = CANx;

    CanHandle.MspInitCallback = CAN_MspInit;
    CanHandle.MspDeInitCallback = CAN_MspDeInit;

    CanHandle.Init.TimeTriggeredMode = DISABLE;
    CanHandle.Init.AutoBusOff = ENABLE;
    CanHandle.Init.AutoWakeUp = DISABLE;
    CanHandle.Init.AutoRetransmission = ENABLE;
    CanHandle.Init.ReceiveFifoLocked = DISABLE;
    CanHandle.Init.TransmitFifoPriority = DISABLE;
    CanHandle.Init.Mode = CAN_MODE_NORMAL;

    CanHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
    CanHandle.Init.TimeSeg1 = CAN_BS1_3TQ;
    CanHandle.Init.TimeSeg2 = CAN_BS2_4TQ;
    CanHandle.Init.Prescaler = 16;

    if (const auto result = HAL_CAN_Init(&CanHandle); result == HAL_OK)
        myPrintf("HAL_CAN_Init() succeeded");
    else
    {
        myPrintf("HAL_CAN_Init() failed with %i", result);
        while (true);
    }

    /* Configure the CAN Filter */
    sFilterConfig.FilterBank = 0;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig.FilterIdHigh = 0xFFFF; // TODO
    sFilterConfig.FilterIdLow = 0xFFFF; // TODO
    sFilterConfig.FilterMaskIdHigh = 0xFFFF; // TODO
    sFilterConfig.FilterMaskIdLow = 0xFFFF; // TODO
    sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
    sFilterConfig.FilterActivation = ENABLE;
    sFilterConfig.SlaveStartFilterBank = 14;

    if (const auto result = HAL_CAN_ConfigFilter(&CanHandle, &sFilterConfig); result == HAL_OK)
        myPrintf("HAL_CAN_ConfigFilter() succeeded");
    else
    {
        myPrintf("HAL_CAN_ConfigFilter() failed with %i", result);
        while (true);
    }

    if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID, CAN_RxFifo0MsgPendingCallback); result == HAL_OK)
        myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID succeeded");
    else
    {
        myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID failed with %i", result);
        while (true);
    }

    if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK)
        myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID succeeded");
    else
    {
        myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID failed with %i", result);
        while (true);
    }

    if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK)
        myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID succeeded");
    else
    {
        myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID failed with %i", result);
        while (true);
    }

    if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK)
        myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID succeeded");
    else
    {
        myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID failed with %i", result);
        while (true);
    }

    /* Start the CAN peripheral */
    if (const auto result = HAL_CAN_Start(&CanHandle); result == HAL_OK)
        myPrintf("HAL_CAN_Start() succeeded");
    else
    {
        myPrintf("HAL_CAN_Start() failed with %i", result);
        while (true);
    }

    /* Activate CAN RX notification */
    if (const auto result = HAL_CAN_ActivateNotification(&CanHandle, CAN_IT_RX_FIFO0_MSG_PENDING); result == HAL_OK)
        myPrintf("HAL_CAN_ActivateNotification() CAN_IT_RX_FIFO0_MSG_PENDING succeeded");
    else
    {
        myPrintf("HAL_CAN_ActivateNotification() CAN_IT_RX_FIFO0_MSG_PENDING failed with %i", result);
        while (true);
    }

    /* Activate CAN TX notification */
    if (const auto result = HAL_CAN_ActivateNotification(&CanHandle, CAN_IT_TX_MAILBOX_EMPTY); result == HAL_OK)
        myPrintf("HAL_CAN_ActivateNotification() CAN_IT_TX_MAILBOX_EMPTY succeeded");
    else
    {
        myPrintf("HAL_CAN_ActivateNotification() CAN_IT_TX_MAILBOX_EMPTY failed with %i", result);
        while (true);
    }
}

void CAN_MspInit(CAN_HandleTypeDef *hcan)
{
    myPrintf("CAN_MspInit() called");

    GPIO_InitTypeDef   GPIO_InitStruct;

    /*##-1- Enable peripherals and GPIO Clocks #################################*/
    /* CAN1 Periph clock enable */
    CANx_CLK_ENABLE();
    /* Enable GPIO clock ****************************************/
    CANx_GPIO_CLK_ENABLE();
    /* Enable AFIO clock and Remap CAN PINs to PB8 and PB9*******/
    CANx_AFIO_REMAP_CLK_ENABLE();
    CANx_AFIO_REMAP_RX_TX_PIN();

    /*##-2- Configure peripheral GPIO ##########################################*/
    /* CAN1 TX GPIO pin configuration */
    GPIO_InitStruct.Pin = CANx_TX_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Pull = GPIO_PULLUP;

    HAL_GPIO_Init(CANx_TX_GPIO_PORT, &GPIO_InitStruct);

    /* CAN1 RX GPIO pin configuration */
    GPIO_InitStruct.Pin = CANx_RX_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Pull = GPIO_PULLUP;

    HAL_GPIO_Init(CANx_RX_GPIO_PORT, &GPIO_InitStruct);

    /*##-3- Configure the NVIC #################################################*/
    /* NVIC configuration for CAN1 Reception complete interrupt */
    HAL_NVIC_SetPriority(CANx_RX_IRQn, 1, 0);
    HAL_NVIC_EnableIRQ(CANx_RX_IRQn);

    HAL_NVIC_SetPriority(CANx_TX_IRQn, 1, 0);
    HAL_NVIC_EnableIRQ(CANx_TX_IRQn);
}

void CAN_MspDeInit(CAN_HandleTypeDef *hcan)
{
    myPrintf("CAN_MspDeInit() called");

    /*##-1- Reset peripherals ##################################################*/
    CANx_FORCE_RESET();
    CANx_RELEASE_RESET();

    /*##-2- Disable peripherals and GPIO Clocks ################################*/
    /* De-initialize the CAN1 TX GPIO pin */
    HAL_GPIO_DeInit(CANx_TX_GPIO_PORT, CANx_TX_PIN);
    /* De-initialize the CAN1 RX GPIO pin */
    HAL_GPIO_DeInit(CANx_RX_GPIO_PORT, CANx_RX_PIN);

    /*##-4- Disable the NVIC for CAN reception #################################*/
    HAL_NVIC_DisableIRQ(CANx_RX_IRQn);
}

void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle)
{
    //myPrintf("CAN_RxFifo0MsgPendingCallback() called");

    CAN_RxHeaderTypeDef header;
    uint8_t buf[8];
    if (const auto result = HAL_CAN_GetRxMessage(CanHandle, CAN_RX_FIFO0, &header, buf); result == HAL_OK)
    {
        myPrintf("StdId=%x %u ExtId=%x %u IDE=%x %u RTR=%x %u DLC=%x %u Timestamp=%x %u FilterMatchIndex=%x %u",
            header.StdId, header.StdId,
            header.ExtId, header.ExtId,
            header.IDE, header.IDE,
            header.RTR, header.RTR,
            header.DLC, header.DLC,
            header.Timestamp, header.Timestamp,
            header.FilterMatchIndex, header.FilterMatchIndex
        );
    }
    else
    {
        myPrintf("HAL_CAN_GetRxMessage() failed with %i", result);
        while (true);
    }

    //if (header.IDE == CAN_ID_STD &&
    //    (header.StdId == CAN_ID_COMMAND_STW_TO_BOARD(BOARD_INDEX) ||
    //     header.StdId == CAN_ID_FEEDBACK_STW_TO_BOARD(BOARD_INDEX)))
    //{
    //    can_feedc0de_handle_frame(header.StdId, RxData, dlc_to_len(header.DLC));
    //}

    // slightly yucky, but we don't want to block inside the IRQ handler
    //if (HAL_CAN_GetTxMailboxesFreeLevel(CanHandle) >= 2)
    //{
    //    can_feedc0de_poll();
    //}
}

void CAN_TxMailboxCompleteCallback(CAN_HandleTypeDef *hcan)
{
    //myPrintf("CAN_TxMailboxCompleteCallback() called");

    // slightly yucky, but we don't want to block inside the IRQ handler
    //if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) >= 2)
    //{
    //    can_feedc0de_poll();
    //}
}
#endif

void MX_GPIO_Init()
{
    GPIO_InitTypeDef GPIO_InitStruct;

    /* GPIO Ports Clock Enable */
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();

    GPIO_InitStruct.Mode  = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull  = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

    GPIO_InitStruct.Pin = LEFT_HALL_U_PIN;
    HAL_GPIO_Init(LEFT_HALL_U_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = LEFT_HALL_V_PIN;
    HAL_GPIO_Init(LEFT_HALL_V_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = LEFT_HALL_W_PIN;
    HAL_GPIO_Init(LEFT_HALL_W_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_HALL_U_PIN;
    HAL_GPIO_Init(RIGHT_HALL_U_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_HALL_V_PIN;
    HAL_GPIO_Init(RIGHT_HALL_V_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_HALL_W_PIN;
    HAL_GPIO_Init(RIGHT_HALL_W_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = CHARGER_PIN;
    HAL_GPIO_Init(CHARGER_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = BUTTON_PIN;
    HAL_GPIO_Init(BUTTON_PORT, &GPIO_InitStruct);


    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;

    GPIO_InitStruct.Pin = LED_PIN;
    HAL_GPIO_Init(LED_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = BUZZER_PIN;
    HAL_GPIO_Init(BUZZER_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = OFF_PIN;
    HAL_GPIO_Init(OFF_PORT, &GPIO_InitStruct);


    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;

    GPIO_InitStruct.Pin = LEFT_DC_CUR_PIN;
    HAL_GPIO_Init(LEFT_DC_CUR_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = LEFT_U_CUR_PIN;
    HAL_GPIO_Init(LEFT_U_CUR_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = LEFT_V_CUR_PIN;
    HAL_GPIO_Init(LEFT_V_CUR_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_DC_CUR_PIN;
    HAL_GPIO_Init(RIGHT_DC_CUR_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_U_CUR_PIN;
    HAL_GPIO_Init(RIGHT_U_CUR_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_V_CUR_PIN;
    HAL_GPIO_Init(RIGHT_V_CUR_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = DCLINK_PIN;
    HAL_GPIO_Init(DCLINK_PORT, &GPIO_InitStruct);

    //Analog in
    GPIO_InitStruct.Pin = GPIO_PIN_3;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
    GPIO_InitStruct.Pin = GPIO_PIN_2;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;

    GPIO_InitStruct.Pin = LEFT_TIM_UH_PIN;
    HAL_GPIO_Init(LEFT_TIM_UH_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = LEFT_TIM_VH_PIN;
    HAL_GPIO_Init(LEFT_TIM_VH_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = LEFT_TIM_WH_PIN;
    HAL_GPIO_Init(LEFT_TIM_WH_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = LEFT_TIM_UL_PIN;
    HAL_GPIO_Init(LEFT_TIM_UL_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = LEFT_TIM_VL_PIN;
    HAL_GPIO_Init(LEFT_TIM_VL_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = LEFT_TIM_WL_PIN;
    HAL_GPIO_Init(LEFT_TIM_WL_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_TIM_UH_PIN;
    HAL_GPIO_Init(RIGHT_TIM_UH_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_TIM_VH_PIN;
    HAL_GPIO_Init(RIGHT_TIM_VH_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_TIM_WH_PIN;
    HAL_GPIO_Init(RIGHT_TIM_WH_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_TIM_UL_PIN;
    HAL_GPIO_Init(RIGHT_TIM_UL_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_TIM_VL_PIN;
    HAL_GPIO_Init(RIGHT_TIM_VL_PORT, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = RIGHT_TIM_WL_PIN;
    HAL_GPIO_Init(RIGHT_TIM_WL_PORT, &GPIO_InitStruct);
}

void MX_TIM_Init()
{
    __HAL_RCC_TIM1_CLK_ENABLE();
    __HAL_RCC_TIM8_CLK_ENABLE();

    TIM_MasterConfigTypeDef sMasterConfig;
    TIM_OC_InitTypeDef sConfigOC;
    TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig;
    TIM_SlaveConfigTypeDef sTimConfig;

    htim_right.Instance               = RIGHT_TIM;
    htim_right.Init.Prescaler         = 0;
    htim_right.Init.CounterMode       = TIM_COUNTERMODE_CENTERALIGNED1;
    htim_right.Init.Period            = 64000000 / 2 / PWM_FREQ;
    htim_right.Init.ClockDivision     = TIM_CLOCKDIVISION_DIV1;
    htim_right.Init.RepetitionCounter = 0;
    htim_right.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    HAL_TIM_PWM_Init(&htim_right);

    sMasterConfig.MasterOutputTrigger = TIM_TRGO_ENABLE;
    sMasterConfig.MasterSlaveMode     = TIM_MASTERSLAVEMODE_DISABLE;
    HAL_TIMEx_MasterConfigSynchronization(&htim_right, &sMasterConfig);

    sConfigOC.OCMode       = TIM_OCMODE_PWM1;
    sConfigOC.Pulse        = 0;
    sConfigOC.OCPolarity   = TIM_OCPOLARITY_HIGH;
#ifdef PETERS_PLATINE
    sConfigOC.OCNPolarity  = TIM_OCNPOLARITY_HIGH;
#else
    sConfigOC.OCNPolarity  = TIM_OCNPOLARITY_LOW;
#endif
    sConfigOC.OCFastMode   = TIM_OCFAST_DISABLE;
    sConfigOC.OCIdleState  = TIM_OCIDLESTATE_RESET;
    sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_SET;
    HAL_TIM_PWM_ConfigChannel(&htim_right, &sConfigOC, TIM_CHANNEL_1);
    HAL_TIM_PWM_ConfigChannel(&htim_right, &sConfigOC, TIM_CHANNEL_2);
    HAL_TIM_PWM_ConfigChannel(&htim_right, &sConfigOC, TIM_CHANNEL_3);

    sBreakDeadTimeConfig.OffStateRunMode  = TIM_OSSR_ENABLE;
    sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_ENABLE;
    sBreakDeadTimeConfig.LockLevel        = TIM_LOCKLEVEL_OFF;
    sBreakDeadTimeConfig.DeadTime         = DEAD_TIME;
    sBreakDeadTimeConfig.BreakState       = TIM_BREAK_DISABLE;
    sBreakDeadTimeConfig.BreakPolarity    = TIM_BREAKPOLARITY_LOW;
    sBreakDeadTimeConfig.AutomaticOutput  = TIM_AUTOMATICOUTPUT_DISABLE;
    HAL_TIMEx_ConfigBreakDeadTime(&htim_right, &sBreakDeadTimeConfig);

    htim_left.Instance               = LEFT_TIM;
    htim_left.Init.Prescaler         = 0;
    htim_left.Init.CounterMode       = TIM_COUNTERMODE_CENTERALIGNED1;
    htim_left.Init.Period            = 64000000 / 2 / PWM_FREQ;
    htim_left.Init.ClockDivision     = TIM_CLOCKDIVISION_DIV1;
    htim_left.Init.RepetitionCounter = 0;
    htim_left.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    HAL_TIM_PWM_Init(&htim_left);

    sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE;
    sMasterConfig.MasterSlaveMode     = TIM_MASTERSLAVEMODE_ENABLE;
    HAL_TIMEx_MasterConfigSynchronization(&htim_left, &sMasterConfig);

    sTimConfig.InputTrigger = TIM_TS_ITR0;
    sTimConfig.SlaveMode    = TIM_SLAVEMODE_GATED;
    HAL_TIM_SlaveConfigSynchro(&htim_left, &sTimConfig);

    // Start counting >0 to effectively offset timers by the time it takes for one ADC conversion to complete.
    // This method allows that the Phase currents ADC measurements are properly aligned with LOW-FET ON region for both motors
    LEFT_TIM->CNT 		     = ADC_TOTAL_CONV_TIME;

    sConfigOC.OCMode       = TIM_OCMODE_PWM1;
    sConfigOC.Pulse        = 0;
    sConfigOC.OCPolarity   = TIM_OCPOLARITY_HIGH;
#ifdef PETERS_PLATINE
    sConfigOC.OCNPolarity  = TIM_OCNPOLARITY_HIGH;
#else
    sConfigOC.OCNPolarity  = TIM_OCNPOLARITY_LOW;
#endif
    sConfigOC.OCFastMode   = TIM_OCFAST_DISABLE;
    sConfigOC.OCIdleState  = TIM_OCIDLESTATE_RESET;
    sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_SET;
    HAL_TIM_PWM_ConfigChannel(&htim_left, &sConfigOC, TIM_CHANNEL_1);
    HAL_TIM_PWM_ConfigChannel(&htim_left, &sConfigOC, TIM_CHANNEL_2);
    HAL_TIM_PWM_ConfigChannel(&htim_left, &sConfigOC, TIM_CHANNEL_3);

    sBreakDeadTimeConfig.OffStateRunMode  = TIM_OSSR_ENABLE;
    sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_ENABLE;
    sBreakDeadTimeConfig.LockLevel        = TIM_LOCKLEVEL_OFF;
    sBreakDeadTimeConfig.DeadTime         = DEAD_TIME;
    sBreakDeadTimeConfig.BreakState       = TIM_BREAK_DISABLE;
    sBreakDeadTimeConfig.BreakPolarity    = TIM_BREAKPOLARITY_LOW;
    sBreakDeadTimeConfig.AutomaticOutput  = TIM_AUTOMATICOUTPUT_DISABLE;
    HAL_TIMEx_ConfigBreakDeadTime(&htim_left, &sBreakDeadTimeConfig);

    LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
    RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;

    HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Start(&htim_left, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&htim_left, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&htim_left, TIM_CHANNEL_3);

    HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_3);

    htim_left.Instance->RCR = 1;

    __HAL_TIM_ENABLE(&htim_right);
}

void MX_ADC1_Init()
{
    ADC_MultiModeTypeDef multimode;
    ADC_ChannelConfTypeDef sConfig;

    __HAL_RCC_ADC1_CLK_ENABLE();

    hadc1.Instance                   = ADC1;
    hadc1.Init.ScanConvMode          = ADC_SCAN_ENABLE;
    hadc1.Init.ContinuousConvMode    = DISABLE;
    hadc1.Init.DiscontinuousConvMode = DISABLE;
    hadc1.Init.ExternalTrigConv      = ADC_EXTERNALTRIGCONV_T8_TRGO;
    hadc1.Init.DataAlign             = ADC_DATAALIGN_RIGHT;
    hadc1.Init.NbrOfConversion       = 5;
    HAL_ADC_Init(&hadc1);
    /**Enable or disable the remapping of ADC1_ETRGREG:
    * ADC1 External Event regular conversion is connected to TIM8 TRG0
    */
    __HAL_AFIO_REMAP_ADC1_ETRGREG_ENABLE();

    /**Configure the ADC multi-mode
    */
    multimode.Mode = ADC_DUALMODE_REGSIMULT;
    HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode);

    sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
    sConfig.Channel = ADC_CHANNEL_11;  // pc1 left cur  ->  right
    sConfig.Rank    = 1;
    HAL_ADC_ConfigChannel(&hadc1, &sConfig);

    // sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
    sConfig.SamplingTime = ADC_SAMPLETIME_7CYCLES_5;
    sConfig.Channel = ADC_CHANNEL_0;  // pa0 right a   ->  left
    sConfig.Rank    = 2;
    HAL_ADC_ConfigChannel(&hadc1, &sConfig);

    sConfig.Channel = ADC_CHANNEL_14;  // pc4 left b   -> right
    sConfig.Rank    = 3;
    HAL_ADC_ConfigChannel(&hadc1, &sConfig);

    sConfig.Channel = ADC_CHANNEL_12;  // pc2 vbat
    sConfig.Rank    = 4;
    HAL_ADC_ConfigChannel(&hadc1, &sConfig);

    //temperature requires at least 17.1uS sampling time
    sConfig.SamplingTime = ADC_SAMPLETIME_239CYCLES_5;
    sConfig.Channel = ADC_CHANNEL_TEMPSENSOR;  // internal temp
    sConfig.Rank    = 5;
    HAL_ADC_ConfigChannel(&hadc1, &sConfig);

    hadc1.Instance->CR2 |= ADC_CR2_DMA | ADC_CR2_TSVREFE;

    __HAL_ADC_ENABLE(&hadc1);

    __HAL_RCC_DMA1_CLK_ENABLE();

    DMA1_Channel1->CCR   = 0;
    DMA1_Channel1->CNDTR = 5;
    DMA1_Channel1->CPAR  = uint64_t(&(ADC1->DR));
    DMA1_Channel1->CMAR  = uint64_t(&adc_buffer);
    DMA1_Channel1->CCR   = DMA_CCR_MSIZE_1 | DMA_CCR_PSIZE_1 | DMA_CCR_MINC | DMA_CCR_CIRC | DMA_CCR_TCIE;
    DMA1_Channel1->CCR |= DMA_CCR_EN;

    HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn);
}

/* ADC2 init function */
void MX_ADC2_Init()
{
    ADC_ChannelConfTypeDef sConfig;

    __HAL_RCC_ADC2_CLK_ENABLE();

    // HAL_ADC_DeInit(&hadc2);
    // hadc2.Instance->CR2 = 0;
    /**Common config
    */
    hadc2.Instance                   = ADC2;
    hadc2.Init.ScanConvMode          = ADC_SCAN_ENABLE;
    hadc2.Init.ContinuousConvMode    = DISABLE;
    hadc2.Init.DiscontinuousConvMode = DISABLE;
    hadc2.Init.ExternalTrigConv      = ADC_SOFTWARE_START;
    hadc2.Init.DataAlign             = ADC_DATAALIGN_RIGHT;
    hadc2.Init.NbrOfConversion       = 5;
    HAL_ADC_Init(&hadc2);


    sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
    sConfig.Channel = ADC_CHANNEL_10;  // pc0 right cur   -> left
    sConfig.Rank    = 1;
    HAL_ADC_ConfigChannel(&hadc2, &sConfig);

    // sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
    sConfig.SamplingTime = ADC_SAMPLETIME_7CYCLES_5;
    sConfig.Channel = ADC_CHANNEL_13;  // pc3 right b   -> left
    sConfig.Rank    = 2;
    HAL_ADC_ConfigChannel(&hadc2, &sConfig);

    sConfig.Channel = ADC_CHANNEL_15;  // pc5 left c   -> right
    sConfig.Rank    = 3;
    HAL_ADC_ConfigChannel(&hadc2, &sConfig);

    sConfig.Channel = ADC_CHANNEL_2;  // pa2 uart-l-tx
    sConfig.Rank    = 4;
    HAL_ADC_ConfigChannel(&hadc2, &sConfig);

    sConfig.SamplingTime = ADC_SAMPLETIME_239CYCLES_5;
    sConfig.Channel = ADC_CHANNEL_3;  // pa3 uart-l-rx
    sConfig.Rank    = 5;
    HAL_ADC_ConfigChannel(&hadc2, &sConfig);

    hadc2.Instance->CR2 |= ADC_CR2_DMA;
    __HAL_ADC_ENABLE(&hadc2);
}

void poweroff()
{
  //  if (abs(speed) < 20) {  // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons
        buzzer.state.pattern = 0;
        left.state.enable = right.state.enable = 0;
        for (int i = 0; i < 8; i++) {
            buzzer.state.freq = (uint8_t)i;
            HAL_Delay(50);
        }
        HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
        for (int i = 0; i < 5; i++)
            HAL_Delay(1000);
  //  }
}

#ifdef FEATURE_SERIAL_CONTROL
void parseCommand()
{
    bool any_parsed{false};

    for (int i = 0; i < 1; i++)
    {
        if (command.start != Command::VALID_HEADER)
            continue;

        uint16_t checksum = calculateChecksum(command);
        if (command.checksum != checksum)
            continue;

        left.state = command.left;
        right.state = command.right;

        buzzer.state = command.buzzer;

        if (command.poweroff)
            poweroff();

        HAL_GPIO_WritePin(LED_PORT, LED_PIN, command.led ? GPIO_PIN_RESET : GPIO_PIN_SET);

        command.start     = Command::INVALID_HEADER; // Change the Start Frame for timeout detection in the next cycle
        timeoutCntSerial  = 0;                       // Reset the timeout counter

        any_parsed = true;
        break;
    }

    if (!any_parsed)
    {
        if (timeoutCntSerial++ >= 100) // Timeout qualification
        {
            timeoutCntSerial  = 100; // Limit timout counter value

            left.state = right.state = {.enable=true};

            //buzzer.state = { 24, 1 };

            HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_RESET);

            // Check periodically the received Start Frame. Try to re-sync by reseting the DMA
            if (main_loop_counter % 25 == 0)
            {
#ifdef HUARN2
                HAL_UART_DMAStop(&huart2);
                HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, sizeof(command));
#endif
#ifdef HUARN3
                HAL_UART_DMAStop(&huart3);
                HAL_UART_Receive_DMA(&huart3, (uint8_t *)&command, sizeof(command));
#endif
            }
        }
    }
}
#endif

#ifdef FEATURE_SERIAL_FEEDBACK
void sendFeedback()
{
#ifdef HUARN2
#define UART_DMA_CHANNEL DMA1_Channel7
#endif
#ifdef HUARN3
#define UART_DMA_CHANNEL DMA1_Channel2
#endif

    if (UART_DMA_CHANNEL->CNDTR != 0)
        return;

    static Feedback feedback;

    feedback.start = Feedback::VALID_HEADER;

    feedback.left.angle = left.rtY.a_elecAngle;
    feedback.right.angle = right.rtY.a_elecAngle;

    feedback.left.speed = left.rtY.n_mot;
    feedback.right.speed = right.rtY.n_mot;

    feedback.left.error = left.rtY.z_errCode;
    feedback.right.error = right.rtY.z_errCode;

    feedback.left.current = left.rtU.i_DCLink;
    feedback.right.current = right.rtU.i_DCLink;

    feedback.left.chops = left.chops;
    feedback.right.chops = right.chops;
    left.chops = 0;
    right.chops = 0;

    feedback.left.hallA = left.rtU.b_hallA;
    feedback.left.hallB = left.rtU.b_hallB;
    feedback.left.hallC = left.rtU.b_hallC;
    feedback.right.hallA = right.rtU.b_hallA;
    feedback.right.hallB = right.rtU.b_hallB;
    feedback.right.hallC = right.rtU.b_hallC;

    feedback.batVoltage = batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC;
    feedback.boardTemp = board_temp_deg_c;
    feedback.timeoutCntSerial = timeoutCntSerial;

    feedback.checksum = calculateChecksum(feedback);

    UART_DMA_CHANNEL->CCR    &= ~DMA_CCR_EN;
    UART_DMA_CHANNEL->CNDTR   = sizeof(feedback);
    UART_DMA_CHANNEL->CMAR    = uint64_t(&feedback);
    UART_DMA_CHANNEL->CCR    |= DMA_CCR_EN;
}
#endif

#ifdef FEATURE_CAN
void sendCanFeedback()
{
    //myPrintf("sendCanFeedback() called");

    const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle);
    if (!free)
        return;

    enum { //                         vv
        DeviceTypeMotorController = 0b00000000000
    };

    enum { //                         ..vv
        MotorControllerRec =        0b00000000000,
        MotorControllerSend =       0b00010000000,
    };

    enum { //                         ....vvvvv
        MotorControllerDcLink =     0b00000000000,
        MotorControllerSpeed =      0b00000000100,
        MotorControllerError =      0b00000001000,
        MotorControllerAngle =      0b00000001100,
        MotorControllerDcPhaA =     0b00000010000,
        MotorControllerDcPhaB =     0b00000010100,
        MotorControllerDcPhaC =     0b00000011000,
        MotorControllerChops =      0b00000011100,
        MotorControllerHall =       0b00000100000,
        MotorControllerVoltage =    0b00000100100,
        MotorControllerTemp =       0b00000101000
    };

    enum { //                         .........v
        MotorControllerFront =      0b00000000000,
        MotorControllerBack =       0b00000000010,
    };

    enum { //                         ..........v
        MotorControllerLeft =       0b00000000000,
        MotorControllerRight =      0b00000000001,
    };

    enum {
        MotorControllerFrontLeftDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerBack | MotorControllerRight,

        MotorControllerFrontLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerBack | MotorControllerRight,

        MotorControllerFrontLeftError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerBack | MotorControllerRight,

        MotorControllerFrontLeftAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerBack | MotorControllerRight,

        MotorControllerFrontLeftDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerBack | MotorControllerRight,

        MotorControllerFrontLeftDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerBack | MotorControllerRight,

        MotorControllerFrontLeftDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerBack | MotorControllerRight,

        MotorControllerFrontLeftChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerBack | MotorControllerRight,

        MotorControllerFrontLeftHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerBack | MotorControllerRight,

        MotorControllerFrontLeftVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerBack | MotorControllerRight,

        MotorControllerFrontLeftTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerFront | MotorControllerLeft,
        MotorControllerFrontRightTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerFront | MotorControllerRight,
        MotorControllerBackLeftTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerBack | MotorControllerLeft,
        MotorControllerBackRightTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerBack | MotorControllerRight,
    };

    enum SendState : uint8_t {
        LeftDcLink,
        RightDcLink,
        LeftSpeed,
        RightSpeed,
        LeftError,
        RightError,
        LeftAngle,
        RightAngle,
        LeftDcPhaA,
        RightDcPhaA,
        LeftDcPhaB,
        RightDcPhaB,
        LeftDcPhaC,
        RightDcPhaC,
        LeftChops,
        RightChops,
        LeftHall,
        RightHall,
        LeftVoltage,
        RightVoltage,
        LeftTemp,
        RightTemp,
        Restart
    };

    static union {
        SendState sendState{LeftDcLink};
        uint8_t asUint8;
    };

    constexpr auto send = [](uint32_t addr, auto value){
        CAN_TxHeaderTypeDef header;
        header.StdId = addr;
        header.ExtId = 0x01;
        header.RTR = CAN_RTR_DATA;
        header.IDE = CAN_ID_STD;
        header.DLC = sizeof(value);
        header.TransmitGlobalTime = DISABLE;

        uint8_t buf[8] {0};
        std::memcpy(buf, &value, sizeof(value));

        static uint32_t TxMailbox;
        if (const auto result = HAL_CAN_AddTxMessage(&CanHandle, &header, buf, &TxMailbox); result != HAL_OK)
        {
            myPrintf("HAL_CAN_AddTxMessage() failed with %i", result);
            //while (true);
        }
    };

    switch (sendState)
    {
    case LeftDcLink:   /* myPrintf("LeftCurrent  free=%u", free); */ send(MotorControllerFrontLeftDcLink,   left. rtU.i_DCLink);    break;
    case RightDcLink:  /* myPrintf("RightCurrent free=%u", free); */ send(MotorControllerFrontRightDcLink,  right.rtU.i_DCLink);    break;
    case LeftSpeed:    /* myPrintf("LeftSpeed    free=%u", free); */ send(MotorControllerFrontLeftSpeed,    left. rtY.n_mot);       break;
    case RightSpeed:   /* myPrintf("RightSpeed   free=%u", free); */ send(MotorControllerFrontRightSpeed,   right.rtY.n_mot);       break;
    case LeftError:    /* myPrintf("LeftError    free=%u", free); */ send(MotorControllerFrontLeftError,    left. rtY.z_errCode);   break;
    case RightError:   /* myPrintf("RightError   free=%u", free); */ send(MotorControllerFrontRightError,   right.rtY.z_errCode);   break;
    case LeftAngle:    /* myPrintf("LeftAngle    free=%u", free); */ send(MotorControllerFrontLeftAngle,    left. rtY.a_elecAngle); break;
    case RightAngle:   /* myPrintf("RightAngle   free=%u", free); */ send(MotorControllerFrontRightAngle,   right.rtY.a_elecAngle); break;
    case LeftDcPhaA:   /* myPrintf("LeftDcPhaA   free=%u", free); */ send(MotorControllerFrontLeftDcPhaA,   left. rtY.DC_phaA);     break;
    case RightDcPhaA:  /* myPrintf("RightDcPhaA  free=%u", free); */ send(MotorControllerFrontRightDcPhaA,  right.rtY.DC_phaA);     break;
    case LeftDcPhaB:   /* myPrintf("LeftDcPhaB   free=%u", free); */ send(MotorControllerFrontLeftDcPhaB,   left. rtY.DC_phaB);     break;
    case RightDcPhaB:  /* myPrintf("RightDcPhaB  free=%u", free); */ send(MotorControllerFrontRightDcPhaB,  right.rtY.DC_phaB);     break;
    case LeftDcPhaC:   /* myPrintf("LeftDcPhaC   free=%u", free); */ send(MotorControllerFrontLeftDcPhaC,   left. rtY.DC_phaC);     break;
    case RightDcPhaC:  /* myPrintf("RightDcPhaC  free=%u", free); */ send(MotorControllerFrontRightDcPhaC,  right.rtY.DC_phaC);     break;
    case LeftChops:    /* myPrintf("LeftChops    free=%u", free); */ send(MotorControllerFrontLeftChops,    left. chops);           break;
    case RightChops:   /* myPrintf("RightChops   free=%u", free); */ send(MotorControllerFrontRightChops,   right.chops);           break;
    case LeftHall:     /* myPrintf("LeftHall     free=%u", free); */ send(MotorControllerFrontLeftHall,     left.hallBits());       break;
    case RightHall:    /* myPrintf("RightHall    free=%u", free); */ send(MotorControllerFrontRightHall,    right.hallBits());      break;
    case LeftVoltage:  /* myPrintf("LeftVoltage  free=%u", free); */ send(MotorControllerFrontLeftVoltage,  batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break;
    case RightVoltage: /* myPrintf("RightVoltage free=%u", free); */ send(MotorControllerFrontRightVoltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break;
    case LeftTemp:     /* myPrintf("LeftTemp     free=%u", free); */ send(MotorControllerFrontLeftTemp,     board_temp_deg_c);      break;
    case RightTemp:    /* myPrintf("RightTemp    free=%u", free); */ send(MotorControllerFrontRightTemp,    board_temp_deg_c);      break;
    default: __builtin_unreachable();
    }

    asUint8++;

    if (sendState >= Restart)
        sendState = LeftDcLink;
}
#endif

} // anonymous namespace

/******************************************************************************/
/*            Cortex-M3 Processor Interruption and Exception Handlers         */
/******************************************************************************/

/**
* @brief This function handles Non maskable interrupt.
*/
extern "C" void NMI_Handler()
{
    /* USER CODE BEGIN NonMaskableInt_IRQn 0 */

    /* USER CODE END NonMaskableInt_IRQn 0 */
    /* USER CODE BEGIN NonMaskableInt_IRQn 1 */

    /* USER CODE END NonMaskableInt_IRQn 1 */
}

/**
* @brief This function handles Hard fault interrupt.
*/
extern "C" void HardFault_Handler()
{
    /* USER CODE BEGIN HardFault_IRQn 0 */

    /* USER CODE END HardFault_IRQn 0 */
    while (true);
    /* USER CODE BEGIN HardFault_IRQn 1 */

    /* USER CODE END HardFault_IRQn 1 */
}

/**
* @brief This function handles Memory management fault.
*/
extern "C" void MemManage_Handler()
{
  /* USER CODE BEGIN MemoryManagement_IRQn 0 */

  /* USER CODE END MemoryManagement_IRQn 0 */
  while (true);
  /* USER CODE BEGIN MemoryManagement_IRQn 1 */

  /* USER CODE END MemoryManagement_IRQn 1 */
}

/**
* @brief This function handles Prefetch fault, memory access fault.
*/
extern "C" void BusFault_Handler()
{
    /* USER CODE BEGIN BusFault_IRQn 0 */

    /* USER CODE END BusFault_IRQn 0 */
    while (true);
    /* USER CODE BEGIN BusFault_IRQn 1 */

    /* USER CODE END BusFault_IRQn 1 */
}

/**
* @brief This function handles Undefined instruction or illegal state.
*/
extern "C" void UsageFault_Handler()
{
    /* USER CODE BEGIN UsageFault_IRQn 0 */

    /* USER CODE END UsageFault_IRQn 0 */
    while (true);
    /* USER CODE BEGIN UsageFault_IRQn 1 */

    /* USER CODE END UsageFault_IRQn 1 */
}

/**
* @brief This function handles System service call via SWI instruction.
*/
extern "C" void SVC_Handler()
{
    /* USER CODE BEGIN SVCall_IRQn 0 */

    /* USER CODE END SVCall_IRQn 0 */
    /* USER CODE BEGIN SVCall_IRQn 1 */

    /* USER CODE END SVCall_IRQn 1 */
}

/**
* @brief This function handles Debug monitor.
*/
extern "C" void DebugMon_Handler()
{
    /* USER CODE BEGIN DebugMonitor_IRQn 0 */

    /* USER CODE END DebugMonitor_IRQn 0 */
    /* USER CODE BEGIN DebugMonitor_IRQn 1 */

    /* USER CODE END DebugMonitor_IRQn 1 */
}

/**
* @brief This function handles Pendable request for system service.
*/
extern "C" void PendSV_Handler()
{
  /* USER CODE BEGIN PendSV_IRQn 0 */

  /* USER CODE END PendSV_IRQn 0 */
  /* USER CODE BEGIN PendSV_IRQn 1 */

  /* USER CODE END PendSV_IRQn 1 */
}

extern "C" void SysTick_Handler()
{
    /* USER CODE BEGIN SysTick_IRQn 0 */

    /* USER CODE END SysTick_IRQn 0 */
    HAL_IncTick();
    HAL_SYSTICK_IRQHandler();
    /* USER CODE BEGIN SysTick_IRQn 1 */

    /* USER CODE END SysTick_IRQn 1 */
}

// =================================
// DMA interrupt frequency =~ 16 kHz
// =================================
extern "C" void DMA1_Channel1_IRQHandler()
{
    /* USER CODE BEGIN DMA1_Channel1_IRQn 0 */

    /* USER CODE END DMA1_Channel1_IRQn 0 */
    updateMotors();
    updateBatVoltage();
    updateBuzzer();
    /* USER CODE BEGIN DMA1_Channel1_IRQn 1 */

    /* USER CODE END DMA1_Channel1_IRQn 1 */
}

#ifdef HUARN2
extern "C" void DMA1_Channel6_IRQHandler()
{
    /* USER CODE BEGIN DMA1_Channel4_IRQn 0 */

    /* USER CODE END DMA1_Channel4_IRQn 0 */
    HAL_DMA_IRQHandler(&hdma_usart2_rx);
    /* USER CODE BEGIN DMA1_Channel4_IRQn 1 */

    /* USER CODE END DMA1_Channel4_IRQn 1 */
}

/**
* @brief This function handles DMA1 channel5 global interrupt.
*/
extern "C" void DMA1_Channel7_IRQHandler()
{
    /* USER CODE BEGIN DMA1_Channel5_IRQn 0 */

    /* USER CODE END DMA1_Channel5_IRQn 0 */
    HAL_DMA_IRQHandler(&hdma_usart2_tx);
    /* USER CODE BEGIN DMA1_Channel5_IRQn 1 */

    /* USER CODE END DMA1_Channel5_IRQn 1 */
}
#endif

#ifdef HUARN3
/**
* @brief This function handles DMA1 channel2 global interrupt.
*/
extern "C" void DMA1_Channel2_IRQHandler()
{
    /* USER CODE BEGIN DMA1_Channel2_IRQn 0 */

    /* USER CODE END DMA1_Channel2_IRQn 0 */
    HAL_DMA_IRQHandler(&hdma_usart3_tx);
    /* USER CODE BEGIN DMA1_Channel2_IRQn 1 */

    /* USER CODE END DMA1_Channel2_IRQn 1 */
}

/**
* @brief This function handles DMA1 channel3 global interrupt.
*/
extern "C" void DMA1_Channel3_IRQHandler()
{
    /* USER CODE BEGIN DMA1_Channel3_IRQn 0 */

    /* USER CODE END DMA1_Channel3_IRQn 0 */
    HAL_DMA_IRQHandler(&hdma_usart3_rx);
    /* USER CODE BEGIN DMA1_Channel3_IRQn 1 */

    /* USER CODE END DMA1_Channel3_IRQn 1 */
}
#endif

#ifdef FEATURE_CAN
extern "C" void CANx_RX_IRQHandler(void)
{
    HAL_CAN_IRQHandler(&CanHandle);
}

extern "C" void CANx_TX_IRQHandler(void)
{
    HAL_CAN_IRQHandler(&CanHandle);
}
#endif