Программируем квадрокоптер на STM32

В этой статье рассмотрим систему стабилизации квадрокоптера.  Статья вдохновлена Программируем квадрокоптер на 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.

Источники.