Стартап полетный контроллер: ПИД регулятор для квадрокоптера

[dcopm999]

Здравствуйте уважаемые форумчане

Пишу ПИД регулятор для самодельного полетного контроллера для квадрокоптера, кто сталкивался с данной задачей,
подскажите пожалуйста правильно ли у меня выполняются расчеты?

if ((now = micros()) - loop_timer > period) {
ibus_values.clear();
imu_values.clear();
ibus_values = ibus->Pull(); // Получение текущих значений с аппаратуры радиоуправления
imu_values = imu->Pull(); // Получение текущих значений с гироскопа
/*
Расчет ошибок
ibus_values[3] - yaw
ibus_values[1] - pitch
ibus_values[0] - roll
ibus_values[2] - throttle
*/
// Расчет текущих ошибок
error[YAW] = (ibus_values[3] - 1500) / 5 - imu_values[0];
error[PITCH] = (ibus_values[1] - 1500) / 5 - imu_values[1];
error[ROLL] = (ibus_values[0] - 1500) / 5 - imu_values[2];

// Суммы ошибок
error_sum[YAW] += error[YAW];
error_sum[PITCH] += error[PITCH];
error_sum[ROLL] += error[ROLL];

// Keep values in acceptable range
error_sum[YAW] = minMax(error_sum[YAW], -400 / Ki[YAW], 400 / Ki[YAW]);
error_sum[PITCH] = minMax(error_sum[PITCH], -400 / Ki[PITCH], 400 / Ki[PITCH]);
error_sum[ROLL] = minMax(error_sum[ROLL], -400 / Ki[ROLL], 400 / Ki[ROLL]);

// Calculate error delta : Derivative coefficients
error_delta[YAW] = error[YAW] - error_pred[YAW];
error_delta[PITCH] = error[PITCH] - error_pred[PITCH];
error_delta[ROLL] = error[ROLL] - error_pred[ROLL];

// Save current error as previous_error for next time
error_pred[YAW] = error[YAW];
error_pred[PITCH] = error[PITCH];
error_pred[ROLL] = error[ROLL];

// Расчет значений для ПИД регулятора
yaw_pid = (error[YAW] * Kp[YAW]) + (error_sum[YAW] * Ki[YAW]) + (error_delta[YAW] * Kd[YAW]);
pitch_pid = (error[PITCH] * Kp[PITCH]) + (error_sum[PITCH] * Ki[PITCH]) + (error_delta[PITCH] * Kd[PITCH]);
roll_pid = (error[ROLL] * Kp[ROLL]) + (error_sum[ROLL] * Ki[ROLL]) + (error_delta[ROLL] * Kd[ROLL]);
yaw_pid = this->minMax(yaw_pid, -400, 400);
pitch_pid = this->minMax(pitch_pid, -400, 400);
roll_pid = this->minMax(roll_pid, -400, 400);

this->message_.clear();
this->message_.push_back(this->minMax(ibus_values[2] - roll_pid - pitch_pid + yaw_pid, 1100, 2000));
this->message_.push_back(this->minMax(ibus_values[2] + roll_pid - pitch_pid - yaw_pid, 1100, 2000));
this->message_.push_back(this->minMax(ibus_values[2] - roll_pid + pitch_pid + yaw_pid, 1100, 2000));
this->message_.push_back(this->minMax(ibus_values[2] + roll_pid + pitch_pid - yaw_pid, 1100, 2000));
this->Notify();

// Update loop timer
loop_timer = now;
};