Проектирование контроллера полета с использованием Arduino Nano IOT 33, GPS, барометра и других компонентов квадрокоптера

Я разрабатываю и создаю контроллер полета квадрокоптера и хотел бы узнать мнение других людей о дизайне.

Я планирую использовать Arduino Nano IOT 33. Этот Arduino имеет встроенный IMU, который можно использовать для получения сигналов для приведения в действие двигателей. Этот Arduino Nano IOT будет подключен к электронному контроллеру скорости BL_HELI, и он будет подключен к 4 двигателям мощностью 2300 кВ. Arduino будет подключен к модулю GPS и барометру для навигации.

ИДУ будет использовать дополнительный фильтр и ПИД-регуляторы для каждой оси, чтобы поддерживать стабильность дрона. Уставки для ПИД-контроллеров IMU будут выводиться с вторичных ПИД-контроллеров для GPS.

Для связи я планирую использовать модули приемопередатчика Xbee S2 Pro. Эти сообщения будут использоваться только для обновления уставок для вторичных ПИД-контроллеров. Это означает, что кто-то, управляющий с помощью модуля приемопередатчика, отправит координаты GPS и высоты. Это обновит вторичные ПИД-контроллеры, которые затем будут управлять первичными ПИД-контроллерами.

Это код, который у меня есть до сих пор:

основной скетч:

#include <DFRobot_ICP10111.h>
#include <complimentary_filter.h>
#include <pid_loop.h>
#include <Arduino_LSM6DS3.h>
#include <Servo.h>

Servo Motor1;
Servo Motor2;
Servo Motor3;
Servo Motor4;

PID_LOOP X_PID(0.8, 0, 0);
PID_LOOP Y_PID(0.8, 0, 0);
PID_LOOP Z_PID(0.5, 0, 0);

PID_LOOP GPS_LAT_PID(0.3, 0, 0);
PID_LOOP GPS_LONG_PID(0.3, 0, 0);
PID_LOOP ALTITUDE_PID(0.6, 0, 0);

//УПРАВЛЕНИЕ COMMUNICATION_CONTROL();

float motor1;
float motor2; 
float motor3;
float motor4;
float throttle = 40; // Дроссель в процентах от 0-100 (будет контролироваться PID после измерения высоты)

int MIN = 1000;
int MAX = 2000;

void setup() {
  Serial.begin(230400);

  Motor1.attach(2, MIN, MAX); //вывод D2
  Motor2.attach(3, MIN, MAX); //вывод D3
  Motor3.attach(5, MIN, MAX); //вывод D5
  Motor4.attach(6, MIN, MAX); //вывод D6

  Motor1.write(MIN);
  Motor2.write(MIN);
  Motor3.write(MIN);
  Motor4.write(MIN);
  
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!"); 
  }

  //Инициализируйте барометр здесь 
  //Для оценки высоты необходим барометр, ИДУ и дополнительный фильтр.
  //https://www.miro.ing.unitn.it/complementary-filtering-for-height-estimation-and-control-of-a-drone/
  //https://forum.arduino.cc/t/altitude-estimation-from-barometer-and-accelerometer-fusion-algoritm/296757/6
  
  calibrateIMU(250, 250);

// Serial.Print ("Motor1");
// Serial.Print ("Motor2");
// Serial.Print ("Motor3");
// Serial.println("Motor4");
 
}

void loop() {

  if(readIMU()){
    long currentTime = micros();
    lastInterval = currentTime - lastTime;
    lastTime = currentTime;
    doCalculations();
    }

   char received = 0;
   //Связь с Xbee
   
   if(Serial.available() > 0) {
    
    
    Serial.println();
    //Код здесь используется для вычисления новых желаемых координат GPS и высоты
    // Необходимо быть осторожным, чтобы не "блокировать поток", так как это происходит в том же основном цикле
    
   } else {
    //Код здесь предназначен для того, чтобы прервать связь
    // Установите GPS и высоту на удержание в течение 2 минут, если соединение по-прежнему отсутствует, перейдите в исходную точку.

    // Raspberry Pi может взять управление на себя, если нет управления более пары минут
    
   }

   // Связь с Raspberry Pi через I2C

   //----------------------------------------
   // Вычислить PID-коды
   //----------------------------------------

   //Вычисление линейных (вторичных) ПИД-контуров
// float lat_pid_output = GPS_LAT_PID.compute_loop(0, ACTUAL_GPS_LAT);
// float long_pid_output = GPS_LONG_PID.compute_loop(0, ACTUAL_GPS_LONG);
// float altitude_pid_output = GPS_ALTITUDE_PID.compute_loop(0, ИЗМЕРЕННАЯ ВЫСОТА);

   //Добавьте сюда код для установки желаемых точек первичных pid на основе вторичных pid
   // Убедитесь, что первичные значения PID не превышают определенного значения, скажем, 45 градусов
 
   // вычислить вращательный (первичный) ПИД-петли
   float x_pid_output = X_PID.compute_loop(0, complementaryRoll);
   float y_pid_output = Y_PID.compute_loop(0, complementaryPitch);
   float z_pid_output = Z_PID.compute_loop(0, complementaryYaw);
    
    // X 4 X 1
    // \ / ^x
    //     \____/                 |
    //     |    |                 |--->y
    // |____| z
    //    /      \                
    //   /        \               
    // X 3 X 2
    
    motor1 = throttle-x_pid_output+y_pid_output+z_pid_output; 
    motor2 = throttle+x_pid_output+y_pid_output-z_pid_output; 
    motor3 = throttle+x_pid_output-y_pid_output+z_pid_output;
    motor4 = throttle-x_pid_output-y_pid_output-z_pid_output;
    
    motor1 = constrain(map(motor1, 0, 100, 1000, 2000), 1000, 2000);
    motor2 = constrain(map(motor2, 0, 100, 1000, 2000), 1000, 2000);
    motor3 = constrain(map(motor3, 0, 100, 1000, 2000), 1000, 2000);
    motor4 = constrain(map(motor4, 0, 100, 1000, 2000), 1000, 2000);

// Serial.print(motor1);
// Serial.print('\t');
// Serial.print(motor2);
// Serial.print('\t');
// Serial.print(motor3);
// Serial.print('\t');
// Serial.print(motor4);
    
    Motor1.write(motor1);   
    Motor2.write(motor2);
    Motor3.write(motor3);
    Motor4.write(motor4);

  }

Библиотеки: ПИД-регулятор pid_loop.cpp

#include "pid_loop.h"

PID_LOOP::PID_LOOP(float p_value, float i_value, float d_value) {
    _p_value = p_value;
    _i_value = i_value;
    _d_value = d_value;

    current_error = 0;
    previous_error = 0;

    proportional = 0;
    integral = 0;
    differential = 0;

    pid_output = 0;
}

float PID_LOOP::compute_loop(float desired_value, float actual_value){

    current_error = desired_value - actual_value;
    
    proportional = current_error;
    integral += current_error;
    differential = current_error - previous_error;

    pid_output = (proportional*_p_value) + (integral*_i_value) + (differential*_d_value);

    previous_error = current_error;

    return pid_output;
}

void get_PID_values(){
    //Функция для вывода значений из PID для построения графиков
}

pid_loop.h

#ifndef PID_LOOP_H
#define PID_LOOP_H

#include <Arduino.h>

class PID_LOOP {

public:
    PID_LOOP(float p_value, float i_value, float d_value);
    
    float compute_loop(float desired_value, float actual_value);
    
    void get_PID_values();
private:
    
    float current_error;
    float previous_error;

    float desired_value;
    float actual_value;

    float _p_value;
    float _i_value;
    float _d_value;

    float proportional;
    float integral;
    float differential; 

    float pid_output;

};
#endif

Я уже протестировал настройку, и, похоже, все работает хорошо. Однако мне все еще нужно точно настроить ПИД-регуляторы. Я протестировал систему, установив дроссельную заслонку как раз на то место, где двигатели перестают вращаться, когда они разряжены. Затем я поворачиваю беспилотник, и двигатели запускаются ожидаемым образом.

Мои следующие шаги - найти подходящий барометр и систему фильтров. Нужно ли мне добавлять измерения ИДУ к барометру в каком-то дополнительном фильтре? Я стремлюсь использовать барометр для точного контроля высоты.

После этого мне нужно добавить код для измерения GPS, а также модуль Xbee для обновления уставок для ПИД-контроллеров. Как я могу использовать ПИД-контроллеры из модуля GPS для управления ПИД-контроллерами наклона, крена и рыскания? Может быть, я мог бы сопоставить выходные данные GPS-контроллера с углом наклона и тангажа?