Программный код бездатчиковой векторной системы управления для синхронного двигателя с возбуждением от постоянных магнитов, реализованный на ARM-процессоре с ядром Cortex-M4

Рабочие файлы: [Шаг 1: ШИМ-драйвер] [Шаг 2: ШИМ +АЦП +ПДП] [Шаг 3: Регуляторы тока]
[Шаг 4: Вкл. мК и Мост] [Шаг 5: ARM PMSM FOC] [по Табл.] [IQsin] [IQatan2]

Данный проект отличается от предыдущего лишь тем, что синтез напряжения статора выполняется по табличным значениям синусоидальной функции. При наличии 32-х разрядного микроконтроллера обосновать актуальность подобного решения быстродействием невозможно. Вычисление синуса и косинуса по двум членам ряда выполняется столь же быстро. А погрешность – 0.0045 – много меньше, чем отклонение противо-ЭДС любой реальной машины от синусоидальной формы.

Но. Если требуется привод, с высокими требованиями к равномерности вращения, или необходимо резервирование при обрыве фазы, то одно из возможных решений компенсации пульсаций момента связано модификацией системы управления, что требует наличия таблицы.

...

Приложение. Листинги файлов исходного кода бездатчиковой векторной системы управления для синхронного двигателя с возбуждением от постоянных магнитов, реализованной на ARM-процессоре с ядром Cortex-M4

Листинг 1. Файл mckit.h

/*************************************************************************
* File Name :   mckit.h
* Author :      Клиначев Николай Васильевич
* Version :     1.0.1
* Date :        20150707
* Description :
*      Ниже представлена база данных силовых инвертеров (MC KIT), которые
*      применялись при отладке программы векторной системы управления.
*      Для определения параметров инвертера, достаточно раскомментировать
*      определение его каталожного идентификатора в списке.
*************************************************************************/
#define PRIVATE_KIT_1
//#define BOOSTXL_DRV8301
//#define DRV8312_C2_KIT

#if defined PRIVATE_KIT_1       // Самодельный MC KIT

#define BASE_VOLTAGE    25.0    // Напряжение dc-шины, соответствующие шкале АЦП, (В)
#define BASE_CURRENT    25.0    // Ток стойки, соответствующий половине шкалы АЦП, (А)
#define CS_U0           0.4105  // смещение нуля датчика тока фазы U (доля шкалы АЦП)
#define CS_V0           0.4124  // смещение нуля датчика тока фазы V (доля шкалы АЦП)
#define CS_W0           0.5000  // смещение нуля датчика тока фазы W (доля шкалы АЦП)
#define CS_DC0          0.0000  // смещение нуля датчика тока DC-шины (доля шкалы АЦП)
#define R_DC            0.175   // внутреннее сопротивление источника питания, (Ом)
#define SW_H_POLARITY   0       // Активный уровень предрайверов верхних ключей
#define SW_L_POLARITY   0       // Активный уровень предрайверов  нижних ключей
#define SW_H_IDLESTATE  1       // Уровень сигнала для верхних ключей в IDLE-режиме
#define SW_L_IDLESTATE  1       // Уровень сигнала для  нижних ключей в IDLE-режиме
#define MCU_OPEN_DRAIN  1       // 1, если питание у MCU и у Моста раздельное
#define SVPWM_ON        0       // 0, если мала полоса ОУ (не работает на ХХ)

#elif defined BOOSTXL_DRV8301   // MC KIT производства Texas Instruments

#define BASE_VOLTAGE    26.3    // Напряжение dc-шины, соответствующие шкале АЦП, (В)
#define BASE_CURRENT    16.5    // Ток стойки, соответствующий половине шкалы АЦП, (А)
#define CS_U0           0.5294  // смещение нуля датчика тока фазы U (доля шкалы АЦП)
#define CS_V0           0.5330  // смещение нуля датчика тока фазы V (доля шкалы АЦП)
#define CS_W0           0.5383  // смещение нуля датчика тока фазы W (доля шкалы АЦП)
#define CS_DC0          0.0000  // смещение нуля датчика тока DC-шины (доля шкалы АЦП)
#define R_DC            0.175   // внутреннее сопротивление источника питания, (Ом)
// Необходимо допилить ШИМ-драйвер семейства процессоров С2000 фирмы ТИ для ...
//#define SW_H_POLARITY   1       // Активный уровень предрайверов верхних ключей
//#define SW_L_POLARITY   1       // Активный уровень предрайверов  нижних ключей
//#define SW_H_IDLESTATE  0       // Уровень сигнала для верхних ключей в IDLE-режиме
//#define SW_L_IDLESTATE  0       // Уровень сигнала для  нижних ключей в IDLE-режиме
//#define MCU_OPEN_DRAIN  0       // 1, если питание у MCU и у Моста раздельное
#define SVPWM_ON        0       // 0, если мала полоса ОУ (не работает на ХХ)

#elif defined DRV8312_C2_KIT    // MC KIT производства Texas Instruments

#define BASE_VOLTAGE    66.32   // Напряжение dc-шины, соответствующие шкале АЦП, (В)
#define BASE_CURRENT    8.6     // Ток стойки, соответствующий половине шкалы АЦП, (А)
// ...

#endif

Листинг 2. Файл motors.h

/*************************************************************************
* File Name :   motors.h
* Author :      Клиначев Николай Васильевич
* Version :     1.0.1
* Date :        20150707
* Description :
*      Ниже представлена база данных электродвигателей, для которых была
*      настроена программа, реализующая векторную систему управления.
*      В контексте определения паспорта двигателя (#if defined MOTOR_ID)
*      приводятся контекст инвертера, к которому двигатель подключался.
*      Один двигатель может быть подключен к разным инвертерам (MC KIT).
*      Поэтому, в контексте двигателя, списков параметров системы
*      управления может быть несколько.
*      Для определения параметров системы управления, достаточно
*      раскомментировать определение каталожного идентификатора
*      одного двигателя из списка.
*************************************************************************/
//#define MOTOR_V1
#define FL42BLS01
//#define DVM100_021
//#define DT4260_24_055_04
//#define DN4261_24_053           // близкий аналог DT4260-24-055-04
//#define FL42BLS02               // близкий аналог DT4260_24_055_04
//#define BLY172S_24V_4000        // близкий аналог DT4260-24-055-04
//#define ECMA_C10604GS           // Delta Electronics 0.4 кВт 3000 об/мин
//#define MOTOR_KOLESO

/*************************************************************************
* SAFE_I – это такая уставка по току, при которой ротор (маховик),
*          не может накапливать энергию быстро, и при которой, система
*          управления не может быстро сбросить энергию в источник.
*          Лабораторные источники постоянного тока не поддерживают реверс
*          энергии. Если машина переходит в генераторный режим или в режим
*          рекуперативного торможения (при реверсе), то потенциал шины
*          постоянного тока увеличивается, у источника срабатывает защита.
*          В типовом случае, силовая часть MC_KIT не имеет балластного
*          резистора.
* BASE_  - это префикс, для физических величин, которые используются для
*          приведения координат системы управления к относительным
*          величинам, (pu), [-1.0, +1.0]. Например, при уставке SAFE_I,
*          абсолютная величина тока равна: SAFE_I * BASE_CURRENT
* BASE_FREQ
*          К одному инвертеру, с целью "покрутить", можно подключить очень
*          разные двигатели. Параметры могут различаться в 10 и боле раз.
*          Ниже приведены настройки для трех двигателей, один на 4000 об/мин,
*          55 Вт, другой на 750 об/мин, 100 Вт, третий двигатель на 30 кВт
*          и 300 В. В таких случаях параметр BASE_FREQ можно выбрать не по
*          паспорту, а так, чтобы при уставке скорости равной 1, противо-ЭДС
*          не превысила напряжение dc-шины.
* KE
*          Внимание! Для определения константы продиво-ЭДС используют разные
*          формулы. Здесь константа противо-ЭДС связывает номинальную угловую
*          скорость вала (рад/с) и амплитуду противо-ЭДС фазы двигателя, чьи
*          секции соединены в звезду. Грубая транспортировка машины может
*          быть причиной снижения Ke до 5%. Для тюнинга Ke следует задать
*          скорость вала, вычислить соответствующую ей частоту тока в статоре
*          и добиться совпадения. В регуляторе скорости должен быть включен
*          интегрирующий канал.
*
* Порядок настройки регулятора скорости:
*          1. Комментируем строку   #define KP_S
*          2. Определяем            #define TI_S   0
*          3. Проверяем качество работы, подстраиваем значение: pid_spd.Kp
*          4. Фиксируем             #define KP_S   (pid_spd.Kp / (1 << GL_Q))
*          5. Комментируем строку   #define TI_S
*          6. Проверяем качество работы, подстраиваем значение: pid_spd.Ki
*          7. Фиксируем             #define TI_S   (KP_S / KI_S * dT)
*
*          Если Tм > 3 * Tя, то настройки регулятора, которые вычисляет
*          программа должны подойти. Привод сразу должен работать качественно.
*          Переопределение KP_S и TI_S может потребоваться, если у привода
*          меняется приведенный к валу момент инерции.
*
* Начальное значение WF = 30
*          Тюнинг выполнять без нагрузки. При уставке скорости равной 0.25.
*          Отслеживать осциллограммы тока и угла. Значение сопрягающей частоты
*          необходимо уменьшать до тех пор, пока шумы тока не начнут искажать
*          осциллограмму угла. Значение WF = 0.5 .. 5 признается хорошим.
*          http://model.exponenta.ru/k2/Jigrein/JS/fwlink.htm#B0BB   ф-ла (9)
*
* Начальное значение KK_C = 1
*          Тюнинг выполняется в режиме программного реверса. Значение
*          увеличивают, если есть перерегулирование на участке рекуперации
*          энергии. Редко, но встречаются машины, у которых коэффициент
*          усиления контура тока следует увеличить до 4
*************************************************************************/

#if defined DVM100_021
/*** www.kbm36.ru ********************************************************
*                                                                Таблица 1
+------------------+---+----+--------+-------+----------+----------------+
|     Тип СДПМ     | m | Zp |   Ke   |   J   | L_ф  R_ф | I_фm  Mн   Pн  |
|                  | Y        В/рад/с  kg*m^2   mH   Om     A   Nm    W  |
|    ДВМ100.021    | 3   13   0.618     2e-3   2.5  1.28  2.5  2.31  45  |
+------------------+---+----+--------+-------+----------+----------------+
Kt = 1.5 * Ke,   Mн = I_фm * Kt,   omega_н = Pн / Mн,   Eфm = omega_н * Ke

*                                                                Таблица 2
+------------------+--------+--------+-------+----------+----------------+
|     Экв. ДПТ     |   E_я  |   KФ   |   J   | L_я  R_я |  Iн   Mн   Pн  |
|     условие:     |    В     В/рад/с  kg*m^2   mH   Om     A   Nm    W  |
|     Eя = Eфm     |  15.6    0.618     2e-3  1.67 0.853  3.75 2.31  45  |
+------------------+--------+--------+-------+----------+----------------+
*************************************************************************/
#define BASE_FREQ       39      // Максимальная частота фазных токов, (Гц)
#define POLES           26      // Число полюсов = 2 * Zp
#define KE      0.617           // Y Константа противо-ЭДС секции, (В/(рад/сек))
#define RS      (2.615 / 2)     // Y активное сопротивление секции статора, (Ом)
#define LS      (0.002 / 2)     // Y индуктивность секции статора, (Гн)
#define IS      2.5             // Y максимальный ток секции статора, (А)
#define JPR     (2e-3 + 0)      // Приведенный к ротору момент инерции, (кг * м^2)
//
// Параметры векторной системы управления для данного СДПМ и инвертера
//
#if defined PRIVATE_KIT_1       // Самодельный MC KIT
#define SAFE_S  0.5             // 0 или Уставка по скорости, [-1.0, 1.0]
#define SAFE_I  0.04            // Безопасная уставка по току, [0.0, 1.0]
#define KK_C    0.8             // Коэффициент усиления контура тока 0.5 .. 4
#define KP_S    0.15            // Коэффициент П-канала регулятора скорости
#define TI_S    8e-3            // 0 или Постоянная времени И-канала рег. скорости
#define WL      5               // Весовой коэффициент предела ошибки рег. скорости
#define WF      30              // Сопр. частота фильтров в наб. потокосцеплений, (рад/с)
#define START_F 6               // Частота фазных токов в синхронном режиме запуска, (Гц)
#elif defined BOOSTXL_DRV8301   // MC KIT производства Texas Instruments
// ...
#elif defined DRV8312_C2_KIT    // MC KIT производства Texas Instruments
// ...
#endif

#elif defined(DT4260_24_055_04) || defined(DN4261_24_053) \
     || defined(FL42BLS02) || defined(BLY172S_24V_4000)
/*************************************************************************
*                                                                Таблица 1
+------------------+---+----+--------+-------+----------+----------------+
|     Тип СДПМ     | m | Zp |   Ke   |   J   | L_ф  R_ф | I_фm  Mн   Pн  |
|                  | Y        В/рад/с  kg*m^2   mH   Om     A   Nm    W  |
| DT4260-24-055-04 | 3    4   0.0224    4e-6   0.6 0.483  3.9  .131  55  |
+------------------+---+----+--------+-------+----------+----------------+
Kt = 1.5 * Ke,   Mн = I_фm * Kt,   omega_н = Pн / Mн,   Eфm = omega_н * Ke

*                                                                Таблица 2
+------------------+--------+--------+-------+----------+----------------+
|     Экв. ДПТ     |   E_я  |   KФ   |   J   | L_я  R_я |  Iн   Mн   Pн  |
|     условие:     |    В     В/рад/с  kg*m^2   mH   Om     A   Nm    W  |
|     Eя = Eфm     |  9.41    0.0224    4e-6   0.4 0.322  5.85   -    -  |
+------------------+--------+--------+-------+----------+----------------+
Другие названия этой машины: DN4261-24-053, FL42BLS02, BLY172S-24V-4000
*************************************************************************/
#define BASE_FREQ       200     // Максимальная частота фазных токов, (Гц)
#define POLES           8       // Число полюсов = 2 * Zp
#define KE      0.02245         // Y Константа противо-ЭДС секции, (В/(рад/сек))
#define RS      (0.965 / 2)     // Y активное сопротивление секции статора, (Ом)
#define LS      (0.0012 / 2)    // Y индуктивность секции статора, (Гн)
#define IS      3.9             // Y максимальный ток секции статора, (А)
#define JPR     (4e-6 + 0e-6)   // Приведенный к ротору момент инерции, (кг * м^2)
//
// Параметры векторной системы управления для данного СДПМ и инвертера
//
#if defined PRIVATE_KIT_1       // Самодельный MC KIT
#define SAFE_S  0.5             // 0 или Уставка по скорости, [-1.0, 1.0]
#define SAFE_I  0.05            // Безопасная уставка по току, [0.0, 1.0]
#define KK_C    0.8             // Коэффициент усиления контура тока 0.5 .. 4
#define KP_S    0.4             // Коэффициент П-канала регулятора скорости
#define TI_S    0.006           // 0 или Постоянная времени И-канала рег. скорости
#define WL      2800            // Весовой коэффициент предела ошибки рег. скорости
#define WF      45              // Сопр. частота фильтров в наб. потокосцеплений, (рад/с)
#define START_F 9               // Частота фазных токов в синхронном режиме запуска, (Гц)
#elif defined BOOSTXL_DRV8301   // MC KIT производства Texas Instruments
#define SAFE_S  0.5             // 0 или Уставка по скорости, [-1.0, 1.0]
#define SAFE_I  0.05            // Безопасная уставка по току, [0.0, 1.0]
#define KK_C    0.8             // Коэффициент усиления контура тока 0.5 .. 4
#define KP_S    0.4             // Коэффициент П-канала регулятора скорости
#define TI_S    0.006           // 0 или Постоянная времени И-канала рег. скорости
#define WL      2800            // Весовой коэффициент предела ошибки рег. скорости
#define WF      45              // Сопр. частота фильтров в наб. потокосцеплений, (рад/с)
#define START_F 9               // Частота фазных токов в синхронном режиме запуска, (Гц)
#elif defined DRV8312_C2_KIT    // MC KIT производства Texas Instruments
// ...
#endif

#elif defined FL42BLS01
/*************************************************************************
*                                                                Таблица 1
+------------------+---+----+--------+-------+----------+----------------+
|     Тип СДПМ     | m | Zp |   Ke   |   J   | L_ф  R_ф | I_фm  Mн   Pн  |
|                  | Y        В/рад/с  kg*m^2   mH   Om     A   Nm    W  |
|     FL42BLS01    | 3    4   0.0185   2.4e-6  1.3  0.9   1.5  .062  26  |
+------------------+---+----+--------+-------+----------+----------------+
Kt = 1.5 * Ke,   Mн = I_фm * Kt,   omega_н = Pн / Mн,   Eфm = omega_н * Ke
*************************************************************************/
#define BASE_FREQ       250     // Максимальная частота фазных токов, (Гц)
#define POLES           8       // Число полюсов = 2 * Zp
#define KE      0.0185          // Y Константа противо-ЭДС секции, (В/(рад/сек))
#define RS      (1.8 / 2)       // Y активное сопротивление секции статора, (Ом)
#define LS      (0.0026 / 2)    // Y индуктивность секции статора, (Гн)
#define IS      1.5             // Y максимальный ток секции статора, (А)
#define JPR     (2.4e-6 * 1)    // Приведенный к ротору момент инерции, (кг * м^2)
//
// Параметры векторной системы управления для данного СДПМ и инвертера
//
#if defined PRIVATE_KIT_1       // Самодельный MC KIT
#define SAFE_S  0.4             // 0 или Уставка по скорости, [-1.0, 1.0]
#define SAFE_I  0.04            // Безопасная уставка по току, [0.0, 1.0]
#define KK_C    1.0             // Коэффициент усиления контура тока 0.5 .. 4
#define KP_S    0.15            // Коэффициент П-канала регулятора скорости
#define TI_S    0.008           // 0 или Постоянная времени И-канала рег. скорости
#define WL      2800            // Весовой коэффициент предела ошибки рег. скорости
#define WF      45              // Сопр. частота фильтров в наб. потокосцеплений, (рад/с)
#define START_F 20              // Частота фазных токов в синхронном режиме запуска, (Гц)
#elif defined BOOSTXL_DRV8301   // MC KIT производства Texas Instruments
// ...
#elif defined DRV8312_C2_KIT    // MC KIT производства Texas Instruments
// ...
#endif

#elif defined ECMA_C10604GS
/*************************************************************************
*                                                                Таблица 1
+------------------+---+----+--------+-------+----------+----------------+
|     Тип СДПМ     | m | Zp |   Ke   |   J   | L_ф  R_ф | I_фm  Mн   Pн  |
|                  | Y        В/рад/с  kg*m^2   mH   Om     A   Nm   kW  |
|   ECMA_C10604GS  | 3    5    0.25   27.7e-6  5.0  1.7   3.7  1.27  0.4 |
+------------------+---+----+--------+-------+----------+----------------+
Kt = 1.5 * Ke,   Mн = I_фm * Kt,   omega_н = Pн / Mн,   Eфm = omega_н * Ke
*************************************************************************/
#define BASE_FREQ       250     // Максимальная частота фазных токов, (Гц)
#define POLES           10      // Число полюсов = 2 * Zp
#define KE      0.25            // Y Константа противо-ЭДС секции, (В/(рад/сек))
#define RS      (3.4 / 2)       // Y активное сопротивление секции статора, (Ом)
#define LS      (0.01 / 2)      // Y индуктивность секции статора, (Гн)
#define IS      3.7             // Y максимальный ток секции статора, (А)
#define JPR     (27.7e-6 * 1)   // Приведенный к ротору момент инерции, (кг * м^2)
//
// Параметры векторной системы управления для данного СДПМ и инвертера
//
#if defined PRIVATE_KIT_1       // Самодельный MC KIT
#define SAFE_S  0.1             // 0 или Уставка по скорости, [-1.0, 1.0]
#define SAFE_I  0.04            // Безопасная уставка по току, [0.0, 1.0]
#define KK_C    1.0             // Коэффициент усиления контура тока 0.5 .. 4
#define KP_S    2               // Коэффициент П-канала регулятора скорости
#define TI_S    0.06            // 0 или Постоянная времени И-канала рег. скорости
#define WL      6000            // Весовой коэффициент предела ошибки рег. скорости
#define WF      25              // Сопр. частота фильтров в наб. потокосцеплений, (рад/с)
#define START_F 6               // Частота фазных токов в синхронном режиме запуска, (Гц)
#elif defined BOOSTXL_DRV8301   // MC KIT производства Texas Instruments
// ...
#elif defined DRV8312_C2_KIT    // MC KIT производства Texas Instruments
// ...
#endif

#elif defined MOTOR_KOLESO
/*************************************************************************
*                                                                Таблица 1
+------------------+---+----+--------+-------+----------+----------------+
|     Тип СДПМ     | m | Zp |   Ke   |   J   | L_ф  R_ф | I_фm  Mн   Pн  |
|                  | Y        В/рад/с  kg*m^2   mH   Om     A   Nm   kW  |
|   MOTOR_KOLESO   | 3   14    0.87     0.29   8.25 0.75   20   26       |
+------------------+---+----+--------+-------+----------+----------------+
Kt = 1.5 * Ke,   Mн = I_фm * Kt,   omega_н = Pн / Mн,   Eфm = omega_н * Ke
*************************************************************************/
#define BASE_FREQ      100      // Максимальная частота фазных токов, (Гц)
#define POLES           28      // Число полюсов = 2 * Zp
#define KE      0.87            // Y Константа противо-ЭДС секции, (В/(рад/сек))
#define RS      (1.5 / 2)       // Y активное сопротивление секции статора, (Ом)
#define LS      (0.0165 / 2)    // Y индуктивность секции статора, (Гн)
#define IS      20              // Y максимальный ток секции статора, (А)
#define JPR     0.29            // Приведенный к ротору момент инерции, (кг * м^2)
//
// Параметры векторной системы управления для данного СДПМ и инвертера
//
#if defined PRIVATE_KIT_1       // Самодельный MC KIT
#define SAFE_S  0.2             // 0 или Уставка по скорости, [-1.0, 1.0]
#define SAFE_I  0.1             // Безопасная уставка по току, [0.0, 1.0]
#define KK_C    1.0             // Коэффициент усиления контура тока 0.5 .. 4
#define KP_S    8               // Коэффициент П-канала регулятора скорости
#define TI_S    0.06            // 0 или Постоянная времени И-канала рег. скорости
#define WL      6000            // Весовой коэффициент предела ошибки рег. скорости
#define WF      20              // Сопр. частота фильтров в наб. потокосцеплений, (рад/с)
#define START_F 1               // Частота фазных токов в синхронном режиме запуска, (Гц)
#elif defined BOOSTXL_DRV8301   // MC KIT производства Texas Instruments
#define SAFE_S  0.1             // 0 или Уставка по скорости, [-1.0, 1.0]
#define SAFE_I  0.1             // Безопасная уставка по току, [0.0, 1.0]
#define KK_C    1.0             // Коэффициент усиления контура тока 0.5 .. 4
#define KP_S    4               // Коэффициент П-канала регулятора скорости
#define TI_S    0.06            // 0 или Постоянная времени И-канала рег. скорости
#define WL      6000            // Весовой коэффициент предела ошибки рег. скорости
#define WF      20              // Сопр. частота фильтров в наб. потокосцеплений, (рад/с)
#define START_F 1               // Частота фазных токов в синхронном режиме запуска, (Гц)
#elif defined DRV8312_C2_KIT    // MC KIT производства Texas Instruments
// ...
#endif

#elif defined MOTOR_V1
#define BASE_FREQ       50      // Максимальная частота фазных токов, (Гц)
#define POLES           34      // Число полюсов = 2 * Zp
#define KE      2.66            // Y Константа противо-ЭДС секции, (В/(рад/сек))
#define RS      (5e-3 * 9 +0.05)// Y активное сопротивление секции статора, (Ом)
#define LS      (60e-6 * 9)     // Y индуктивность секции статора, (Гн)
#define IS      400.0           // Y максимальный ток секции статора, (А)
#define JPR     (0.336 + 0)     // Приведенный к ротору момент инерции, (кг * м^2)
//
// Параметры векторной системы управления для данного СДПМ и инвертера
//
#if defined PRIVATE_KIT_1       // Самодельный MC KIT
#define SAFE_S  0.2             // 0 или Уставка по скорости, [-1.0, 1.0]
#define SAFE_I  0.07            // Безопасная уставка по току, [0.0, 1.0]
#define KK_C    1.0             // Коэффициент усиления контура тока 0.5 .. 4
#define KP_S    2.4             // Коэффициент П-канала регулятора скорости
#define TI_S    0.08            // 0 или Постоянная времени И-канала рег. скорости
#define WL      3000            // Весовой коэффициент предела ошибки рег. скорости
#define WF      5               // Сопр. частота фильтров в наб. потокосцеплений, (рад/с)
#define START_F 1               // Частота фазных токов в синхронном режиме запуска, (Гц)
#elif defined BOOSTXL_DRV8301   // MC KIT производства Texas Instruments
// ...
#elif defined DRV8312_C2_KIT    // MC KIT производства Texas Instruments
// ...
#endif

#endif

Листинг 3. Файл mclib.h

/******************************************************************************
* File Name :   mclib.h
* Author :      Клиначев Николай Васильевич
* Version :     1.1.0
* Date :        20150707
* Description : Библиотеки IQ-Math library и Motor Control library
*       Библиотека математических преобразователей, необходимых
*       при реализации цифровых систем управления электропривода
*       и необходимые макросы целочисленной арифметики (IQ-математика)
*******************************************************************************/
/* Защита от рекурсивных подключений библиотеки-------------------------------*/
#ifndef __MCLIB_H
#define __MCLIB_H

/* Includes ------------------------------------------------------------------*/

/* Exported types ------------------------------------------------------------*/
// ******************************************************************************* //
typedef long    _iq;    // это _iq24, _iq22 или _iq20 в зависимости от GL_Q

// ******************************************************************************* //
// Указатель на угловое положение вала, Запускающий Интегратор и пр.
typedef struct {
    _iq *yPhi;      // Input: Указатель на угловое положение вала, [0.0, 1.0]
    _iq Sine;       // Output: (8000..FFFF-0000..7FFF) << (GL_Q - 15)
    _iq Cosine;     // Output: (8000..FFFF-0000..7FFF) << (GL_Q - 15)
    _iq gS;         // Output: Уставка Скорости (Start mode), [-1.0, 1.0]
    _iq uPhi;       // Output: угол Интегратора Запуска (Start mode), [0.0, 1.0]
}
ROTOR_TypeDef;

// ******************************************************************************* //
// Инверсный преобразователь Парка
typedef struct {
    _iq *d;         // Input: Вещественная составляющая координаты (по оси d)
    _iq *q;         // Input: Мнимая составляющая координаты (по оси q)
    _iq *Sine;      // Input: Указатель на опорный синусоидальный сигнал (pu)
    _iq *Cosine;    // Input: Указатель на опорный косинусоидальный сигнал (pu)
    _iq a;          // Output: Alpha-фаза вращающейся координаты (сдвиг - 0 градусов)
    _iq b;          // Output: Beta-фаза вращающейся координаты (сдвиг - 90 градусов)
}
PIPARK_TypeDef;

// ******************************************************************************* //
// Преобразователь числа фаз (двухфазной системы в трехфазную)
typedef struct {
    _iq *a;         // Input: Alpha-фаза вращающейся координаты (сдвиг - 0 градусов)
    _iq *b;         // Input: Beta-фаза вращающейся координаты (сдвиг - 90 градусов)
    _iq  u;         // Output: U-фаза трехфазной системы UVW
    _iq  v;         // Output: V-фаза трехфазной системы UVW
    _iq  w;         // Output: W-фаза трехфазной системы UVW
}
PICLARKE_TypeDef;

// ******************************************************************************* //
// Преобразователь числа фаз (трехфазной системы в двухфазную)
typedef struct
{
    _iq *u;         // Input:  Ток фазы "U" статора (UVW - индексы трехфазной системы)
    _iq *v;         // Input:  Ток фазы "V" статора (UVW - индексы трехфазной системы)
    _iq *w;         // Input:  Ток фазы "W" статора (UVW - индексы трехфазной системы)
    _iq a;          // Output: Ток фазы "a" статора ("Alpha/Beta" - 2-х фазная система)
    _iq b;          // Output: Ток фазы "b" статора ("Alpha/Beta" - 2-х фазная система)
} PCLARKE_TypeDef;

// ******************************************************************************* //
// Преобразователь Парка
typedef struct
{
    _iq *a;         // Input: Alpha-фаза вращающейся координаты (сдвиг - 0 градусов)
    _iq *b;         // Input: Beta-фаза вращающейся координаты (сдвиг - 90 градусов)
    _iq *Sine;      // Input: Указатель на опорный синусоидальный сигнал (pu)
    _iq *Cosine;    // Input: Указатель на опорный косинусоидальный сигнал (pu)
    _iq d;          // Output: Вещественная составляющая координаты (по оси d)
    _iq q;          // Output: Мнимая составляющая координаты (по оси q)
} PPARK_TypeDef;

// ******************************************************************************* //
// ПИ-регулятор контура тока (без канала комбинированного управления)
typedef struct
{
    _iq *gI;        // Input: Указатель на уставку (задание) для РТ, [-1.0, 1.0]
    _iq *yI;        // Input: Указатель на сигнал ОС с датчика тока, [0.0, 1.0]
    _iq zo;         // sPrivate: Выход регистра задержки интегратора
    _iq uV;         // Output: Управляющее значение для ШИМ, [-1.0, 1.0]
    _iq Kp;         // Const: Коэффициент усиления пропорционального канала PI-регулятора
    _iq Ki;         // Const: Коэффициент усиления интегрирующего канала PI-регулятора
} DQ_PID_TypeDef;

#define DQ_PID_MACRO(ir)                                                    \
{                                                                           \
    _iq ir_xI;                             /* Ошибка регулятора тока      */\
    _iq ir_zi;                             /* Вход регистра задержки      */\
    ir_xI  = *ir.gI - *ir.yI;              /* Ошибка регулятора           */\
    ir.uV  = _IQmpy(ir_xI, ir.Kp);         /* РT:Kp                       */\
    ir.uV += ir.zo;                        /* ERK11                       */\
    ir_zi  = _IQmpy(ir_xI, ir.Ki);         /* РT:Ki                       */\
    ir_zi += ir.zo;                        /* 1/s: Значение на входе      */\
    /* ---------------------------------------------------                */\
    ir.zo = ir_zi;                                                          \
}

// ******************************************************************************* //
// Наблюдатель потокосцеплений
typedef struct
{
    _iq *Vm;        // Input: Указатель max амплитуду фазного напряжения, (pu)
    _iq *Va;        // Input: Alpha-фаза вращающейся координаты (сдвиг - 0 градусов)
    _iq *Vb;        // Input: Beta-фаза вращающейся координаты (сдвиг - 90 градусов)
    _iq *Ia;        // Input: Alpha-фаза вращающейся координаты (сдвиг - 0 градусов)
    _iq *Ib;        // Input: Beta-фаза вращающейся координаты (сдвиг - 90 градусов)
    _iq zoA;        // sPrivate: Выход регистра задержки
    _iq zoB;        // sPrivate: Выход регистра задержки
    _iq a;          // Output: Потокосцепление Alpha-фазы
    _iq b;          // Output: Потокосцепление Beta-фазы
    _iq phi;        // Output: Угловое положение ротора, [0.0, 1.0]. (Регистр задержки)
    _iq Rs;         // Const: Активное сопротивление обмотки статора
    _iq Ls;         // Const: Индуктивность рассеяния обмотки статора
    _iq wf;         // Const: сопрягающая частота фильтра (1 / Tф = Speed Zp / 30..20 / 10)
} PSI_ESTIMATOR_TypeDef;

// ******************************************************************************* //
// Наблюдатель Скорости вала СДПМ
typedef struct
{
    _iq *Vm;  // Input: Указатель max амплитуду фазного напряжения (pu), [0.0, 1.0]
    _iq *Vq;  // Input: Указатель на текущую скважность ШИМ (Duty) по моменту, [-1.0, +1.0]
    _iq *Iq;  // Input: Указатель на сигнал ОС с датчика тока, [-1.0, +1.0]
    _iq zo;   // sPrivate: Выход регистра задержки апериодического звена
    _iq yS;   // Output: Скорость вала = (Uq / (1 + Te s) - Iq * Rs) / Ke, [-1.0, +1.0]
    _iq TS_div_TE;  // Const: Произведение сопрягающей частоты (1/Te) и шага
    _iq Rs_mul_BC;  // Const: Произведение сопротивления статора и предела шкалы АЦП тока
    _iq EMF2SPD_PU; // Const: (1 / KE) * (1 / BASE_SPEED), [-1.0, +1.0]
} SPD_ESTIMATOR_TypeDef;

// ******************************************************************************* //
// Измеряемые величины на силовом модуле
typedef struct
{
    _iq Vdc;        // Output: Измеренное напряжение DC-шины (сигнал ОС), [0.0, 1.0]
    _iq Idc;        // Output: Измеренный ток DC-шины (сигнал ОС),        [-1.0, 1.0]
    _iq uIs;        // Output: Измеренный ток фазы "U" статора,           [-1.0, 1.0]
    _iq vIs;        // Output: Измеренный ток фазы "V" статора,           [-1.0, 1.0]
    _iq uIs0;       // Output: Смещение нуля датчика тока фазы "U",       0.5 +/-10 %
    _iq vIs0;       // Output: Смещение нуля датчика тока фазы "V",       0.5 +/-10 %
    _iq Vm;         // Output: Максимальная амплитуда фазного напряжения, [0.0, 1.0]
} MCKIT_TypeDef;

typedef struct
{
    _iq *gI;  // Input: Указатель на предел уставки по току (Уставка Момента)
    _iq *gS;  // Input: Указатель на Уставку Скорости (задание для РС), [-1.0, 1.0]
    _iq *yS;  // Input: Указатель на сигнал ОС для РС, [-1.0, 1.0]
    _iq zo;   // sPrivate: Выход регистра задержки интегратора
    _iq uS;   // Output: Уставка (задание) для РТ, [-1.0, 1.0] (с ограничением)
    _iq Kp;   // Const: Коэффициент усиления пропорционального канала PI-регулятора
    _iq Ki;   // Const: Коэффициент усиления интегрирующего канала PI-регулятора
    _iq Lx;   // Const: Пределы ограничения сигнала на входе И-канала
    _iq Lu;   // Const: Пределы ограничения сигнала на выходе регулятора
} SPD_PID_TypeDef;

#define SPD_PID_MACRO(sr)                                                            \
{                                                                                    \
    _iq sr_xS;                                    /* Ошибка Регулятора Скорости    */\
    _iq sr_zi;                                    /* Вход регистра задержки        */\
    sr_xS = *sr.gS - *sr.yS;                      /* Ошибка регулятора             */\
    sr.uS = _IQmpy(sr_xS, sr.Kp);                 /* РС:Kp                         */\
    sr_zi = _IQmpy(sr_xS, sr.Ki);                 /* РС:Ki                         */\
    sr_zi = sr_zi > sr.Lx ? sr.Lx : sr_zi < -sr.Lx ? -sr.Lx : sr_zi;  /* xS_Limit  */\
    sr.uS += sr.zo;                               /* ERK11                         */\
    sr.uS = sr.uS > sr.Lu ? sr.Lu : sr.uS < -sr.Lu ? -sr.Lu : sr.uS;  /* uS_Limit  */\
    sr_zi += sr.zo;                               /* 1/s: Значение на входе        */\
    sr_zi = sr_zi > sr.Lu ? sr.Lu : sr_zi < -sr.Lu ? -sr.Lu : sr_zi;  /* 1/s_Limit */\
    /* ---------------------------------------------------                         */\
    sr.zo = sr_zi;                                                                   \
}

/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
#ifndef GL_Q            // Точность типа данных для IQ-вычислений: 20 22 24
#define GL_Q 24         // Определять здесь. В motors.h много мороки
#endif
#if GL_Q == 24          // _iq24:   -128 .. +127.999 999 940    0.000 000 060
#define _IQ(A)          (long) ((A) * 16777216.0L)      // _IQ24
#elif GL_Q == 22        // _iq22:   -512 .. +511.999 999 76     0.000 000 24
#define _IQ(A)          (long) ((A) * 4194304.0L)       // _IQ22
#elif GL_Q == 20        // _iq20:  -2048 .. 2047.999 999 0      0.000 001
#define _IQ(A)          (long) ((A) * 1048576.0L)       // _IQ20
#else
#error "PMSM: Wrong IQ type. Use: IQ24 or IQ22 or IQ20"
#endif                  // Если в motors.h, то так надо все IQ-макросы

// _iq15:    -1 .. +1 - 0.000030517578125
#define _IQ15(A)        (s16) ((A) * 32768.0L)
/*******************************************************************************
* Description :  Макрос преобразования биполярного значения
*                к униполярному с масштабированием
* Input : x    - число в формате _Q15: 8000..FFFF-0000..7FFF
* Param : BASE - униполярное число: 0000..FFFF, на 1 большее MAX значения
* Return :       0x0000 .. (base - 1)
*/
#define _Q15toBASE(x, BASE) ((unsigned long) ((  signed short) (x) + 0x8000)   \
                                           * ((unsigned short) (BASE)) >> 16)
#define _IQtoIQ15(A)        ((long) (A) >> (GL_Q - 15))
#define _IQdiv2(A)          ((A) >> 1)
#define _IQmpy(A, B)        (long) (((long long) (A) * (long long) (B)) >> GL_Q)
#define _IQmpy2(A)          ((A) << 1)

/* Exported functions ------------------------------------------------------- */
void ROTOR_GetTrigonometic(ROTOR_TypeDef* Rotor);
_iq _IQatan2(_iq A, _iq B);

#endif /* __MCLIB_H */

Листинг 4. Файл mclib.c

/******************************************************************************
* File Name :   mclib.c
* Author :      Клиначев Николай Васильевич
* Version :     1.1.0
* Date :        20150707
* Description : Библиотеки IQ-Math library и Motor Control library
*       Библиотека математических преобразователей, необходимых
*       при реализации цифровых систем управления электропривода
*       и необходимые макросы целочисленной арифметики (IQ-математика)
*******************************************************************************/
#include "mclib.h"

#define TABSIZE 2048
#define _PU15toBASE(x, BASE) ((x) * (BASE) >> 15)
#define _IQ15toIQ(A) ((long) (A) << (GL_Q - 15))
/**
* Таблица значений выборок функции sin
* формат s16 (_Q15: 8000..FFFF-0000..7FFF)
*/
const s16 sinTbl[TABSIZE] = {
    0x0000, 0x0064, 0x00c9, 0x012d, 0x0192, 0x01f6, 0x025b, 0x02bf,
    0x0324, 0x0388, 0x03ed, 0x0451, 0x04b6, 0x051a, 0x057e, 0x05e3,
    0x0647, 0x06ac, 0x0710, 0x0774, 0x07d9, 0x083d, 0x08a1, 0x0906,
    0x096a, 0x09ce, 0x0a32, 0x0a97, 0x0afb, 0x0b5f, 0x0bc3, 0x0c27,
    0x0c8b, 0x0cef, 0x0d53, 0x0db7, 0x0e1b, 0x0e7f, 0x0ee3, 0x0f47,
    0x0fab, 0x100e, 0x1072, 0x10d6, 0x1139, 0x119d, 0x1201, 0x1264,
    0x12c7, 0x132b, 0x138e, 0x13f2, 0x1455, 0x14b8, 0x151b, 0x157e,
    0x15e1, 0x1645, 0x16a7, 0x170a, 0x176d, 0x17d0, 0x1833, 0x1895,
    0x18f8, 0x195b, 0x19bd, 0x1a20, 0x1a82, 0x1ae4, 0x1b47, 0x1ba9,
    0x1c0b, 0x1c6d, 0x1ccf, 0x1d31, 0x1d93, 0x1df4, 0x1e56, 0x1eb8,
    0x1f19, 0x1f7b, 0x1fdc, 0x203e, 0x209f, 0x2100, 0x2161, 0x21c2,
    0x2223, 0x2284, 0x22e5, 0x2345, 0x23a6, 0x2406, 0x2467, 0x24c7,
    0x2527, 0x2588, 0x25e8, 0x2648, 0x26a7, 0x2707, 0x2767, 0x27c7,
    0x2826, 0x2885, 0x28e5, 0x2944, 0x29a3, 0x2a02, 0x2a61, 0x2ac0,
    0x2b1f, 0x2b7d, 0x2bdc, 0x2c3a, 0x2c98, 0x2cf6, 0x2d55, 0x2db3,
    0x2e10, 0x2e6e, 0x2ecc, 0x2f29, 0x2f87, 0x2fe4, 0x3041, 0x309e,
    0x30fb, 0x3158, 0x31b5, 0x3211, 0x326e, 0x32ca, 0x3326, 0x3382,
    0x33de, 0x343a, 0x3496, 0x34f1, 0x354d, 0x35a8, 0x3603, 0x365e,
    0x36b9, 0x3714, 0x376f, 0x37c9, 0x3824, 0x387e, 0x38d8, 0x3932,
    0x398c, 0x39e6, 0x3a3f, 0x3a99, 0x3af2, 0x3b4b, 0x3ba4, 0x3bfd,
    0x3c56, 0x3caf, 0x3d07, 0x3d5f, 0x3db7, 0x3e0f, 0x3e67, 0x3ebf,
    0x3f17, 0x3f6e, 0x3fc5, 0x401c, 0x4073, 0x40ca, 0x4121, 0x4177,
    0x41cd, 0x4224, 0x4279, 0x42cf, 0x4325, 0x437a, 0x43d0, 0x4425,
    0x447a, 0x44cf, 0x4524, 0x4578, 0x45cc, 0x4621, 0x4675, 0x46c8,
    0x471c, 0x4770, 0x47c3, 0x4816, 0x4869, 0x48bc, 0x490f, 0x4961,
    0x49b3, 0x4a05, 0x4a57, 0x4aa9, 0x4afb, 0x4b4c, 0x4b9d, 0x4bee,
    0x4c3f, 0x4c90, 0x4ce0, 0x4d30, 0x4d81, 0x4dd0, 0x4e20, 0x4e70,
    0x4ebf, 0x4f0e, 0x4f5d, 0x4fac, 0x4ffb, 0x5049, 0x5097, 0x50e5,
    0x5133, 0x5181, 0x51ce, 0x521b, 0x5268, 0x52b5, 0x5302, 0x534e,
    0x539a, 0x53e6, 0x5432, 0x547e, 0x54c9, 0x5514, 0x555f, 0x55aa,
    0x55f5, 0x563f, 0x5689, 0x56d3, 0x571d, 0x5767, 0x57b0, 0x57f9,
    0x5842, 0x588b, 0x58d3, 0x591b, 0x5964, 0x59ab, 0x59f3, 0x5a3a,
    0x5a82, 0x5ac9, 0x5b0f, 0x5b56, 0x5b9c, 0x5be2, 0x5c28, 0x5c6e,
    0x5cb3, 0x5cf8, 0x5d3d, 0x5d82, 0x5dc7, 0x5e0b, 0x5e4f, 0x5e93,
    0x5ed7, 0x5f1a, 0x5f5d, 0x5fa0, 0x5fe3, 0x6025, 0x6068, 0x60aa,
    0x60eb, 0x612d, 0x616e, 0x61af, 0x61f0, 0x6231, 0x6271, 0x62b1,
    0x62f1, 0x6331, 0x6370, 0x63af, 0x63ee, 0x642d, 0x646b, 0x64aa,
    0x64e8, 0x6525, 0x6563, 0x65a0, 0x65dd, 0x661a, 0x6656, 0x6693,
    0x66cf, 0x670a, 0x6746, 0x6781, 0x67bc, 0x67f7, 0x6831, 0x686c,
    0x68a6, 0x68df, 0x6919, 0x6952, 0x698b, 0x69c4, 0x69fc, 0x6a35,
    0x6a6d, 0x6aa4, 0x6adc, 0x6b13, 0x6b4a, 0x6b81, 0x6bb7, 0x6bed,
    0x6c23, 0x6c59, 0x6c8e, 0x6cc3, 0x6cf8, 0x6d2d, 0x6d61, 0x6d95,
    0x6dc9, 0x6dfd, 0x6e30, 0x6e63, 0x6e96, 0x6ec8, 0x6efa, 0x6f2c,
    0x6f5e, 0x6f8f, 0x6fc1, 0x6ff2, 0x7022, 0x7052, 0x7083, 0x70b2,
    0x70e2, 0x7111, 0x7140, 0x716f, 0x719d, 0x71cb, 0x71f9, 0x7227,
    0x7254, 0x7281, 0x72ae, 0x72db, 0x7307, 0x7333, 0x735e, 0x738a,
    0x73b5, 0x73e0, 0x740a, 0x7435, 0x745f, 0x7488, 0x74b2, 0x74db,
    0x7504, 0x752c, 0x7555, 0x757d, 0x75a5, 0x75cc, 0x75f3, 0x761a,
    0x7641, 0x7667, 0x768d, 0x76b3, 0x76d8, 0x76fe, 0x7722, 0x7747,
    0x776b, 0x778f, 0x77b3, 0x77d7, 0x77fa, 0x781d, 0x783f, 0x7862,
    0x7884, 0x78a5, 0x78c7, 0x78e8, 0x7909, 0x7929, 0x794a, 0x7969,
    0x7989, 0x79a9, 0x79c8, 0x79e6, 0x7a05, 0x7a23, 0x7a41, 0x7a5f,
    0x7a7c, 0x7a99, 0x7ab6, 0x7ad2, 0x7aee, 0x7b0a, 0x7b26, 0x7b41,
    0x7b5c, 0x7b77, 0x7b91, 0x7bab, 0x7bc5, 0x7bde, 0x7bf8, 0x7c10,
    0x7c29, 0x7c41, 0x7c59, 0x7c71, 0x7c88, 0x7c9f, 0x7cb6, 0x7ccd,
    0x7ce3, 0x7cf9, 0x7d0e, 0x7d24, 0x7d39, 0x7d4d, 0x7d62, 0x7d76,
    0x7d89, 0x7d9d, 0x7db0, 0x7dc3, 0x7dd5, 0x7de8, 0x7dfa, 0x7e0b,
    0x7e1d, 0x7e2e, 0x7e3e, 0x7e4f, 0x7e5f, 0x7e6f, 0x7e7e, 0x7e8d,
    0x7e9c, 0x7eab, 0x7eb9, 0x7ec7, 0x7ed5, 0x7ee2, 0x7eef, 0x7efc,
    0x7f09, 0x7f15, 0x7f21, 0x7f2c, 0x7f37, 0x7f42, 0x7f4d, 0x7f57,
    0x7f61, 0x7f6b, 0x7f74, 0x7f7d, 0x7f86, 0x7f8f, 0x7f97, 0x7f9f,
    0x7fa6, 0x7fad, 0x7fb4, 0x7fbb, 0x7fc1, 0x7fc7, 0x7fcd, 0x7fd2,
    0x7fd8, 0x7fdc, 0x7fe1, 0x7fe5, 0x7fe9, 0x7fec, 0x7ff0, 0x7ff3,
    0x7ff5, 0x7ff7, 0x7ff9, 0x7ffb, 0x7ffd, 0x7ffe, 0x7ffe, 0x7fff,
    0x7fff, 0x7fff, 0x7ffe, 0x7ffe, 0x7ffd, 0x7ffb, 0x7ff9, 0x7ff7,
    0x7ff5, 0x7ff3, 0x7ff0, 0x7fec, 0x7fe9, 0x7fe5, 0x7fe1, 0x7fdc,
    0x7fd8, 0x7fd2, 0x7fcd, 0x7fc7, 0x7fc1, 0x7fbb, 0x7fb4, 0x7fad,
    0x7fa6, 0x7f9f, 0x7f97, 0x7f8f, 0x7f86, 0x7f7d, 0x7f74, 0x7f6b,
    0x7f61, 0x7f57, 0x7f4d, 0x7f42, 0x7f37, 0x7f2c, 0x7f21, 0x7f15,
    0x7f09, 0x7efc, 0x7eef, 0x7ee2, 0x7ed5, 0x7ec7, 0x7eb9, 0x7eab,
    0x7e9c, 0x7e8d, 0x7e7e, 0x7e6f, 0x7e5f, 0x7e4f, 0x7e3e, 0x7e2e,
    0x7e1d, 0x7e0b, 0x7dfa, 0x7de8, 0x7dd5, 0x7dc3, 0x7db0, 0x7d9d,
    0x7d89, 0x7d76, 0x7d62, 0x7d4d, 0x7d39, 0x7d24, 0x7d0e, 0x7cf9,
    0x7ce3, 0x7ccd, 0x7cb6, 0x7c9f, 0x7c88, 0x7c71, 0x7c59, 0x7c41,
    0x7c29, 0x7c10, 0x7bf8, 0x7bde, 0x7bc5, 0x7bab, 0x7b91, 0x7b77,
    0x7b5c, 0x7b41, 0x7b26, 0x7b0a, 0x7aee, 0x7ad2, 0x7ab6, 0x7a99,
    0x7a7c, 0x7a5f, 0x7a41, 0x7a23, 0x7a05, 0x79e6, 0x79c8, 0x79a9,
    0x7989, 0x7969, 0x794a, 0x7929, 0x7909, 0x78e8, 0x78c7, 0x78a5,
    0x7884, 0x7862, 0x783f, 0x781d, 0x77fa, 0x77d7, 0x77b3, 0x778f,
    0x776b, 0x7747, 0x7722, 0x76fe, 0x76d8, 0x76b3, 0x768d, 0x7667,
    0x7641, 0x761a, 0x75f3, 0x75cc, 0x75a5, 0x757d, 0x7555, 0x752c,
    0x7504, 0x74db, 0x74b2, 0x7488, 0x745f, 0x7435, 0x740a, 0x73e0,
    0x73b5, 0x738a, 0x735e, 0x7333, 0x7307, 0x72db, 0x72ae, 0x7281,
    0x7254, 0x7227, 0x71f9, 0x71cb, 0x719d, 0x716f, 0x7140, 0x7111,
    0x70e2, 0x70b2, 0x7083, 0x7052, 0x7022, 0x6ff2, 0x6fc1, 0x6f8f,
    0x6f5e, 0x6f2c, 0x6efa, 0x6ec8, 0x6e96, 0x6e63, 0x6e30, 0x6dfd,
    0x6dc9, 0x6d95, 0x6d61, 0x6d2d, 0x6cf8, 0x6cc3, 0x6c8e, 0x6c59,
    0x6c23, 0x6bed, 0x6bb7, 0x6b81, 0x6b4a, 0x6b13, 0x6adc, 0x6aa4,
    0x6a6d, 0x6a35, 0x69fc, 0x69c4, 0x698b, 0x6952, 0x6919, 0x68df,
    0x68a6, 0x686c, 0x6831, 0x67f7, 0x67bc, 0x6781, 0x6746, 0x670a,
    0x66cf, 0x6693, 0x6656, 0x661a, 0x65dd, 0x65a0, 0x6563, 0x6525,
    0x64e8, 0x64aa, 0x646b, 0x642d, 0x63ee, 0x63af, 0x6370, 0x6331,
    0x62f1, 0x62b1, 0x6271, 0x6231, 0x61f0, 0x61af, 0x616e, 0x612d,
    0x60eb, 0x60aa, 0x6068, 0x6025, 0x5fe3, 0x5fa0, 0x5f5d, 0x5f1a,
    0x5ed7, 0x5e93, 0x5e4f, 0x5e0b, 0x5dc7, 0x5d82, 0x5d3d, 0x5cf8,
    0x5cb3, 0x5c6e, 0x5c28, 0x5be2, 0x5b9c, 0x5b56, 0x5b0f, 0x5ac9,
    0x5a82, 0x5a3a, 0x59f3, 0x59ab, 0x5964, 0x591b, 0x58d3, 0x588b,
    0x5842, 0x57f9, 0x57b0, 0x5767, 0x571d, 0x56d3, 0x5689, 0x563f,
    0x55f5, 0x55aa, 0x555f, 0x5514, 0x54c9, 0x547e, 0x5432, 0x53e6,
    0x539a, 0x534e, 0x5302, 0x52b5, 0x5268, 0x521b, 0x51ce, 0x5181,
    0x5133, 0x50e5, 0x5097, 0x5049, 0x4ffb, 0x4fac, 0x4f5d, 0x4f0e,
    0x4ebf, 0x4e70, 0x4e20, 0x4dd0, 0x4d81, 0x4d30, 0x4ce0, 0x4c90,
    0x4c3f, 0x4bee, 0x4b9d, 0x4b4c, 0x4afb, 0x4aa9, 0x4a57, 0x4a05,
    0x49b3, 0x4961, 0x490f, 0x48bc, 0x4869, 0x4816, 0x47c3, 0x4770,
    0x471c, 0x46c8, 0x4675, 0x4621, 0x45cc, 0x4578, 0x4524, 0x44cf,
    0x447a, 0x4425, 0x43d0, 0x437a, 0x4325, 0x42cf, 0x4279, 0x4224,
    0x41cd, 0x4177, 0x4121, 0x40ca, 0x4073, 0x401c, 0x3fc5, 0x3f6e,
    0x3f17, 0x3ebf, 0x3e67, 0x3e0f, 0x3db7, 0x3d5f, 0x3d07, 0x3caf,
    0x3c56, 0x3bfd, 0x3ba4, 0x3b4b, 0x3af2, 0x3a99, 0x3a3f, 0x39e6,
    0x398c, 0x3932, 0x38d8, 0x387e, 0x3824, 0x37c9, 0x376f, 0x3714,
    0x36b9, 0x365e, 0x3603, 0x35a8, 0x354d, 0x34f1, 0x3496, 0x343a,
    0x33de, 0x3382, 0x3326, 0x32ca, 0x326e, 0x3211, 0x31b5, 0x3158,
    0x30fb, 0x309e, 0x3041, 0x2fe4, 0x2f87, 0x2f29, 0x2ecc, 0x2e6e,
    0x2e10, 0x2db3, 0x2d55, 0x2cf6, 0x2c98, 0x2c3a, 0x2bdc, 0x2b7d,
    0x2b1f, 0x2ac0, 0x2a61, 0x2a02, 0x29a3, 0x2944, 0x28e5, 0x2885,
    0x2826, 0x27c7, 0x2767, 0x2707, 0x26a7, 0x2648, 0x25e8, 0x2588,
    0x2527, 0x24c7, 0x2467, 0x2406, 0x23a6, 0x2345, 0x22e5, 0x2284,
    0x2223, 0x21c2, 0x2161, 0x2100, 0x209f, 0x203e, 0x1fdc, 0x1f7b,
    0x1f19, 0x1eb8, 0x1e56, 0x1df4, 0x1d93, 0x1d31, 0x1ccf, 0x1c6d,
    0x1c0b, 0x1ba9, 0x1b47, 0x1ae4, 0x1a82, 0x1a20, 0x19bd, 0x195b,
    0x18f8, 0x1895, 0x1833, 0x17d0, 0x176d, 0x170a, 0x16a7, 0x1645,
    0x15e1, 0x157e, 0x151b, 0x14b8, 0x1455, 0x13f2, 0x138e, 0x132b,
    0x12c7, 0x1264, 0x1201, 0x119d, 0x1139, 0x10d6, 0x1072, 0x100e,
    0x0fab, 0x0f47, 0x0ee3, 0x0e7f, 0x0e1b, 0x0db7, 0x0d53, 0x0cef,
    0x0c8b, 0x0c27, 0x0bc3, 0x0b5f, 0x0afb, 0x0a97, 0x0a32, 0x09ce,
    0x096a, 0x0906, 0x08a1, 0x083d, 0x07d9, 0x0774, 0x0710, 0x06ac,
    0x0647, 0x05e3, 0x057e, 0x051a, 0x04b6, 0x0451, 0x03ed, 0x0388,
    0x0324, 0x02bf, 0x025b, 0x01f6, 0x0192, 0x012d, 0x00c9, 0x0064,
    0x0000, 0xff9b, 0xff36, 0xfed2, 0xfe6d, 0xfe09, 0xfda4, 0xfd40,
    0xfcdb, 0xfc77, 0xfc12, 0xfbae, 0xfb49, 0xfae5, 0xfa81, 0xfa1c,
    0xf9b8, 0xf953, 0xf8ef, 0xf88b, 0xf826, 0xf7c2, 0xf75e, 0xf6f9,
    0xf695, 0xf631, 0xf5cd, 0xf568, 0xf504, 0xf4a0, 0xf43c, 0xf3d8,
    0xf374, 0xf310, 0xf2ac, 0xf248, 0xf1e4, 0xf180, 0xf11c, 0xf0b8,
    0xf054, 0xeff1, 0xef8d, 0xef29, 0xeec6, 0xee62, 0xedfe, 0xed9b,
    0xed38, 0xecd4, 0xec71, 0xec0d, 0xebaa, 0xeb47, 0xeae4, 0xea81,
    0xea1e, 0xe9ba, 0xe958, 0xe8f5, 0xe892, 0xe82f, 0xe7cc, 0xe76a,
    0xe707, 0xe6a4, 0xe642, 0xe5df, 0xe57d, 0xe51b, 0xe4b8, 0xe456,
    0xe3f4, 0xe392, 0xe330, 0xe2ce, 0xe26c, 0xe20b, 0xe1a9, 0xe147,
    0xe0e6, 0xe084, 0xe023, 0xdfc1, 0xdf60, 0xdeff, 0xde9e, 0xde3d,
    0xdddc, 0xdd7b, 0xdd1a, 0xdcba, 0xdc59, 0xdbf9, 0xdb98, 0xdb38,
    0xdad8, 0xda77, 0xda17, 0xd9b7, 0xd958, 0xd8f8, 0xd898, 0xd838,
    0xd7d9, 0xd77a, 0xd71a, 0xd6bb, 0xd65c, 0xd5fd, 0xd59e, 0xd53f,
    0xd4e0, 0xd482, 0xd423, 0xd3c5, 0xd367, 0xd309, 0xd2aa, 0xd24c,
    0xd1ef, 0xd191, 0xd133, 0xd0d6, 0xd078, 0xd01b, 0xcfbe, 0xcf61,
    0xcf04, 0xcea7, 0xce4a, 0xcdee, 0xcd91, 0xcd35, 0xccd9, 0xcc7d,
    0xcc21, 0xcbc5, 0xcb69, 0xcb0e, 0xcab2, 0xca57, 0xc9fc, 0xc9a1,
    0xc946, 0xc8eb, 0xc890, 0xc836, 0xc7db, 0xc781, 0xc727, 0xc6cd,
    0xc673, 0xc619, 0xc5c0, 0xc566, 0xc50d, 0xc4b4, 0xc45b, 0xc402,
    0xc3a9, 0xc350, 0xc2f8, 0xc2a0, 0xc248, 0xc1f0, 0xc198, 0xc140,
    0xc0e8, 0xc091, 0xc03a, 0xbfe3, 0xbf8c, 0xbf35, 0xbede, 0xbe88,
    0xbe32, 0xbddb, 0xbd86, 0xbd30, 0xbcda, 0xbc85, 0xbc2f, 0xbbda,
    0xbb85, 0xbb30, 0xbadb, 0xba87, 0xba33, 0xb9de, 0xb98a, 0xb937,
    0xb8e3, 0xb88f, 0xb83c, 0xb7e9, 0xb796, 0xb743, 0xb6f0, 0xb69e,
    0xb64c, 0xb5fa, 0xb5a8, 0xb556, 0xb504, 0xb4b3, 0xb462, 0xb411,
    0xb3c0, 0xb36f, 0xb31f, 0xb2cf, 0xb27e, 0xb22f, 0xb1df, 0xb18f,
    0xb140, 0xb0f1, 0xb0a2, 0xb053, 0xb004, 0xafb6, 0xaf68, 0xaf1a,
    0xaecc, 0xae7e, 0xae31, 0xade4, 0xad97, 0xad4a, 0xacfd, 0xacb1,
    0xac65, 0xac19, 0xabcd, 0xab81, 0xab36, 0xaaeb, 0xaaa0, 0xaa55,
    0xaa0a, 0xa9c0, 0xa976, 0xa92c, 0xa8e2, 0xa898, 0xa84f, 0xa806,
    0xa7bd, 0xa774, 0xa72c, 0xa6e4, 0xa69b, 0xa654, 0xa60c, 0xa5c5,
    0xa57d, 0xa536, 0xa4f0, 0xa4a9, 0xa463, 0xa41d, 0xa3d7, 0xa391,
    0xa34c, 0xa307, 0xa2c2, 0xa27d, 0xa238, 0xa1f4, 0xa1b0, 0xa16c,
    0xa128, 0xa0e5, 0xa0a2, 0xa05f, 0xa01c, 0x9fda, 0x9f97, 0x9f55,
    0x9f14, 0x9ed2, 0x9e91, 0x9e50, 0x9e0f, 0x9dce, 0x9d8e, 0x9d4e,
    0x9d0e, 0x9cce, 0x9c8f, 0x9c50, 0x9c11, 0x9bd2, 0x9b94, 0x9b55,
    0x9b17, 0x9ada, 0x9a9c, 0x9a5f, 0x9a22, 0x99e5, 0x99a9, 0x996c,
    0x9930, 0x98f5, 0x98b9, 0x987e, 0x9843, 0x9808, 0x97ce, 0x9793,
    0x9759, 0x9720, 0x96e6, 0x96ad, 0x9674, 0x963b, 0x9603, 0x95ca,
    0x9592, 0x955b, 0x9523, 0x94ec, 0x94b5, 0x947e, 0x9448, 0x9412,
    0x93dc, 0x93a6, 0x9371, 0x933c, 0x9307, 0x92d2, 0x929e, 0x926a,
    0x9236, 0x9202, 0x91cf, 0x919c, 0x9169, 0x9137, 0x9105, 0x90d3,
    0x90a1, 0x9070, 0x903e, 0x900d, 0x8fdd, 0x8fad, 0x8f7c, 0x8f4d,
    0x8f1d, 0x8eee, 0x8ebf, 0x8e90, 0x8e62, 0x8e34, 0x8e06, 0x8dd8,
    0x8dab, 0x8d7e, 0x8d51, 0x8d24, 0x8cf8, 0x8ccc, 0x8ca1, 0x8c75,
    0x8c4a, 0x8c1f, 0x8bf5, 0x8bca, 0x8ba0, 0x8b77, 0x8b4d, 0x8b24,
    0x8afb, 0x8ad3, 0x8aaa, 0x8a82, 0x8a5a, 0x8a33, 0x8a0c, 0x89e5,
    0x89be, 0x8998, 0x8972, 0x894c, 0x8927, 0x8901, 0x88dd, 0x88b8,
    0x8894, 0x8870, 0x884c, 0x8828, 0x8805, 0x87e2, 0x87c0, 0x879d,
    0x877b, 0x875a, 0x8738, 0x8717, 0x86f6, 0x86d6, 0x86b5, 0x8696,
    0x8676, 0x8656, 0x8637, 0x8619, 0x85fa, 0x85dc, 0x85be, 0x85a0,
    0x8583, 0x8566, 0x8549, 0x852d, 0x8511, 0x84f5, 0x84d9, 0x84be,
    0x84a3, 0x8488, 0x846e, 0x8454, 0x843a, 0x8421, 0x8407, 0x83ef,
    0x83d6, 0x83be, 0x83a6, 0x838e, 0x8377, 0x8360, 0x8349, 0x8332,
    0x831c, 0x8306, 0x82f1, 0x82db, 0x82c6, 0x82b2, 0x829d, 0x8289,
    0x8276, 0x8262, 0x824f, 0x823c, 0x822a, 0x8217, 0x8205, 0x81f4,
    0x81e2, 0x81d1, 0x81c1, 0x81b0, 0x81a0, 0x8190, 0x8181, 0x8172,
    0x8163, 0x8154, 0x8146, 0x8138, 0x812a, 0x811d, 0x8110, 0x8103,
    0x80f6, 0x80ea, 0x80de, 0x80d3, 0x80c8, 0x80bd, 0x80b2, 0x80a8,
    0x809e, 0x8094, 0x808b, 0x8082, 0x8079, 0x8070, 0x8068, 0x8060,
    0x8059, 0x8052, 0x804b, 0x8044, 0x803e, 0x8038, 0x8032, 0x802d,
    0x8027, 0x8023, 0x801e, 0x801a, 0x8016, 0x8013, 0x800f, 0x800c,
    0x800a, 0x8008, 0x8006, 0x8004, 0x8002, 0x8001, 0x8001, 0x8000,
    0x8000, 0x8000, 0x8001, 0x8001, 0x8002, 0x8004, 0x8006, 0x8008,
    0x800a, 0x800c, 0x800f, 0x8013, 0x8016, 0x801a, 0x801e, 0x8023,
    0x8027, 0x802d, 0x8032, 0x8038, 0x803e, 0x8044, 0x804b, 0x8052,
    0x8059, 0x8060, 0x8068, 0x8070, 0x8079, 0x8082, 0x808b, 0x8094,
    0x809e, 0x80a8, 0x80b2, 0x80bd, 0x80c8, 0x80d3, 0x80de, 0x80ea,
    0x80f6, 0x8103, 0x8110, 0x811d, 0x812a, 0x8138, 0x8146, 0x8154,
    0x8163, 0x8172, 0x8181, 0x8190, 0x81a0, 0x81b0, 0x81c1, 0x81d1,
    0x81e2, 0x81f4, 0x8205, 0x8217, 0x822a, 0x823c, 0x824f, 0x8262,
    0x8276, 0x8289, 0x829d, 0x82b2, 0x82c6, 0x82db, 0x82f1, 0x8306,
    0x831c, 0x8332, 0x8349, 0x8360, 0x8377, 0x838e, 0x83a6, 0x83be,
    0x83d6, 0x83ef, 0x8407, 0x8421, 0x843a, 0x8454, 0x846e, 0x8488,
    0x84a3, 0x84be, 0x84d9, 0x84f5, 0x8511, 0x852d, 0x8549, 0x8566,
    0x8583, 0x85a0, 0x85be, 0x85dc, 0x85fa, 0x8619, 0x8637, 0x8656,
    0x8676, 0x8696, 0x86b5, 0x86d6, 0x86f6, 0x8717, 0x8738, 0x875a,
    0x877b, 0x879d, 0x87c0, 0x87e2, 0x8805, 0x8828, 0x884c, 0x8870,
    0x8894, 0x88b8, 0x88dd, 0x8901, 0x8927, 0x894c, 0x8972, 0x8998,
    0x89be, 0x89e5, 0x8a0c, 0x8a33, 0x8a5a, 0x8a82, 0x8aaa, 0x8ad3,
    0x8afb, 0x8b24, 0x8b4d, 0x8b77, 0x8ba0, 0x8bca, 0x8bf5, 0x8c1f,
    0x8c4a, 0x8c75, 0x8ca1, 0x8ccc, 0x8cf8, 0x8d24, 0x8d51, 0x8d7e,
    0x8dab, 0x8dd8, 0x8e06, 0x8e34, 0x8e62, 0x8e90, 0x8ebf, 0x8eee,
    0x8f1d, 0x8f4d, 0x8f7c, 0x8fad, 0x8fdd, 0x900d, 0x903e, 0x9070,
    0x90a1, 0x90d3, 0x9105, 0x9137, 0x9169, 0x919c, 0x91cf, 0x9202,
    0x9236, 0x926a, 0x929e, 0x92d2, 0x9307, 0x933c, 0x9371, 0x93a6,
    0x93dc, 0x9412, 0x9448, 0x947e, 0x94b5, 0x94ec, 0x9523, 0x955b,
    0x9592, 0x95ca, 0x9603, 0x963b, 0x9674, 0x96ad, 0x96e6, 0x9720,
    0x9759, 0x9793, 0x97ce, 0x9808, 0x9843, 0x987e, 0x98b9, 0x98f5,
    0x9930, 0x996c, 0x99a9, 0x99e5, 0x9a22, 0x9a5f, 0x9a9c, 0x9ada,
    0x9b17, 0x9b55, 0x9b94, 0x9bd2, 0x9c11, 0x9c50, 0x9c8f, 0x9cce,
    0x9d0e, 0x9d4e, 0x9d8e, 0x9dce, 0x9e0f, 0x9e50, 0x9e91, 0x9ed2,
    0x9f14, 0x9f55, 0x9f97, 0x9fda, 0xa01c, 0xa05f, 0xa0a2, 0xa0e5,
    0xa128, 0xa16c, 0xa1b0, 0xa1f4, 0xa238, 0xa27d, 0xa2c2, 0xa307,
    0xa34c, 0xa391, 0xa3d7, 0xa41d, 0xa463, 0xa4a9, 0xa4f0, 0xa536,
    0xa57d, 0xa5c5, 0xa60c, 0xa654, 0xa69b, 0xa6e4, 0xa72c, 0xa774,
    0xa7bd, 0xa806, 0xa84f, 0xa898, 0xa8e2, 0xa92c, 0xa976, 0xa9c0,
    0xaa0a, 0xaa55, 0xaaa0, 0xaaeb, 0xab36, 0xab81, 0xabcd, 0xac19,
    0xac65, 0xacb1, 0xacfd, 0xad4a, 0xad97, 0xade4, 0xae31, 0xae7e,
    0xaecc, 0xaf1a, 0xaf68, 0xafb6, 0xb004, 0xb053, 0xb0a2, 0xb0f1,
    0xb140, 0xb18f, 0xb1df, 0xb22f, 0xb27e, 0xb2cf, 0xb31f, 0xb36f,
    0xb3c0, 0xb411, 0xb462, 0xb4b3, 0xb504, 0xb556, 0xb5a8, 0xb5fa,
    0xb64c, 0xb69e, 0xb6f0, 0xb743, 0xb796, 0xb7e9, 0xb83c, 0xb88f,
    0xb8e3, 0xb937, 0xb98a, 0xb9de, 0xba33, 0xba87, 0xbadb, 0xbb30,
    0xbb85, 0xbbda, 0xbc2f, 0xbc85, 0xbcda, 0xbd30, 0xbd86, 0xbddb,
    0xbe32, 0xbe88, 0xbede, 0xbf35, 0xbf8c, 0xbfe3, 0xc03a, 0xc091,
    0xc0e8, 0xc140, 0xc198, 0xc1f0, 0xc248, 0xc2a0, 0xc2f8, 0xc350,
    0xc3a9, 0xc402, 0xc45b, 0xc4b4, 0xc50d, 0xc566, 0xc5c0, 0xc619,
    0xc673, 0xc6cd, 0xc727, 0xc781, 0xc7db, 0xc836, 0xc890, 0xc8eb,
    0xc946, 0xc9a1, 0xc9fc, 0xca57, 0xcab2, 0xcb0e, 0xcb69, 0xcbc5,
    0xcc21, 0xcc7d, 0xccd9, 0xcd35, 0xcd91, 0xcdee, 0xce4a, 0xcea7,
    0xcf04, 0xcf61, 0xcfbe, 0xd01b, 0xd078, 0xd0d6, 0xd133, 0xd191,
    0xd1ef, 0xd24c, 0xd2aa, 0xd309, 0xd367, 0xd3c5, 0xd423, 0xd482,
    0xd4e0, 0xd53f, 0xd59e, 0xd5fd, 0xd65c, 0xd6bb, 0xd71a, 0xd77a,
    0xd7d9, 0xd838, 0xd898, 0xd8f8, 0xd958, 0xd9b7, 0xda17, 0xda77,
    0xdad8, 0xdb38, 0xdb98, 0xdbf9, 0xdc59, 0xdcba, 0xdd1a, 0xdd7b,
    0xdddc, 0xde3d, 0xde9e, 0xdeff, 0xdf60, 0xdfc1, 0xe023, 0xe084,
    0xe0e6, 0xe147, 0xe1a9, 0xe20b, 0xe26c, 0xe2ce, 0xe330, 0xe392,
    0xe3f4, 0xe456, 0xe4b8, 0xe51b, 0xe57d, 0xe5df, 0xe642, 0xe6a4,
    0xe707, 0xe76a, 0xe7cc, 0xe82f, 0xe892, 0xe8f5, 0xe958, 0xe9ba,
    0xea1e, 0xea81, 0xeae4, 0xeb47, 0xebaa, 0xec0d, 0xec71, 0xecd4,
    0xed38, 0xed9b, 0xedfe, 0xee62, 0xeec6, 0xef29, 0xef8d, 0xeff1,
    0xf054, 0xf0b8, 0xf11c, 0xf180, 0xf1e4, 0xf248, 0xf2ac, 0xf310,
    0xf374, 0xf3d8, 0xf43c, 0xf4a0, 0xf504, 0xf568, 0xf5cd, 0xf631,
    0xf695, 0xf6f9, 0xf75e, 0xf7c2, 0xf826, 0xf88b, 0xf8ef, 0xf953,
    0xf9b8, 0xfa1c, 0xfa81, 0xfae5, 0xfb49, 0xfbae, 0xfc12, 0xfc77,
    0xfcdb, 0xfd40, 0xfda4, 0xfe09, 0xfe6d, 0xfed2, 0xff36, 0xff9b,
};

inline void ROTOR_GetTrigonometic(ROTOR_TypeDef* Rotor) {
    // Внимание! Перегрузка: _IQtoIQ15(_IQ(1.0)) === 0x8000
    long A = *Rotor->yPhi > _IQ(0.99999) ? _IQ(0.99999) : *Rotor->yPhi;
    u16 phi = _PU15toBASE(_IQtoIQ15(A), TABSIZE);
    Rotor->Sine = _IQ15toIQ(sinTbl[phi]);
    phi += TABSIZE >> 2;
    phi = phi >= TABSIZE ? phi - TABSIZE : phi;
    Rotor->Cosine = _IQ15toIQ(sinTbl[phi]);
}

/*******************************************************************************
* IQ-математика необходимая для:
* 1. бездатчикового привода
* 2. обработки сигналов резольвера / СКВТ / ЛДТ
*******************************************************************************/
#define M_PI            3.141592653589793       // Число Пи
#define M_PI2           (M_PI / 2)              // Число Пи / 2

_iq _IQdiv14BIT(_iq A, _iq B);
_iq _IQatan(_iq A); // не макрос т.к. в макрсое аргумент дважды вчисляется

/*******************************************************************************
* Function Name : _IQdiv14BIT
* Description :   Функция деления. Делит первый аргумент на второй.
*     Аргументы - 32-х битные целые числа со знаком.
*     Тип данных аргументов и возвращаемого значения - GL_Q.
*     Точность результата - 14 бит (30 / 2).
*     При одновременном уменьшении значений аргументов - не падает.
*     Время исполнения от значений аргументов не зависит - 1.1 uS.
*******************************************************************************/
inline _iq _IQdiv14BIT(_iq A, _iq B) {
    _iq x = A > 0 ? A : -A;
    _iq y = B > 0 ? B : -B;
    y = x > y ? x : y;
    unsigned char i, j;
    if (y & 0xFFFF8000U) {
        if (y & 0xFF800000U) {
            if (y & 0xF8000000U) {
                i = y & 0xE0000000U ? 0 : 2;
            } else {
                i = y & 0xFE000000U ? 4 : 6;
            }
        } else {
            if (y & 0xFFF80000U) {
                i = y & 0xFFE00000U ? 8 : 10;
            } else {
                i = y & 0xFFFE0000U ? 12 : 14;
            }
        }
    } else {
        if (y & 0xFFFFFF80U) {
            if (y & 0xFFFFF800U) {
                i = y & 0xFFFFE000U ? 30 - 14 : 30 - 12;
            } else {
                i = y & 0xFFFFFE00U ? 30 - 10 : 30 - 8;
            }
        } else {
            if (y & 0xFFFFFFF8U) {
                i = y & 0xFFFFFFE0U ? 30 - 6 : 30 - 4;
            } else {
                i = y & 0xFFFFFFFEU ? 30 - 2 : 30 - 0;
            }
        }
    }
    j = i >> 1;
    j = i <= GL_Q ? (GL_Q >> 1) - j : j - (GL_Q >> 1);
    return
    i <= GL_Q
        ? ((long)((long)(A << i) / (B >> j)) << j)
        : ((long)((long)(A << i) / (B << j)) >> j);
}

/*******************************************************************************
* Function Name : _IQatan
* Description :   Базовая функция вычисления значения арктангенса полиномом.
*     Аргумент и возвращаемое значение - 32-х битные целые числа со знаком.
*     Тип данных аргумента и возвращаемого значения - GL_Q.
*     Диапазон значений для аргумента: от _IQ(0.0) до _IQ(1.0)
*     Абсолютная погрешность вычисления результата - 0.0035 рад.
*     Время исполнения от значений аргументов не зависит - #.# uS.
*******************************************************************************/
inline _iq _IQatan(_iq A) {
    return _IQmpy(A, (_IQ(1.054) - _IQmpy(_IQ(0.265), A))); // 0.42 uS.
    // Формула, имеющая абсолютную погрешность 0.00134 рад.
    //return _IQmpy(A, (_IQ(1.0268) - _IQmpy(A,
    //    (_IQmpy(A, _IQ(0.0779)) + _IQ(0.1649))))); // 0.58 uS.
    // Формула, имеющая абсолютную погрешность 0.000113 рад.
    //return
    //    _IQmpy(A, (_IQmpy(A, (_IQmpy(A, (_IQmpy(A, _IQ(0.14142))
    //    - _IQ(0.34523))) - _IQ(0.01306))) + _IQ(1.00241))); // 0.77 uS.
    // x * (1.0268 - x * (x * 0.0779 + 0.1649))
    // x * (x * (x * (x * 0.14142 - 0.34523) - 0.01306) + 1.00241)
}

/*******************************************************************************
* Function Name : _IQatan2
* Description :   Функция вычисления значения арктангенса _IQatan2(Y / X).
*     Аргументы и возвращаемое значение - 32-х битные целые числа со знаком.
*     Тип данных аргумента и возвращаемого значения - GL_Q.
*     Диапазон значений для аргументов: весь диапазон типа данных.
*     Абсолютная погрешность вычисления результата - 0.0035 рад.
*     Время исполнения от значений аргументов не зависит - 2.7 .. 2.9 uS.
*******************************************************************************/
inline _iq _IQatan2(_iq A, _iq B) {
    if (A == 0) {
        return B >= 0 ? 0 : -_IQ(M_PI);
    } else if (B == 0) {
        return A > 0 ? _IQ(M_PI2) : -_IQ(M_PI2);
    } else if (B > 0) {
        if (A > 0) {
            return B > A ? +_IQatan(_IQdiv14BIT(A, B))
                : +_IQ(M_PI2) - _IQatan(_IQdiv14BIT(B, A));
        } else {
            A = -A;
            return B > A ? -_IQatan(_IQdiv14BIT(A, B))
                : -_IQ(M_PI2) + _IQatan(_IQdiv14BIT(B, A));
        }
    } else {
        B = -B;
        if (A > 0) {
            return B > A ? +_IQ(M_PI) - _IQatan(_IQdiv14BIT(A, B))
                : +_IQ(M_PI2) + _IQatan(_IQdiv14BIT(B, A));
        } else {
            A = -A;
            return B > A ? -_IQ(M_PI) + _IQatan(_IQdiv14BIT(A, B))
                : -_IQ(M_PI2) - _IQatan(_IQdiv14BIT(B, A));
        }
    }
}

Листинг 5. Файл main.c

/******************************************************************************
* File Name :   main.c
* Author :      Клиначев Николай Васильевич
* Version :     1.1.0
* Date :        20150707
* Target :      ARM Cortex-M4 (STM32F303xC / STM32F3DISCOVERY), FPU:Off
* Description : Программа векторной бездатчиковой системы управления для
*               синхронного двигателя с возбуждением от постоянных магнитов
*               СДПМ (PMSM / BLAC).
*     Программа реализует подчиненную управляющую систему, которая позволяет
*     контролировать момент и угловую скорость вала синхронного двигателя с
*     постоянными магнитами (СДПМ / PMSM) во всех четырех квадрантах плоскости
*     механической характеристики. Тип управления – векторный, бездатчиковый.
*     Скорость вала вычисляется по напряжению DC-шины и мгновенным значениям
*     токов фаз двигателя. Положение ротора - с помощью вычислителя мгновенных
*     значений потокосцеплений по осям Alpha/Beta.
*     ШИМ-драйвер поддерживает и классическую и векторную модуляцию.
*     Сторонние библиотеки цифровой обработки сигналов не используются.
*     Для эмуляции выполнения операций с плавающей точкой на целочисленном
*     АЛУ определены макросы IQ-математики. При адаптации к высоковольтным
*     инвертерам или к инвертерам на большие токи потребуется смена типа
*     данных. Текущий тип данных, _iq24, поддерживает числа в диапазоне:
*     от -128 до 127.999999940, с разрешением 0.000000060.
*     Возможна адаптация программы к менее мощным 32-х разрядным процессорам
*     ARM Cortex-M3 / 72 МГц (ЗАО "ПКК Миландр" и ОАО "НИИЭТ").
*     Текущий процессор справиться и с обработкой сигналов резольвера.
*
*     Соглашение о префиксах для координат САР:
*          g – задающее воздействие (Ref),
*          y – регулируемая координата (Fbk),
*          x – ошибка регулирования (Err),
*          u – управление на объект (Out),
*          p – верхний предел координаты (upper, pozitive),
*          n – нижний предел координаты (lower, negative),
*          zo# – значение хранящееся в регистре задержки (1/z),
*          zi# – значение на входе регистра задержки (input of 1/z),
*          u# – внутренняя координата регулятора (private coordinate).
*
*     Алгоритм работы с отладчиком:
*          1. Запустить отладчик в свободном режиме
*          2. Нажать кнопку для запуска двигателя
*          3. Нажать кнопку для остановки двигателя
*          4. Остановить отладчик
*          5. Изменить значения переменных в Watch window
*          6. Перейти к шагу 1
*******************************************************************************/
// TODO: Определить уровни компиляции проекта для поэтапной настройки привода
// TODO: Реализовать канал комбинированного управления в регуляторе тока Iq
// TODO: Реализовать измерение напряжения шины постоянного тока McKit.Vdc
// TODO: Реализовать выбор 2х фаз для измерения тока (особенно при SVPWM)
// TODO: Увеличить период дискретизации контура тока CURRENT_LOOP_TIMESTEP
// TODO: Увеличить период дискретизации контура скорости SPEED_LOOP_TIMESTEP
// TODO: Привести к относительным единицам Наблюдатель потокосцеплений
// TODO: Привести к относительным единицам Наблюдатель скорости
// TODO: Реализовать управление для ключа балластного резистора
// TODO: Реализовать возбуждение и обработку сигналов резольвера
// TODO: У Оптимизатора кода проблема с Alpha_FSM_Task переделать на case

/* Includes ------------------------------------------------------------------*/
#include "stm32f30x.h"  // Подключаем библиотеки производителя процессора
#include "mckit.h"      // Подключаем базу с параметрами силовых мостов
#include "motors.h"     // Подключаем базу с параметрами электродвигателей
#include "mclib.h"      // Подключаем библиотеки IQ-Math и Motor Control

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define ADC_CDR_ADDRESS ((uint32_t)0x5000030C)  // Адрес регистров данных АЦП1 и АЦП2
#define NULL            ((uint32_t)0x00000000)  // Незаполненный указатель
#define M_PI            3.141592653589793       // Число Пи
#define M_SQRT3         1.7320508075688772      // Math.sqrt(3)

#define SYSCLK          72000000                // SystemCoreClock
#define PWM_COUNTER     2048                    // 2048 == 17578.125 Гц
#define PWM_FREQUENCY   (0.5 * SYSCLK / PWM_COUNTER) // div2 4 TRANGLE
#define PWM_RESOLUTION  PWM_COUNTER             // Разрешение падает с ростом частоты
#define TIMESTEP        (2.0 / PWM_FREQUENCY)   // Шаг дискретизации ИУ, Ротаторов (сек)
#define CURRENT_LOOP_TIMESTEP  (TIMESTEP * 1)   // Шаг дискретизации РТ ЦСУ (сек)
#define SPEED_LOOP_TIMESTEP    (TIMESTEP * 1)   // Шаг дискретизации РC ЦСУ (сек)

// Коэффициент передачи преобразователя напряжения (ШИМ-драйвера)
// или Максимально возможное напряжение на секции двигателя, (Y), KPWM == V_MAX
#define KPWM    (SVPWM_ON ? BASE_VOLTAGE / M_SQRT3 : 0.5 * BASE_VOLTAGE)
// Коэффициент P-канала регулятора тока: Kрт = Kкт Rя / Kдт / Kпн
#define KP_C            (KK_C * RS / (1.0 / BASE_CURRENT) / KPWM)
#define TI_C            (LS / RS)       // Электромагнитная постоянная времени
// Скорость идеального холостого хода двигателя, (рад/с)
#define BASE_SPEED      ((BASE_FREQ * 2 * M_PI) / (POLES / 2))

// Электромагнитная постоянная времени
#define TE              (LS / RS)
// Механическая постоянная времени: Tм = Rя J / (KФ)^2
#define TM              (JPR == 0 ? 0 : ((2.0 / 3 * RS) * JPR / (3.0 / 2 * KE * KE)))

#if !defined KP_S
// Коэффициент усиления контура скорости: Kкс = Tм / (2..3 Tя)
#define KK_S            (TM == 0 ? 0 : (TM / (3 * TE)))
// Коэффициент P-канала регулятора скорости: Kрс = Kкс KФ q Kдт / Rя / Kдс
#define KP_S            (KK_S * KE * (1.0 / BASE_CURRENT) / RS / (1.0 / BASE_SPEED))
#endif

#if !defined TI_S
// Постоянная времени И-канала регулятора скорости: Tрс = Tм .. (4..6 Tя)
#define TI_S            (TM < 2 * TE ? 0 : TM < 6 * TE ? TM : (6 * TE))
//
// Если TI_S == 0, то механическая постоянная времени машины меньше
// электромагнитной. В этом случае необходимо подключить к ротору момент
// инерции нагрузки. Проконтролировать условие TI_S == 0 можно в функции:
// PMSM_CntrlUnit_SetBlkParam, - отслеживая переменную pid_spd.Ki
//
#warning "PMSM: IF (TM < 2 * TE) THEN { enlarge JPR in motors.h }"
#endif

/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/

/* Область памяти, для обращения к которой можно использовать:
* либо имя массива в котором хранятся 32-х разрядные слова (4 шт.),
* либо имя массива в котором хранятся 16-и разрядные полуслова (8 шт.).
*
* adc.Dual[0] == { adc.Single[0], adc.Single[1] }
* adc.Dual[1] == { adc.Single[2], adc.Single[3] }
* adc.Dual[2] == { adc.Single[4], adc.Single[5] }
* adc.Dual[3] == { adc.Single[6], adc.Single[7] }
*
* adc.Single[0] ==  АЦП1, 1-ая выборка без знака (ток фазы статора uIs)
* adc.Single[1] ==  АЦП2, 1-ая выборка без знака (ток фазы статора vIs)
* adc.Single[2] ==  АЦП1, 2-ая выборка без знака (ток фазы статора uIs)
* adc.Single[3] ==  АЦП2, 2-ая выборка без знака (ток фазы статора vIs)
* adc.Single[4]  -  значение, полученное при самокалибровке АЦП1
* adc.Single[5]  -  значение, полученное при самокалибровке АЦП2
* adc.Single[6]  -  среднее по двум выборкам АЦП1 со знаком, тип данных: _iq11
* adc.Single[7]  -  среднее по двум выборкам АЦП2 со знаком, тип данных: _iq11
*/
union {
    uint32_t Dual[4];      // массив 32-х разрядных слов
    int16_t Single[8];     // массив 16-х разрядных полуслов
} adc;                     // Необработанные данные АЦП1 и АЦП2

typedef struct {
    _iq gS;    // Output: Уставка Скорости в относительных ед. (pu), [-1.0, +1.0]
    _iq gI;    // Output: Уставка тока по оси момента, Iq, (pu), [0.0, +1.0]
    _iq g0;    // Уставка тока по оси магнитного поля, Id, (pu). Для PMSM = 0.0
    FunctionalState SVPWM_Enable; // Output: флаг ативизации SVPWM
} SETPOINTS_TypeDef;
// Глобальные уставки Цифровой системы управления для СДПМ (PMSM / BLAC)
SETPOINTS_TypeDef       SetPnt;
// Интегратор Угла и другие координаты, связанные с положением ротора
ROTOR_TypeDef           Rotor;
// Инверсный преобразователь Парка (dq2ab == dc2ac)
PIPARK_TypeDef          dq2ab;
// Инвертирующий преобразователь Кларка (преобразователь числа фаз - 2Ph_2_3Ph)
PICLARKE_TypeDef        ab2uvw;
// Неинверти-щий преобразователь Кларка (преобразователь числа фаз - 3Ph_2_2Ph)
PCLARKE_TypeDef         uvw2ab;
// Прямой преобразователь Парка (ab2dq == ac2dc)
PPARK_TypeDef           ab2dq;
// Подчиненный PI-регулятор q-проекции тока статора СДПМ (ось момента)
DQ_PID_TypeDef          pid_iq;
// Подчиненный PI-регулятор d-проекции тока статора СДПМ (ось поля)
DQ_PID_TypeDef          pid_id;
// Наблюдатель потокосцеплений
PSI_ESTIMATOR_TypeDef   psi;
// Наблюдатель Скорости PMSM (Speed Estimator)
SPD_ESTIMATOR_TypeDef   se;
// Измеряемые величины на силовом модуле
MCKIT_TypeDef           McKit;
// Регулятор Скорости c Ограничителем
SPD_PID_TypeDef         pid_spd;

// Переменные низкоприоритетной Машины Состояний
unsigned long VirtualTimer = 0;         // виртуальный таймер
void (*Alpha_FSM_Task)(void);           // указатель на задачу
unsigned int FSM_TicCounter = 0;        // счетчик тиков
FlagStatus isIRQHandlerLocked = SET;    // флаг блокировки вычислений
FlagStatus isModorInSinxroMode = RESET; // флаг синхроного режима

/* Private function prototypes -----------------------------------------------*/
static void RCC_Configuration(void);
static void GPIO_Configuration(void);
static void NVIC_Configuration(void);
static void TIM_Configuration(void);
static void ADC_Configuration(void);
static void DMA_Configuration(void);
static void DAC_Config(void);
static void PMSM_CntrlUnit_SetBlkParam(void);
static void PMSM_CntrlUnit_CreateWires(void);
static void PMSM_CntrlUnit_Reset_IC(void);
static void Task_motor_off(void);
static void Task_motor_pos(void);
static void Task_motor_rot(void);
static void Task_motor_run(void);

void DMA1_Channel1_IRQHandler(void);

/*******************************************************************************
* Function Name : main
* Description :   Main program
*******************************************************************************/
#pragma optimize=none  // 4 main Иначе Оптимизатор кода отключает Alpha_FSM_Task
int main(void) {
    // На этой стадии тактовый генератор процессора уже сконфигурирован,
    // это сделано функцией SystemInit(), которая была вызвана в "startup-файле"
    // (startup_stm32f30x.s), прежде чем была вызвана главная функция приложения
    // main. Для реконфигурирования значений по умолчанию обратитесь к файлу
    // system_stm32f30x.c, который создаётся изготовителем процессора.

    // Настраиваем тактовый генератор процессора
    RCC_Configuration();
    // Заполняем таблицу векторов прерываний
    NVIC_Configuration();
    // Конфигурируем порты ввода / вывода
    GPIO_Configuration();
    // Конфигурируем АЦП
    ADC_Configuration();
    // Конфигурируем контроллер ПДП
    DMA_Configuration();
    // Конфигурируем таймер
    TIM_Configuration();
    // Конфигурируем ЦАП
    DAC_Config();

    //PMSM_CntrlUnit_CreateBlock(); // Создаем матем. блоки ЦСУ для PMSM
    PMSM_CntrlUnit_SetBlkParam(); // Устанавливаем параметры блоков
    PMSM_CntrlUnit_CreateWires(); // Определяем схему передачи аргументов
    PMSM_CntrlUnit_Reset_IC();    // Предустанавливаем Начальные Условия

    // Включаем счетчик таймера
    TIM_Cmd(TIM1, ENABLE);
    // Не включаем главный ШИМ-выход таймера
    // ...

    unsigned long zVirtualTimer = 0;    // Регистр задержки для тика таймера
    Alpha_FSM_Task = &Task_motor_off;   // Предустановили задачу для FSM

    // Машина состояний для низкоприоритетных операций:
    // опрос копок, индикация, сетевая коммуникация и тп.
    while (1) {
        zVirtualTimer = VirtualTimer;             // запомнили тик таймера
        while (VirtualTimer == zVirtualTimer) {}  // ждем новый тик таймера
        // Каждые 58 mS (каждые 2^9 тиков) обслуживаем Машину Состояний
        if ((VirtualTimer & ((1 << 9) - 1)) == 0) { (*Alpha_FSM_Task)(); }
    }
}

static void Task_motor_off(void) {
    // Декрементируем счетчик времени блокировки кнопки
    if (FSM_TicCounter) { FSM_TicCounter -= 1; return; }
    // Опрос кнопки включения
    if (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == 0) return;

    // Переход к следующей задаче

    // Планируем позиционирование на 2 периода с хвостиком синхронного вращения
    FSM_TicCounter = (unsigned int)
        ((1.0 / START_F) * 2 / ((1 << 9) * TIMESTEP) + 2);
    // Модифицируем блок-схему
    pid_spd.gS = &Rotor.gS;
    Rotor.yPhi = &Rotor.uPhi;
    PMSM_CntrlUnit_Reset_IC();
    // Снимаем флаг блокировки вычислений
    isIRQHandlerLocked = RESET;
    // Включаем двигатель
    TIM_CtrlPWMOutputs(TIM1, ENABLE);
    // Меняем указатель на задачу
    Alpha_FSM_Task = &Task_motor_pos;
}

static void Task_motor_pos(void) {
    // Подсчет времени позиционирования
    if (FSM_TicCounter) { FSM_TicCounter -= 1; return; }

    // Переход к следующей задаче

    // Включаем Запускающий Интегратор
    isModorInSinxroMode = SET;
    // Планируем 7.2 периодов с хвостиком синхронного вращения
    FSM_TicCounter = (unsigned int)
        ((1.0 / START_F) * 7.2 / ((1 << 9) * TIMESTEP) + 8);
    // Меняем указатель на задачу
    Alpha_FSM_Task = &Task_motor_rot;
}

static void Task_motor_rot(void) {
    // Подсчет времени синхронного вращения
    if (FSM_TicCounter) { FSM_TicCounter -= 1; return; }

    // Переход к следующей задаче

    // Выключаем Запускающий Интегратор
    isModorInSinxroMode = RESET;
    // Модифицируем блок-схему
    pid_spd.gS = &SetPnt.gS;
    Rotor.yPhi = &psi.phi;
    // Меняем указатель на задачу
    Alpha_FSM_Task = &Task_motor_run;
}

static void Task_motor_run(void) {
    // Опрос кнопки включения
    if (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == 0) return;

    // Переход к следующей задаче

    // Планируем блокировку кнопки на 1 сек
    FSM_TicCounter = 17; // * 58 mS
    // Поднимаем флаг блокировки вычислений
    isIRQHandlerLocked = SET;
    // Выключаем двигатель
    TIM_CtrlPWMOutputs(TIM1, DISABLE);
    // Меняем указатель на задачу
    Alpha_FSM_Task = &Task_motor_off;
}

/*******************************************************************************
* Function Name : RCC_Configuration
* Description :   Reset and Clock Control.
*******************************************************************************/
static void RCC_Configuration(void) {
#ifdef NONE_SYSTEM_STM32F30X_C
    // Сбрасываем состояние системного генератора к начальному
    RCC_DeInit();       // RCC system reset (for debug purpose)
    // Включаем внешний высокочастотный кварцевый генератор
    RCC_HSEConfig(RCC_HSE_ON);

    // Ожидаем выхода генератора на рабочий режим
    if (RCC_WaitForHSEStartUp() == !SUCCESS) {
        while (1) {} // неисправен тактовый генератор
    }
    // Активируем буфер упреждающей выборки для флеш-памяти
    FLASH_PrefetchBufferCmd(ENABLE);
    // Определим задержку для Флеш-памяти
    FLASH_SetLatency(FLASH_Latency_2);

    // Настроим тактовый генератор процессора,
    //      см. док: DM00043574.pdf, стр. 127, Figure 13
    // Конфигурируем делитель для: ядра, памяти, AHB-шины, ПДП
    RCC_HCLKConfig(RCC_SYSCLK_Div1);        // HCLK = SYSCLK
    // Конфигурируем делитель для высокоскоростной периферии
    RCC_PCLK2Config(RCC_HCLK_Div1);         // PCLK2 = HCLK
    // Конфигурируем делитель для низкоскоростной периферии
    RCC_PCLK1Config(RCC_HCLK_Div2);         // PCLK1 = HCLK/2

    // Установим коэффициент умножения частоты кварца
    // для системного генератора PLLCLK = 8MHz * 9 = 72 MHz
    RCC_PLLConfig(RCC_PLLSource_PREDIV1, RCC_PLLMul_9);
    // Включаем контур ФАПЧ (PLL) системного генератора
    RCC_PLLCmd(ENABLE);
    // Ожидаем завершения переходного процесса в контуре ФАПЧ
    while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET) {}
    // Выбираем контур ФАПЧ в качестве системного генератора
    RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK); // SYSCLK = PLLCLK
    // Ожидаем выхода на рабочий режим всех делителей
    while (RCC_GetSYSCLKSource() != 0x08) {}
#endif

    // Настроим делитель частоты системного генератора для АЦП12
    RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div1); // ADCCLK = PLLCLK

    // Включаем тактирование устройств на системной AHB-шине с высокой
    // пропускной способностью (Advanced High-performance Bus)
    RCC_AHBPeriphClockCmd(
        RCC_AHBPeriph_GPIOA |   // порт A
        RCC_AHBPeriph_GPIOB |   // порт B
        RCC_AHBPeriph_DMA1 |    // контроллер ПДП
        RCC_AHBPeriph_ADC12,    // АЦП1 и АЦП2
        ENABLE);
    // Включаем тактирование устройств на APB2-шине
    // высокоскоростной периферии: TIM1
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    // Включаем тактирование устройств на APB1-шине
    // низкоскоростной периферии: DAC1
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE);
}

/*******************************************************************************
* Function Name : NVIC_Configuration
* Description : Определяем таблицу векторов прерываний и настраиваем приоритеты
*******************************************************************************/
static void NVIC_Configuration(void) {
    NVIC_InitTypeDef NVIC_InitStructure;

#ifdef NONE_SYSTEM_STM32F30X_C
#ifdef VECT_TAB_SRAM
    // Разместим таблицу векторов прерываний в ОЗУ: 0x20000000
    NVIC_SetVectorTable(NVIC_VectTab_RAM, 0x0);
#else
    // Разместим таблицу векторов прерываний в ПЗУ: 0x08000000
    NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0);
#endif
#endif

    // Определим, как прерывания будут сгруппированы по приоритетам
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
    // Определим вектор прерывания для контроллера ПДП (DMA1_IT_TC1)
    NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel1_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
}

/*******************************************************************************
* Function Name : GPIO_Configuration
* Description : Configure the TIM1 Pins.
*******************************************************************************/
static void GPIO_Configuration(void) {
    GPIO_InitTypeDef GPIO_InitStructure;
    // Запросим параметры по умолчанию для терминалов
    GPIO_StructInit(&GPIO_InitStructure);
    // Подправим одно из значений по умолчанию
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    // Порт A: Терминал 5 - аналоговый выход для DAC1_OUT2
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_5;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    // Порт A: Терминал 3 - аналоговый вход для PA3 === ADC1_IN4
    // Порт A: Терминал 4 - аналоговый вход для PA4 === ADC2_IN1
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_4;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    // Порт A: Терминал 1 - цифровой вход подключенный к кнопке
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    // Порт B: Терминал 11 для контроля осциллографом при отладке
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    // Порт A: Терминалы 8, 9, 10 конфигурируем для вывода PWM
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    // Порт B: Терминалы 13, 14, 15 конфигурируем для вывода PWM
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    // Подключаем терминалы к альтернативным источникам сигнала (TIM1_PWM)
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource8, GPIO_AF_6);  // TIM1_CH1
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource9, GPIO_AF_6);  // TIM1_CH2
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource10, GPIO_AF_6); // TIM1_CH3
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource13, GPIO_AF_6); // TIM1_CH1N
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource14, GPIO_AF_6); // TIM1_CH2N
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource15, GPIO_AF_4); // TIM1_CH3N
}

/*******************************************************************************
* Function Name : TIM_Configuration
* Description :   Конфигурируем тоймер TIM1
*     Генерируем 3 пары комплементарных ШИМ-сигналов с бестоковой паузой (для
*     контроля состояния ключей стоек полупроводникового 3-х фазного моста).
*     При изменении частоты ШИМ-драйвера меняется максимальное значение,
*     до которого таймер считает тактирующие импульсы. Уставки для регистров
*     сравнения должны учитывать разрешение ШИ-модулятора (должны быть меньше).
*     Для их масштабирования предложен макрос _Q15toBASE, которому, в качестве
*     аргументов, передается выборка и параметр PWM_RESOLUTION.
*******************************************************************************/
static void TIM_Configuration(void) {
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    TIM_OCInitTypeDef       TIM_OCInitStructure;
    TIM_BDTRInitTypeDef     TIM_BDTRInitStructure;
    // Настроим тактирование и счетчик таймера
    //
    // 1. Настроим двоичный делитель тактирующей частоты
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    // 2. Настроим поглощающий счетчик счетных импульсов
    TIM_TimeBaseStructure.TIM_Prescaler = 0;
    // 3. Активируем режим реверсивного счёта (inc/dec)
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1;
    // 4. Установим максимальное число для счёта
    TIM_TimeBaseStructure.TIM_Period = PWM_COUNTER - 1;
    // 5. Настроим поглощающий прерывания счетчик реверсов
    TIM_TimeBaseStructure.TIM_RepetitionCounter = 2 - 1;
    // и как ... сконфигурируем!
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);

    // Настроим выходы каналов Захвата / Сравнения таймера
    //
    // 1. Установим режим сравнения CCR >= CNT или CCR <= CNT
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    // 2. Активируем выход OCx Регистра Сравнения (верхний ключ)
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    // 3. Активируем выход OCNx Регистра Сравнения (нижний ключ)
    TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
    // 4. Установим полярность сигнала OCx для верхнего ключа
    TIM_OCInitStructure.TIM_OCPolarity =
        SW_H_POLARITY ? TIM_OCPolarity_High : TIM_OCPolarity_Low;
    // 5. Установим полярность сигнала OCNx для нижнего ключа
    TIM_OCInitStructure.TIM_OCNPolarity =
        SW_L_POLARITY ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
    // 6. Определим состояние выходов OCx  для IDLE режима
    TIM_OCInitStructure.TIM_OCIdleState =
        SW_H_IDLESTATE ? TIM_OCIdleState_Set : TIM_OCIdleState_Reset;
    // 7. Определим состояние выходов OCNx для IDLE режима
    TIM_OCInitStructure.TIM_OCNIdleState =
        SW_L_IDLESTATE ? TIM_OCNIdleState_Set : TIM_OCNIdleState_Reset;
    // Установим число в Регистра Сравнения = половину от ARR
    TIM_OCInitStructure.TIM_Pulse = (PWM_RESOLUTION >> 1) - 1;
    // TIM1: Конфигурируем каналы: 1, 2, 3 (заполение 50%)
    TIM_OC1Init(TIM1, &TIM_OCInitStructure);
    TIM_OC2Init(TIM1, &TIM_OCInitStructure);
    TIM_OC3Init(TIM1, &TIM_OCInitStructure);

    // Настроим защиту моста (ST.com: DM00080497.pdf DM00042534.pdf)
    //
    // 1. Определим величину бестоковой паузы
    TIM_BDTRInitStructure.TIM_DeadTime = 54; // 54 / 72e6 = 0.75 uS
    // 2. [Де]Активируем Компаратор Защиты
    TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
    // 3. Укажем активный уровень для Компаратора Защиты
    TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
    // 4. Установим уровень срабатывания Компаратора Защиты
    TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_1;
    // 5. Не используем однотактный режим управления мостом (Run-режим)
    TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
    // 6. Не отключаем таймер от выходов при выключении ШИМ-а (Idle-режим)
    TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
    // 7. Не используем автоматическое включение после срабатывания защиты
    TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
    // и как ... сконфигурируем!
    TIM_BDTRConfig(TIM1, &TIM_BDTRInitStructure);

    // TIM1: Подаем на Триггер Событий (TRGO) сигнал обновления
    TIM_SelectOutputTrigger(TIM1, TIM_TRGOSource_Update);
}

/*******************************************************************************
* Function Name : ADC_Configuration
* Description :   Измеряемые величины: iU, iV, Udc, ...
*     Два АЦП мК конфигурируем для выполнения "Регулярных" измерений.
*     Запуск АЦП1 и АЦП2 производиться синхронно, по триггеру таймера
*     TIM1_TRGO, который срабатывает при обновлении счетчика.
*     По готовности результата оба АЦП формируют запрос на обслуживание
*     контроллеру ПДП. Который пересылает результаты в ОЗУ (в массив).
*******************************************************************************/
static void ADC_Configuration(void) {
    ADC_CommonInitTypeDef   ADC_CommonInitStructure;
    ADC_InitTypeDef         ADC_InitStructure;

    // Выполним процедуру калибровки АЦП1 и АЦП2
    //
    // Включим регулятор напряжения ???
    ADC_VoltageRegulatorCmd(ADC1, ENABLE);
    ADC_VoltageRegulatorCmd(ADC2, ENABLE);
    // Подождем 10 uS
    for (u32 i = 720; i != 0; i--); // Delay(10);
    // Выберем режим калибровки для АЦП1
    ADC_SelectCalibrationMode(ADC1, ADC_CalibrationMode_Single);
    ADC_StartCalibration(ADC1);
    // Выберем режим калибровки для АЦП2
    ADC_SelectCalibrationMode(ADC2, ADC_CalibrationMode_Single);
    ADC_StartCalibration(ADC2);
    // Ожидаем завершения калибровки АЦП1
    while (ADC_GetCalibrationStatus(ADC1) != RESET);
    adc.Single[4] = ADC_GetCalibrationValue(ADC1);
    // Ожидаем завершения калибровки АЦП2
    while (ADC_GetCalibrationStatus(ADC2) != RESET);
    adc.Single[5] = ADC_GetCalibrationValue(ADC2);

    // АЦП1 и АЦП2: сконфигурируем для работы в паре
    //  DM00043574.pdf, стр. 358, 15.3.29 Dual ADC modes
    // Режим преобразований: Независимый или Альтернативы ...
    ADC_CommonInitStructure.ADC_Mode = ADC_Mode_RegSimul;
    // Выберем тактовый генератор высокоскоростной AHB-шины
    ADC_CommonInitStructure.ADC_Clock = ADC_Clock_SynClkModeDiv1;
    // Конфигурируем выдачу данных для контроллера ПДП
    ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1;
    // Укажем режим выдачи данных контроллеру ПДП
    ADC_CommonInitStructure.ADC_DMAMode = ADC_DMAMode_Circular;
    // Укажем величину задержки между стадиями Выборки
    ADC_CommonInitStructure.ADC_TwoSamplingDelay = 10;
    // и как ... настроим!
    ADC_CommonInit(ADC1, &ADC_CommonInitStructure);

    // Триггеры для запуска АЦП перечислены в документе
    // DM00043574.pdf, стр. 329, Table 89.
    // См. так же параграф 20.3.25 ADC synchronization, стр. 563

    // Для АЦП Настроим:
    ADC_StructInit(&ADC_InitStructure);
    // 1. Разрешение в битах
    ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
    // 2. Выравнивание данных
    ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
    // 3. Триггер внешней синхронизации запуска (TIM1, TRGO)
    ADC_InitStructure.ADC_ExternalTrigConvEvent = ADC_ExternalTrigConvEvent_9;
    // 4. Запускающий АЦП фронт триггера
    ADC_InitStructure.ADC_ExternalTrigEventEdge = ADC_ExternalTrigEventEdge_RisingEdge;
    // 5. Длину пакета преобразований
    ADC_InitStructure.ADC_NbrOfRegChannel = 1;
    // 6. Режим преобразования: по событиям или непрерывный
    ADC_InitStructure.ADC_ContinuousConvMode = ADC_ContinuousConvMode_Disable;
    // 7. Режим автоинжекции преобразований после Регулярной группы
    ADC_InitStructure.ADC_AutoInjMode = ADC_AutoInjec_Disable;
    // 8. Режим блокирования новых результатов, если старые не считаны
    ADC_InitStructure.ADC_OverrunMode = ADC_OverrunMode_Disable;
    // и как ... настроим!
    ADC_Init(ADC1, &ADC_InitStructure);
    ADC_Init(ADC2, &ADC_InitStructure);

    // ADC12: Укажем источник сигнала, позицию в секвенсоре, и время выборки
    // PC1 === ADC1_IN7, позиция в секвенсоре, и время выборки
    // PA3 === ADC1_IN4, позиция в секвенсоре, и время выборки
    ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 1, ADC_SampleTime_7Cycles5);
    // PC1 === ADC2_IN7, позиция в секвенсоре, и время выборки
    // PB2 === ADC2_IN12, позиция в секвенсоре, и время выборки
    // PA4 === ADC2_IN1, позиция в секвенсоре, и время выборки
    ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_7Cycles5);

    // Конфигурируем режим запроса канала ПДП
    ADC_DMAConfig(ADC1, ADC_DMAMode_Circular);
    // Разрешаем запросы канала ПДП для АЦП1
    ADC_DMACmd(ADC1, ENABLE);

    ADC_Cmd(ADC1, ENABLE);      // Включаем АЦП1
    ADC_Cmd(ADC2, ENABLE);      // Включаем АЦП2
    // Ожидаем флаг готовности АЦП1
    while (!ADC_GetFlagStatus(ADC1, ADC_FLAG_RDY));
    // Ожидаем флаг готовности АЦП2
    while (!ADC_GetFlagStatus(ADC2, ADC_FLAG_RDY));
    // Дадим волшебный пендель, чтоб все поехало
    ADC_StartConversion(ADC1);
    while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_RDY));
    ADC_StartConversion(ADC2);
    while(!ADC_GetFlagStatus(ADC2, ADC_FLAG_RDY));
}

/*******************************************************************************
* Function Name : DMA_Configuration
* Description :   Настройка 1-ого канала 1-ого контроллера ПДП (DMA1 Channel1)
*     Контроллер прямого доступа в память настраиваем в режим кольцевого
*     буфера для результатов преобразования АЦП. При каждом запросе от
*     ведущего АЦП, контроллер ПДП переносит в память одно 32-х разрядное
*     слово (в котором результаты преобразования двух АЦП), инкрементирует
*     адрес ОЗУ и переходит в режим ожидания следующего запроса.
*     Когда буфер заполняется, передача данных считается завершенной,
*     контроллер формирует прерывание (накапливаются 2 результата).
*******************************************************************************/
static void DMA_Configuration(void) {
    DMA_InitTypeDef DMA_InitStructure;
    // Укажем адрес периферийного устройства (УВВ) т.е. АЦП
    DMA_InitStructure.DMA_PeripheralBaseAddr = ADC_CDR_ADDRESS;
    // Укажем адрес массива в ОЗУ (для результатов преобразования)
    DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&adc;
    // Укажем направление передачи данных: из АЦП в ОЗУ
    DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
    // Укажем размер массива для результатов преобразования
    DMA_InitStructure.DMA_BufferSize = 2; // копим 2 результата
    // Укажем, что адрес УВВ (АЦП) инкрементировать не нужно
    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
    // Укажем, что адрес в ОЗУ необходимо инкрементировать
    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
    // Укажем размер данных по указанному адресу периферии
    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
    // Укажем размер данных по указанному адресу в ОЗУ
    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
    // Укажем режим кольцевого буфера для канала ПДП
    DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
    // Установим приоритет для канала ПДП
    DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
    // Укажем, что канал не используется для передачи данных из ОЗУ в ОЗУ
    DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
    // Инициализируем канал контроллера ПДП
    DMA_Init(DMA1_Channel1, &DMA_InitStructure);
    // Сбрасываем флаг прерывания по завершению передачи
    DMA_ClearFlag(DMA1_FLAG_TC1);
    // Разрешаем прерывание по завершению передачи
    DMA_ITConfig(DMA1_Channel1, DMA_IT_TC, ENABLE);
    // Включаем канал контроллера ПДП
    DMA_Cmd(DMA1_Channel1, ENABLE);
}

/*******************************************************************************
* Function Name : DAC_Config
* Description :   Конфигурируем 2-ой канал ЦАП-а
*******************************************************************************/
static void DAC_Config(void) {
    DAC_InitTypeDef     DAC_InitStructure;
    // Запросим параметры по умолчанию для ЦАП-а
    DAC_StructInit(&DAC_InitStructure);
    // Укажем триггер внешней синхронизации
    DAC_InitStructure.DAC_Trigger = DAC_Trigger_None;
    // Деактивируем встроенный генератор шума и треугольника
    DAC_InitStructure.DAC_WaveGeneration = DAC_WaveGeneration_None;
    // Определим количество разрядов для встроенного генератора
    DAC_InitStructure.DAC_LFSRUnmask_TriangleAmplitude = DAC_LFSRUnmask_Bit0;
    // Деактивируем буферизирующий усилитель на выходе ЦАП-а
    DAC_InitStructure.DAC_OutputBuffer = DAC_OutputBuffer_Disable;
    // Инициализируем 2-ой канал 1-ого ЦАП-а
    DAC_Init(DAC_Channel_2, &DAC_InitStructure);
    // Включаем 2-ой канал 1-ого ЦАП-а PA.05
    DAC_Cmd(DAC_Channel_2, ENABLE);
}

/*******************************************************************************
* Function Name : PMSM_CntrlUnit_SetBlkParam
* Description :   ...
*     Процедура установки параметров математических блоков
*     (объектов программы), которые составляют ЦСУ для СДПМ.
*     Не беспокойтесь о делениях! Подобные выражения вычислит препроцессор
*     компилятора. Тут просто константы будут присваиваться переменным.
*******************************************************************************/
static void PMSM_CntrlUnit_SetBlkParam(void) {
    // Измеряемые величины на силовом модуле *********************************
    McKit.Vdc = _IQ(1.0);                                     // заглушка    *
    McKit.Idc = _IQ(0.0);                                     // заглушка    *
    McKit.Vm = SVPWM_ON ? _IQ(1.0 / M_SQRT3) : _IQ(0.5);      // заглушка    *
    McKit.uIs0 = _IQ(CS_U0);                                  //             *
    McKit.vIs0 = _IQ(CS_V0);                                  //             *
    // Подчиненный PI-регулятор q-проекции тока статора СДПМ (ось момента) ***
    pid_iq.Kp = _IQ(KP_C);                                    // тюнинг KK_C *
    pid_iq.Ki = _IQ(KP_C / TI_C * CURRENT_LOOP_TIMESTEP);     // не меняется *
    // Подчиненный PI-регулятор d-проекции тока статора СДПМ (ось поля) ******
    pid_id.Kp = _IQ(KP_C);                                    // тюнинг KK_C *
    pid_id.Ki = _IQ(KP_C / TI_C * CURRENT_LOOP_TIMESTEP);     // не меняется *
    // Наблюдатель потокосцеплений *******************************************
    psi.Rs = _IQ(RS);                                         // не меняется *
    psi.Ls = _IQ(LS);                                         // не меняется *
    psi.wf = _IQ(WF);                                         // тюнинг WF   *
    // Наблюдатель Скорости PMSM (Speed Estimator) ***************************
    se.TS_div_TE = _IQ(TIMESTEP / (LS / RS));                 // не меняется *
    se.Rs_mul_BC = _IQ((RS + R_DC / 2) * BASE_CURRENT);       // тюнинг R_DC *
    se.EMF2SPD_PU =  _IQ((1.0 / KE) / BASE_SPEED);            // тюнинг KE   *
    // Регулятор Скорости c Ограничителем ************************************
    pid_spd.Kp = _IQ(KP_S);                                   // мастер      *
    pid_spd.Ki = TI_S == 0 ? _IQ(0.0) :                       // мастер      *
        _IQ(KP_S / TI_S * SPEED_LOOP_TIMESTEP);               //             *
    pid_spd.Lx = _IQ(SAFE_I * WL * SPEED_LOOP_TIMESTEP);      // тюнинг WL   *
    pid_spd.Lu = _IQ(SAFE_I);                                 //             *
    // Глобальные уставки Цифровой системы управления для СДПМ (PMSM / BLAC) *
    SetPnt.gS = SAFE_S ? _IQ(SAFE_S) :                        //             *
        _IQ(IS * RS * (1.0 / KE) / BASE_SPEED);               // SAFE_SPEED  *
    SetPnt.gI = _IQ(SAFE_I);                                  //             *
    SetPnt.g0 = _IQ(0.0);                                     // 0 для PMSM  *
    SetPnt.SVPWM_Enable = SVPWM_ON ? ENABLE : DISABLE;        //             *
    // Параметры запускающего интегратора ************************************
    Rotor.gS  = _IQ(IS * RS * (1.0 / KE) / BASE_SPEED);       // SAFE_SPEED  *
}

/*******************************************************************************
* Function Name : PMSM_CntrlUnit_CreateWires
* Description :   ...
*     Процедура выполняет построение цифровой системы управления для
*     СДПМ (PMSM) из вычислительных модулей - экземпляров объектов.
*     Суть построения ЦСУ - определение схемы передачи аргументов.
*     Выход - это атрибут модуля (переменная принадлежит объекту).
*     Вход - не является атрибутом модуля (объект имеет указатели
*     для подключения к аргументам).
*******************************************************************************/
static void PMSM_CntrlUnit_CreateWires(void) {
    // Указатель на угловое положение вала
    Rotor.yPhi   = &psi.phi;        // Подключили к Наблюдателю потокосцеплений
    // Регуляторы Тока (РТ) для проекций тока статора
    pid_iq.gI    = &pid_spd.uS;     // Подключили к регулятору задающий сигнал
    pid_iq.yI    = &ab2dq.q;        // Подключили ОС - q-проекцию тока статора
    pid_id.gI    = &SetPnt.g0;      // Подключили к регулятору задающий сигнал
    pid_id.yI    = &ab2dq.d;        // Подключили ОС - d-проекцию тока статора
    // Инверсный преобразователь Парка
    dq2ab.d      = &pid_id.uV;      // Подключили выходной сигнал РТ для d-оси
    dq2ab.q      = &pid_iq.uV;      // Подключили выходной сигнал РТ для q-оси
    dq2ab.Sine   = &Rotor.Sine;     // Подключили ротатор к опорной синусоиде
    dq2ab.Cosine = &Rotor.Cosine;   // Подключили ротатор к опорной косинусоиде
    // Преобразователь числа фаз
    ab2uvw.a     = &dq2ab.a;        // Подключили Alpha-фазу к преобразователю
    ab2uvw.b     = &dq2ab.b;        // Подключили  Beta-фазу к преобразователю
    // Преобразователь числа фаз
    uvw2ab.u     = &McKit.uIs;      // Подключили датчик тока фазы "U" (UVW)
    uvw2ab.v     = &McKit.vIs;      // Подключили датчик тока фазы "V" (UVW)
    // Ротатор (Преобразователь Парка)
    ab2dq.a      = &uvw2ab.a;       // Подключили ток Alpha-фазы
    ab2dq.b      = &uvw2ab.b;       // Подключили ток  Beta-фазы
    ab2dq.Sine   = &Rotor.Sine;     // Подключили ротатор к опорной   синусоиде
    ab2dq.Cosine = &Rotor.Cosine;   // Подключили ротатор к опорной косинусоиде
    // Наблюдатель потокосцеплений
    psi.Vm       = &McKit.Vm;       // Подключили вход к датчику Напряжения
    psi.Va       = &dq2ab.a;        // Подключили Alpha-фазу к преобразователю
    psi.Vb       = &dq2ab.b;        // Подключили  Beta-фазу к преобразователю
    psi.Ia       = &uvw2ab.a;       // Подключили ток Alpha-фазы
    psi.Ib       = &uvw2ab.b;       // Подключили ток  Beta-фазы
    // Наблюдатель Скорости (Speed Estimator)
    se.Vm        = &McKit.Vm;       // НС: Подключили вход к датчику Напряжения
    se.Vq        = &pid_iq.uV;      // НС: Подключили вход к уставке Скважности
    se.Iq        = &ab2dq.q;        // НС: Подключили вход к датчику Тока
    // Регулятор Скорости (РС)
    pid_spd.gS   = &SetPnt.gS;      // Подключили Уставку Скорости
    pid_spd.yS   = &se.yS;          // Подключили Наблюдатель Скорости
    pid_spd.gI   = &SetPnt.gI;      // Подключили Уставку тока (момента)
}

/*******************************************************************************
* Function Name : PMSM_CntrlUnit_Reset_IC
* Description :   ...
*     Процедура сброса / предустановки Начальных Условий для элементов
*     с эффектом памяти (регистров задержки и интеграторов).
*     Если программа ЦСУ по блок-схеме собрана корректно, то для сброса
*     к начальному состоянию достаточно предустановить начальные условия
*     на регистрах задержки (на интеграторах). Остальные координаты будут
*     корректно рассчитаны от источников Уставок до Силового Моста.
*******************************************************************************/
static void PMSM_CntrlUnit_Reset_IC(void) {
    // 1. Сбросим начальные условия на блоках с эффектом памяти (Reset IC)
    Rotor.uPhi = 0;
    pid_iq.zo = pid_id.zo = 0;
    psi.zoA = psi.zoB = 0;
    se.zo = pid_spd.zo = 0;
    // 2. Поскольку алгоритм системы управления предполагает
    // разные периоды дискретизации для модулей, и закрепленные
    // за ними вычисления могут быть выполнены не при первом
    // вызове MainISR. Предустановим их выходы.
    se.yS = pid_spd.uS = 0; // pid_iq.uV = pid_id.uV =
    // 3. Сделаем знак Уставки скорости для синхронного режима
    // запуска таким же, как у главной Уставки скорости.
    if (SetPnt.gS > 0 && Rotor.gS < 0 || SetPnt.gS < 0 && Rotor.gS > 0) {
        Rotor.gS = -Rotor.gS;
    }
}

/*******************************************************************************
* Function Name : DMA1_Channel1_IRQHandler
* Description :   Самая главная процедура программы (обработка прерывания)
*     Лучше всего сконфигурировать функционирование Таймера, АЦП,
*     и контроллера ПДП, так чтобы прерывание, которое формирует последний,
*     не приходилось прореживать программно. Т.е. чтобы период вызова
*     соответствующей функции совпадал с требуемы шагом дискретизации
*     цифровой системы управления (TIMESTEP).
*     Поглощающий прерывания / события счетчик реверсов Таймера лучше
*     не использовать. Рекомендуется увеличить буфер АЦП для результатов
*     оцифровки сигналов. И корректно настроить котроллер ПДП.
*     В этом случае результаты оцифровки можно усреднять.
*******************************************************************************/
#pragma optimize=speed
void DMA1_Channel1_IRQHandler(void) {
    // ------------------------------------------------------------------------------
    //  Инкрементируем виртуальный таймер и ограничиваем счет 15-ю битами
    // ------------------------------------------------------------------------------
    VirtualTimer++; // VirtualTimer &= 0x00007FFF;

    if (isIRQHandlerLocked) {
        McKit.uIs0 += _IQmpy(((long) adc.Single[6] << (GL_Q - 13)) - McKit.uIs0, _IQ(0.2 * TIMESTEP));
        McKit.vIs0 += _IQmpy(((long) adc.Single[7] << (GL_Q - 13)) - McKit.vIs0, _IQ(0.2 * TIMESTEP));
        DMA_ClearITPendingBit(DMA1_IT_TC1);
        return;
    }

    GPIOB->ODR ^= GPIO_Pin_11; // Перевернем бит порта // +0.18 uS
    // ------------------------------------------------------------------------------
    //  Увеличим угол для синхронного вращения (Start mode)
    // ------------------------------------------------------------------------------
    if (isModorInSinxroMode) {
        Rotor.uPhi += SetPnt.gS > 0
            ? _IQ(START_F * TIMESTEP) : -_IQ(START_F * TIMESTEP);
        if (Rotor.uPhi > _IQ(1.0)) Rotor.uPhi -= _IQ(1.0);
        if (Rotor.uPhi < _IQ(0.0)) Rotor.uPhi += _IQ(1.0);
    }
    // ------------------------------------------------------------------------------
    //  Для текущего Углового положения ротора вычислим sin / cos
    // ------------------------------------------------------------------------------
    ROTOR_GetTrigonometic(&Rotor);
    // +2.2 uS

    // ------------------------------------------------------------------------------
    //  Приведём результаты измерений фазных токов к отн. единицам [-1.0, +1.0]
    // ------------------------------------------------------------------------------
    // Сложим пары униполярных значений (для усреднения по каналам),
    adc.Dual[3] = adc.Dual[0] + adc.Dual[1]; // тип данных результата: _iq12
    McKit.uIs = _IQmpy2((((long) adc.Single[6]) << (GL_Q - 13)) - McKit.uIs0);
    McKit.vIs = _IQmpy2((((long) adc.Single[7]) << (GL_Q - 13)) - McKit.vIs0);
    //McKit.Vdc = (((long) AdcResult.ADCRESULT3) << (GL_Q - 13)) - _IQ(CS_DC0);
    // +1.5 uS
    // ------------------------------------------------------------------------------
    //  Вычислим максимальную амплитуду фазного напряжения (pu), [0.0, 1.0]
    // ------------------------------------------------------------------------------
    McKit.Vm = SetPnt.SVPWM_Enable
        ? _IQmpy(McKit.Vdc, _IQ(1.0 / M_SQRT3)) : _IQdiv2(McKit.Vdc);
    // ------------------------------------------------------------------------------
    //  Преобразуем токи 3-x фазной системы в токи эквивалентной 2-x фазной
    // ------------------------------------------------------------------------------
    uvw2ab.a = *uvw2ab.u;
    uvw2ab.b = _IQmpy((*uvw2ab.u + _IQmpy2(*uvw2ab.v)), _IQ(1.0 / M_SQRT3));
    // +1.2 uS
    // ------------------------------------------------------------------------------
    //  Перейдем от 2-x фазной системы токов к постояному току (к d- q-проекциям)
    // ------------------------------------------------------------------------------
    ab2dq.d = _IQmpy(*ab2dq.a, *ab2dq.Cosine) + _IQmpy(*ab2dq.b, *ab2dq.Sine);
    ab2dq.q = _IQmpy(*ab2dq.b, *ab2dq.Cosine) - _IQmpy(*ab2dq.a, *ab2dq.Sine);
    // +1.4 uS

    // ------------------------------------------------------------------------------
    //  Вызываем макросы ПИ-регулятора контура скорости, и обновления пределов Уставки тока
    // ------------------------------------------------------------------------------
    if (pid_spd.Lu != *pid_spd.gI) {
        pid_spd.Lu = *pid_spd.gI;
        pid_spd.Lx = _IQmpy(*pid_spd.gI, _IQ(WL * SPEED_LOOP_TIMESTEP));
    }
    SPD_PID_MACRO(pid_spd);
    // Если Регулятор Скорости не используется, обнулим Начальное Условие на 1/s
    //if (is_pHystReversMode) pid_spd.zo = 0;

    // ------------------------------------------------------------------------------
    //  Вызываем макросы ПИ-регуляторов контура тока
    // ------------------------------------------------------------------------------
    DQ_PID_MACRO(pid_iq);   // Моментная q-ось
    DQ_PID_MACRO(pid_id);   // Намагничивающая d-ось
    // +4.7 uS

    // ------------------------------------------------------------------------------
    //  Преобразуем управляющие сигналы Регуляторов тока в 2-x фазную систему напряжений
    // ------------------------------------------------------------------------------
    dq2ab.a = _IQmpy(*dq2ab.d, *dq2ab.Cosine) - _IQmpy(*dq2ab.q, *dq2ab.Sine);
    dq2ab.b = _IQmpy(*dq2ab.q, *dq2ab.Cosine) + _IQmpy(*dq2ab.d, *dq2ab.Sine);
    // +1.4 uS
    // ------------------------------------------------------------------------------
    //  Преобразуем 2-x фазную систему напряжений в 3-x фазную (для питания электромотора)
    // ------------------------------------------------------------------------------
    _iq temp_v1 = _IQdiv2(*ab2uvw.a);
    _iq temp_v2 = _IQmpy(*ab2uvw.b, _IQ(M_SQRT3 / 2.0));
    ab2uvw.u = -(*ab2uvw.a);
    ab2uvw.v = temp_v1 - temp_v2;
    ab2uvw.w = temp_v1 + temp_v2;
    // +0.9 uS

    // ------------------------------------------------------------------------------
    //  Преобразуем 3-x фазную синусоидальную последовательность в ... и будет SVPWM
    // ------------------------------------------------------------------------------
    if (SetPnt.SVPWM_Enable) {
        if (ab2uvw.u < ab2uvw.v) {
            if (ab2uvw.u < ab2uvw.w) {
                ab2uvw.v = ab2uvw.v - (_IQ(1.0) + ab2uvw.u);
                ab2uvw.w = ab2uvw.w - (_IQ(1.0) + ab2uvw.u);
                ab2uvw.u = _IQ(-1.0);
            } else {
                ab2uvw.u = ab2uvw.u - (_IQ(1.0) + ab2uvw.w);
                ab2uvw.v = ab2uvw.v - (_IQ(1.0) + ab2uvw.w);
                ab2uvw.w = _IQ(-1.0);
            }
        } else if (ab2uvw.v < ab2uvw.w) {
            ab2uvw.w = ab2uvw.w - (_IQ(1.0) + ab2uvw.v);
            ab2uvw.u = ab2uvw.u - (_IQ(1.0) + ab2uvw.v);
            ab2uvw.v = _IQ(-1.0);
        } else if (ab2uvw.w < ab2uvw.v) {
            ab2uvw.u = ab2uvw.u - (_IQ(1.0) + ab2uvw.w);
            ab2uvw.v = ab2uvw.v - (_IQ(1.0) + ab2uvw.w);
            ab2uvw.w = _IQ(-1.0);
        } else {
            ab2uvw.u = _IQ(-1.0);
            ab2uvw.v = _IQ(-1.0);
            ab2uvw.w = _IQ(-1.0);
        }
    }
    // SVM: +1.9 .. +2.4 uS

    // ------------------------------------------------------------------------------
    //  Ограничиваем сигналы (чтоб не было перегрузки в макросе _IQtoIQ15)
    // ------------------------------------------------------------------------------
    if (ab2uvw.u > _IQ(0.99999)) ab2uvw.u = _IQ(0.99999);
    if (ab2uvw.u < _IQ(-1.0000)) ab2uvw.u = _IQ(-1.0000);
    if (ab2uvw.v > _IQ(0.99999)) ab2uvw.v = _IQ(0.99999);
    if (ab2uvw.v < _IQ(-1.0000)) ab2uvw.v = _IQ(-1.0000);
    if (ab2uvw.w > _IQ(0.99999)) ab2uvw.w = _IQ(0.99999);
    if (ab2uvw.w < _IQ(-1.0000)) ab2uvw.w = _IQ(-1.0000);
    // +1.5 uS | SVM: +1.6 .. +1.82 uS | PWM: +1.68 .. +1.86 uS
    // ------------------------------------------------------------------------------
    //  Обновляем Регистры Сравнения 3-х каналов Таймера
    // ------------------------------------------------------------------------------
    TIM_SetCompare1(TIM1, _Q15toBASE(_IQtoIQ15(ab2uvw.u), PWM_RESOLUTION));
    TIM_SetCompare2(TIM1, _Q15toBASE(_IQtoIQ15(ab2uvw.v), PWM_RESOLUTION));
    TIM_SetCompare3(TIM1, _Q15toBASE(_IQtoIQ15(ab2uvw.w), PWM_RESOLUTION));
    // +2.0 uS
    // ------------------------------------------------------------------------------
    //  Вычисляем потокосцепления и угол
    //  Внимание! Абсолютные величины
    // ------------------------------------------------------------------------------
    // TODO: Оптимизировать количество операций
    _iq aVm = _IQmpy(_IQ(BASE_VOLTAGE), *psi.Vm);       // абсолютная величина Vm
    _iq Va = _IQmpy(*psi.Va, aVm);                      // aVa = скважность * aVm
    _iq Vb = _IQmpy(*psi.Vb, aVm);                      // aVb = скважность * aVm
    _iq Ia = _IQmpy(*psi.Ia, _IQ(BASE_CURRENT));        // абсолютная величина тока
    _iq Ib = _IQmpy(*psi.Ib, _IQ(BASE_CURRENT));        // абсолютная величина тока
    _iq psi_ziA = Va - _IQmpy(psi.zoA, psi.wf) - _IQmpy(Ia, psi.Rs);
    psi_ziA = _IQmpy(psi_ziA, _IQ(CURRENT_LOOP_TIMESTEP));
    psi.a = psi.zoA + _IQdiv2(psi_ziA) - _IQmpy(Ia, psi.Ls);
    psi.zoA += psi_ziA;
    _iq psi_ziB = Vb - _IQmpy(psi.zoB, psi.wf) - _IQmpy(Ib, psi.Rs);
    psi_ziB = _IQmpy(psi_ziB, _IQ(CURRENT_LOOP_TIMESTEP));
    psi.b = psi.zoB + _IQdiv2(psi_ziB) - _IQmpy(Ib, psi.Ls);
    psi.zoB += psi_ziB;
    // +5.6 uS
    psi.phi =_IQatan2(psi.b, psi.a);                    // угол [-M_PI, +M_PI]
    psi.phi = _IQmpy(psi.phi, _IQ(1.0 / (2 * M_PI)));
    psi.phi = psi.phi >= 0 ? psi.phi : _IQ(1.0) + psi.phi; // привели к PU [0..1]
    // +3.0 uS

    // ------------------------------------------------------------------------------
    //  Наблюдатель Скорости (Speed Estimator) = 1.2 мкС
    //  Внимание! Абсолютные величины
    // ------------------------------------------------------------------------------
    // TODO: Убрать константы (весовые коэффициенты) из структуры наблюдателя
    if (1) {
        _iq se_zi;                // Вход регистра задержки апериодического звена
        se_zi = _IQmpy(_IQ(BASE_VOLTAGE), *se.Vm);      // абсолютная величина Vm
        se_zi = _IQmpy(se_zi, *se.Vq);                  // aVq = скважность * aVm
        se_zi = _IQmpy((se_zi - se.zo), se.TS_div_TE);  // еще не zi
        se_zi = se_zi + se.zo;                          // 1/s: Значение на входе
        se.yS = se_zi - _IQmpy(*se.Iq, se.Rs_mul_BC);   // противо-ЭДС (b-EMF), В
        se.yS = _IQmpy(se.yS, se.EMF2SPD_PU);           // Speed / BASE_SPEED, pu
        // ------------------------------------------------------------
        se.zo = se_zi;
    }

    // ------------------------------------------------------------------------------
    //  Используем ЦАП для контроля координат ЦСУ осциллографом
    // ------------------------------------------------------------------------------
    // Шаблон преобразования целого числа A [0..3600] к полной шкале ЦАПа (12bit)
    // _iq dd = (((long) A) << (GL_Q - 11)) - (_IQ(3600.0 / 4096 / 2) << 1);
    // dd = _IQmpy(dd, _IQ(4096.0 / 3600)) + _IQ(1.0); // результат GL_Q: [0.0, 2.0]
    _iq psi_phi = psi.phi > _IQ(0.99999) ? _IQ(0.99999) : psi.phi;
    _iq signal = _IQmpy(McKit.uIs, _IQ(4)) + _IQ(1.0);
    DAC_SetChannel2Data(DAC_Align_12b_L, _IQtoIQ15(VirtualTimer & 1 ? signal :
        SetPnt.gI ? psi_phi : _IQ(1.0))); // осциллограф к контакту PA.05

    DMA_ClearITPendingBit(DMA1_IT_TC1);
    GPIOB->ODR ^= GPIO_Pin_11; // Перевернем бит порта // +0.18 uS
    // Без оптимизации кода время исполнения функции:
    // PWM: 28 .. 29 uS | SVM: (PWM_TIME + (1.4 .. 1.9)) uS
    // С оптимизацией по скорости: PWM: 13.2 uS
}

#ifdef USE_FULL_ASSERT
/*******************************************************************************
* Function Name :       assert_failed
* Description :         Отчеты об имени исходного файла и о номере строки,
*                       где в режиме отладки произошла assert_param ошибка.
* Input : - file :      Указатель на имя исходного файла
*         - line :      Номер строки кода с ошибкой assert_param
* Output :              None
* Return :              None
*******************************************************************************/
void assert_failed(u8 * file, u32 line) {
    // Пользователь может определить собственное сообщение об имени файла
    // и номере строки.
    // Пример:
    // printf("Ошибка величины параметра: file %s on line %d\r\n", file, line);
    while (1) {}
}
#endif

/************************ END OF FILE *****************************************/

7.07.2015