В этой статье рассмотрим систему стабилизации квадрокоптера.  Статья вдохновлена Программируем квадрокоптер на Arduino (часть 1) . Приступим. Начнем с настройки интерфейсов и таймеров мк. Далее короткое описание функций кода и вывод.Полный код проекта снабжен комментариями которые оформлены под стиль комментариев в HAL.

Настроим четыре канала у TIM1 и включим DMA он будет управлять ESC по Multishot. Multishot — это протокол передачи сигналов от полётного контроллера  к регулятору скорости ESC. Ширина импульсов 5 - 25 мкс.  ESC измеряет длительность импульса и преобразует её в скорость вращения двигателя. Чем длиннее импульс, тем выше обороты.

Сигнал Multishot :  
Мин: |‾‾‾|_______| (5 мкс)  
50%:  |‾‾‾‾‾‾‾|____| (15 мкс)  
Макс:  |‾‾‾‾‾‾‾‾‾‾‾‾| (25 мкс)  

Микроконтроллер настроен на 100MHz один тик таймера.T = 1/100 МГц = 0.01 мкс = 10 нс Время переполнения T_overflow = 3000 × 10 нс = 30 мкс В коде укажем минимальное количество тиков 500 это будет 5мкс и максимальное 2500 это будет 25мкс. Далее настроим TIM11 предделитель (Prescaler) таймера 99 -1 период (Period) 999 - 1. Тактовая частота 100 МГц. После предделителя 100 МГц / 100 = 1 МГц длительность одного тика 1 / 1 МГц = 1 мкс период переполнения 1000 × 1 мкс = 1000 мкс = 1 мс частота прерываний 1 / 1 мс = 1000 Гц (1 кГц).

Таймер TIM11 будет генерировать прерывания с частотой 1 кГц, и в обработчике прерывания (функции обратного вызова) будет вызываться основной цикл управления: run_control_loop():

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
    if (htim == &htim11) {
        run_control_loop(); // Вызов всей логики управления
    }
}

В качестве IMU используется BMX055 подключенный по I2C интерфейс настроен на быстрый режим.

В качестве радиомодуля E220-400T22D подключенный по UART6 для которого включено DMA.

Настройки E220-400T22D производились из программы от EBYTE.

Все настройки по умолчанию. Модуль заработал сразу как только был подключен.

Инициализация фильтров и алгоритмов управления.

После инициализации IMU (BMX055) в коде выполняется настройка нескольких типов фильтров и ПИД-регуляторов:

1. Фильтры низких частот (LPF)

Используется каскадная структура биквадратных фильтров (Biquad) из библиотеки CMSIS-DSP:

arm_biquad_cascade_df2T_init_f32(&lpf_Gx, NUM_STAGES, Coeffs_lpf, filterState_lpf_Gx);
arm_biquad_cascade_df2T_init_f32(&lpf_Gy, NUM_STAGES, Coeffs_lpf, filterState_lpf_Gy);
arm_biquad_cascade_df2T_init_f32(&lpf_Gz, NUM_STAGES, Coeffs_lpf, filterState_lpf_Gz);

Всего создается 9 экземпляров фильтров:

• 3 для осей гироскопа (Gx, Gy, Gz)

• 3 для осей акселерометра (Ax, Ay, Az)

• 3 для ошибок положения (Errorx, Errory, Errorz)

2. Фильтры Калмана

Инициализируются отдельно для каждого канала:

kalman_filter_init(&gyro_filter_x, GYRO_PROCESS_NOISE, GYRO_MEASUREMENT_NOISE, GYRO_ESTIMATION_ERROR, GYRO_INITIAL_VALUE);
kalman_filter_init(&gyro_filter_y, GYRO_PROCESS_NOISE, GYRO_MEASUREMENT_NOISE, GYRO_ESTIMATION_ERROR, GYRO_INITIAL_VALUE);
kalman_filter_init(&gyro_filter_z, GYRO_PROCESS_NOISE, GYRO_MEASUREMENT_NOISE, GYRO_ESTIMATION_ERROR, GYRO_INITIAL_VALUE);

3. Режекторные фильтры (Notch)

Настроены для подавления конкретных частотных составляющих:

init_notch_filter(&notch_ax, NOTCH_CENTER_FREQ, NOTCH_WIDTH, NOTCH_SAMPLE_RATE);
init_notch_filter(&notch_ay, NOTCH_CENTER_FREQ, NOTCH_WIDTH, NOTCH_SAMPLE_RATE);
init_notch_filter(&notch_az, NOTCH_CENTER_FREQ, NOTCH_WIDTH, NOTCH_SAMPLE_RATE);

4. Фильтры высоких частот (HPF)

Используются для устранения постоянной составляющей:

HPF_Init(&hpf_x, HPF_ALPHA, Gx[0]);
HPF_Init(&hpf_y, HPF_ALPHA, Gy[0]);
HPF_Init(&hpf_z, HPF_ALPHA, Gz[0]);

5. ПИД-регуляторы

Инициализируются с заданными коэффициентами для каждого канала управления:

PID_Init(&pitch_pid, PITCH_PID_KP, PITCH_PID_KI, PITCH_PID_KD, ALPHA, ALPHA_DERIVATIVE, INTEGRAL_LIMIT, SCALE_FACTOR);
PID_Init(&roll_pid, ROLL_PID_KP, ROLL_PID_KI, ROLL_PID_KD, ALPHA, ALPHA_DERIVATIVE, INTEGRAL_LIMIT, SCALE_FACTOR);
PID_Init(&yaw_pid, YAW_PID_KP, YAW_PID_KI, YAW_PID_KD, ALPHA, ALPHA_DERIVATIVE, INTEGRAL_LIMIT, SCALE_FACTOR);

Основной цикл управления

После завершения инициализации переходим в основной цикл, где управление осуществляется через прерывание таймера TIM11 с частотой 1 кГц. В обработчике прерывания вызывается функция run_control_loop():

1. Проверка связи с пультом управления

check_connection_loss();

void check_connection_loss(void) {
    uint32_t current_time = HAL_GetTick();
    if ((current_time - last_received_time) > CONNECTION_TIMEOUT_MS) {
        connection_lost = true;
        // Сброс управляющих сигналов
        joystick_x = 0;
        joystick_y = 0;
        right_left = 0;
        potentiometer_value = 0;
        button = 0;
        // Установка минимальной тяги на моторах
        Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_1);
        Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_2);
        Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_3);
        Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_4);
    }
}

Получает текущее время системного таймера. Проверяет, превышает ли время с последнего успешного приема данных заданный таймаут. При потере связи устанавливает флаг connection_lost, сбрасывает все управляющие сигналы в ноль, устанавливает минимальные импульсы на всех моторах.

2. Прием сигнала управления

void receive_and_parse_data(int data_size) {
    calculated_checksum = 0;
    
    // Проверка минимального размера пакета
    if (data_size < 4) {
        return; // Недостаточно данных для разбора
    }
    
    // Вычисление контрольной суммы
    for (uint8_t i = 0; i < data_size - 2; i++) {
        calculated_checksum += buffer_message[i];
    }
    
    // Получение контрольной суммы из пакета
    received_checksum = (buffer_message[data_size - 2] << 8) | buffer_message[data_size - 1];
    
    // Проверка контрольной суммы
    if (received_checksum == calculated_checksum) {
        // Обновление времени последнего успешного получения данных
        last_received_time = HAL_GetTick();
        connection_lost = false;
        
        // Разбор данных управления:
        joystick_x = (int16_t)((buffer_message[0] << 8) | buffer_message[1];
        joystick_y = (int16_t)((buffer_message[2] << 8) | buffer_message[3];
        potentiometer_value = (buffer_message[4] << 8) | buffer_message[5];
        right_left = (int16_t)((buffer_message[6] << 8) | buffer_message[7];
        button = (buffer_message[8] << 8) | buffer_message[9];
    }
}

Пакет данных имеет следующую структуру (12 байт):

3. Получение и обработка данных с IMU

Функция get_filter_data_accel();

void get_filter_data_accel() {
    BMX055_Read_Accel(&hi2c1, &BMX055);
    
    Ax[0] = round_to(BMX055.Ax);
    Ax[0] = Ax[0] - offset_Ax;
    Ax[0] = apply_notch_filter(&notch_ax, Ax[0]);
    arm_biquad_cascade_df2T_f32(&lpf_Ax, Ax, filter_Ax, BLOCK_SIZE);
    filter_Ax[0] = kalman_filter_update(&accel_filter_x, filter_Ax[0]);
    
    // Аналогично для Ay и Az
    ...
}

Этапы обработки:

1. Чтение сырых данных с акселерометра через I2C

2. Округление значений до 2 знаков после запятой

3. Вычитание калибровочного смещения

4. Подавление узкополосных помех режекторным фильтром

5. Фильтрация низких частот биквадратным фильтром

6. Окончательная фильтрация фильтром Калмана

Функция get_filter_data_gyro();

void get_filter_data_gyro() {
    BMX055_Read_Gyro(&hi2c1, &BMX055);
    
    Gx[0] = round_to(BMX055.Gx);
    Gx[0] = Gx[0] - bias_Gx;
    Gx[0] = apply_notch_filter(&notch_gx, Gx[0]);
    Gx[0] = HPF_Update(&hpf_x, Gx[0]);
    arm_biquad_cascade_df2T_f32(&lpf_Gx, Gx, filter_Gx, BLOCK_SIZE);
    filter_Gx[0] = kalman_filter_update(&gyro_filter_x, filter_Gx[0]);
    
    // Аналогично для Gy и Gz
    ...
}

1. Дополнительный этап - фильтр высоких частот для устранения дрейфа

2. Используются отдельные калибровочные коэффициенты (bias_Gx вместо offset_Ax)

4. Расчет ориентации

Функция get_angle_mahony();

void get_angle_mahony(void) {
    // Преобразование единиц измерения
    float filterAx = filter_Ax[0]*9.81; // м/с²
    float filterGx = filter_Gx[0]*DEG_TO_RAD; // рад/с
    
    // Обновление алгоритма Mahony
    MahonyAHRSupdateIMU(filterAx, filterAy, filterAz, 
                       filterGx, filterGy, filterGz, 
                       CONTROL_LOOP_DT);
    
    // Получение кватерниона ориентации
    Quat_actual[0] = (*(getQ()));
    ...
    
    // Комплементарная фильтрация кватерниона
    float alpha = 0.5;
    float dot = Quat_previous[0]*Quat_actual[0] + ...;
    if (dot < 0) {
        // Инвертирование при отрицательном скалярном произведении
        Quat_actual[0] = alpha * (-Quat_actual[0]) + ...;
        ...
    }
    
    // Нормализация кватерниона
    normalizeQuaternion(Quat_actual);
    
    // Преобразование кватерниона в углы Эйлера
    float q0 = Quat_actual[0];
    ...
    pitch = asinf(2.0f * (q1q3 - q0q2)) * RAD_TO_DEG;
    roll = atan2f(2.0f * (q0q1 + q2q3), q0q0 - q1q1 - q2q2 + q3q3) * RAD_TO_DEG;
    yaw = atan2f(2.0f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3) * RAD_TO_DEG;
    
    // Фильтрация углов
    roll = apply_notch_filter(&notch_roll, roll);
    ...
}

Описание расчета ошибки ориентации.

Расчет ошибки ориентации между текущим положением квадрокоптера и целевым положением заданным с пульта управления выполняется с использованием кватернионов и включает несколько этапов.

Преобразование команд с пульта в целевой кватернион

Функция eulerToQuaternion() преобразует сигналы джойстика (в диапазоне -100..100) в кватернион целевой ориентации.

void eulerToQuaternion(int joystick_x, int joystick_y, int right_left, float q[4]) {
    // Нормализация входных значений к диапазону [-1, 1] и преобразование в радианы
    float roll = (joystick_y / 100.0f) * M_PI;   // Ось X (крен)
    float pitch = (joystick_x / 100.0f) * M_PI;  // Ось Y (тангаж)
    float yaw = (right_left / 100.0f) * M_PI;    // Ось Z (рыскание)

    // Вычисление половинных углов для оптимизации
    float cy = cos(yaw * 0.5f);
    float sy = sin(yaw * 0.5f);
    float cp = cos(pitch * 0.5f);
    float sp = sin(pitch * 0.5f);
    float cr = cos(roll * 0.5f);
    float sr = sin(roll * 0.5f);

    // Формирование кватерниона
    q[0] = cr * cp * cy + sr * sp * sy; // Вещественная часть (w)
    q[1] = sr * cp * cy - cr * sp * sy; // Мнимая часть (x)
    q[2] = cr * sp * cy + sr * cp * sy; // Мнимая часть (y)
    q[3] = cr * cp * sy - sr * sp * cy; // Мнимая часть (z)

    normalizeQuaternion(q); // Нормализация кватерниона
}

Расчет кватерниона ошибки

Функция объединяющая операции расчета кватерниона ошибки.

void quat_error(float q_actual[4], float q_actual_inv[4], 
               float q_target[4], float q_error[4]) {
    // Шаг 1: Вычисление обратного кватерниона текущей ориентации
    quat_inverse(q_actual, q_actual_inv);
    
    // Шаг 2: Умножение целевого кватерниона на обратный текущий
    // q_error = q_target ⊗ q_actual^-1
    quat_multiply(q_target, q_actual_inv, q_error);
    
    // Шаг 3: Нормализация кватерниона ошибки
    normalizeQuaternion(q_error);
}

Для расчета ошибки необходимо найти обратный кватернион текущей ориентации.

void quat_inverse(float quat[4], float quat_inv[4]) {
    quat_inv[0] = quat[0];   // w' = w
    quat_inv[1] = -quat[1];  // x' = -x
    quat_inv[2] = -quat[2];  // y' = -y
    quat_inv[3] = -quat[3];  // z' = -z
    // Нормализация не требуется, если входной кватернион нормализован
}

Основная операция для расчета ошибки - умножение кватернионов.

void quat_multiply(float q1[4], float q2[4], float result[4]) {
    result[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]; // w
    result[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]; // x
    result[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1]; // y
    result[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0]; // z
}

В формировании управляющих сигналов используется векторная часть кватерниона.

double error_pitch = Quat_error[2] * (-1); // Ось Y
double error_roll = Quat_error[1];         // Ось X
double error_yaw = Quat_error[3];         // Ось Z

forse_pitch = PID_Compute(&pitch_pid, error_pitch, TRIM_PITCH_ERROR);
forse_roll = PID_Compute(&roll_pid, error_roll, TRIM_ROLL_ERROR);
forse_yaw = PID_Compute(&yaw_pid, error_yaw, TRIM_YAW_ERROR);

Расчет тяги для моторов.

if(button == 1 && potentiometer_value > 0) {
    // Передний левый мотор
    float pid_correction_1 = forse_roll + forse_pitch + forse_yaw;
    pid_correction_1 = constrain_float(pid_correction_1, -max_pid_correction_mshot, max_pid_correction_mshot);
    int total_power_1 = throttle_mshot + pid_correction_1 + TRIM_FRONT_LEFT;
    total_power_1 = constrain(total_power_1, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
    
    // Остальные моторы (аналогично с разными знаками коррекции)
    ...
    
    // Установка тяги
    Multishot_SetThrottle(total_power_1, TIM_CHANNEL_1);
    ...
} else {
    // Режим безопасности
    Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_1);
    ...
}

Вывод

Целью написания был интерес и желание создать минимально необходимый для работы квадрокоптера код. И я не доволен результатом. Итоговый вариант летает на твердую 2 из 5. Добиться хотя бы вменяемого зависания на месте не получилось. Причина скорее всего в неправильной работе с данными IMU. А именно в неверной настройке фильтров, например как только я создавал фильтр arm_biquad_cascade_df2T_init_f32 выше 2 порядка он тут же переставал работать. Неверный подбор частот данных и частот среза. Недостроил ПИД регулятор ну и возможно дело в ESC и моторах ибо брал их одним комплектом самые дешевые.

Код на GitHub.

Источники.