controller.cpp - master - Logrus/CopterControl

controller.cpp
157 строк6 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:1799ef28c
#include "pch.h"
#include "controller.h"
#include "config.h"
#define _USE_MATH_DEFINES
//#include <cmath>
//#include <algorithm>
//#define M_PI 3.14159265358979323846
//Controller::Controller(struct Config& cfg,double dt) :cfg(cfg),dt(dt) {}

double Controller::clip(double x, double min_val, double max_val) const {
    if (x < min_val) return min_val;
    if (x > max_val) return max_val;
    return x;
}

Eigen::Vector4d Controller::calculate_motor_speeds(double F_total, const Eigen::Vector3d& M) const {
    double Mx = M.x();
    double My = M.y();
    double Mz = M.z();

    double w1_sq = 0.25 / cfg.k * F_total - 0.5 / (cfg.k * cfg.l) * My + 0.25 / cfg.b * Mz;
    double w2_sq = 0.25 / cfg.k * F_total + 0.5 / (cfg.k * cfg.l) * Mx - 0.25 / cfg.b * Mz;
    double w3_sq = 0.25 / cfg.k * F_total + 0.5 / (cfg.k * cfg.l) * My + 0.25 / cfg.b * Mz;
    double w4_sq = 0.25 / cfg.k * F_total - 0.5 / (cfg.k * cfg.l) * Mx - 0.25 / cfg.b * Mz;

    w1_sq = std::max(w1_sq, 0.0);
    w2_sq = std::max(w2_sq, 0.0);
    w3_sq = std::max(w3_sq, 0.0);
    w4_sq = std::max(w4_sq, 0.0);

    double w1 = std::sqrt(w1_sq);
    double w2 = std::sqrt(w2_sq);
    double w3 = std::sqrt(w3_sq);
    double w4 = std::sqrt(w4_sq);

    return Eigen::Vector4d(w1, w2, w3, w4);
}


void Controller::compute_control(
    const Eigen::Vector3d& quad_pos,
    const Eigen::Vector3d& quad_vel,
    const Eigen::Vector3d& quad_angles,
    const Eigen::Vector3d& quad_ang_vel,
    const Eigen::Vector3d& pos_des,
    const Eigen::Vector3d& vel_des,
    const Eigen::Vector3d& acc_des,
    double psi_des,
    double& F_total,
    Eigen::Vector3d& M
) {
    //Eigen::Vector3d ang_vel_des = Eigen::Vector3d::Zero();
    
    // calculate_commands(
        // quad_pos, quad_vel, quad_angles, quad_ang_vel,
        // pos_des, vel_des, acc_des, psi_des, ang_vel_des,
        // F_total, M
    // );
	     // ---------- POSITION -> velocity setpoint ----------
    Eigen::Vector3d pos_des_filtered = cfg.alpha * state.pos_des_prev + (1 - cfg.alpha) * pos_des;
    Eigen::Vector3d pos_err = pos_des_filtered - quad_pos;
    state.pos_des_prev = pos_des;

    // X/Y: простое P
    Eigen::Vector2d vel_des_cmd_xy = vel_des.head(2) + params.Kp_pos_xy * pos_err.head(2);

    // Z: PID (pos -> desired vz)
    double vz_traj = vel_des.z();
    double pos_err_z = pos_err.z();

    // интегратор
    state.int_z += params.Ki_pos_z * pos_err_z * dt;
    state.int_z = clip(state.int_z, cfg.int_z_min, cfg.int_z_max);

    double vz_des = vz_traj + params.Kp_pos_z * pos_err_z + state.int_z;
    vz_des = clip(vz_des, -5.0, 5.0);

    Eigen::Vector3d vel_des_cmd(vel_des_cmd_xy.x(), vel_des_cmd_xy.y(), vz_des);

    // ---------- VELOCITY -> acceleration command ----------
    Eigen::Vector3d vel_err = vel_des_cmd - quad_vel;

    // XY: PD с фильтрацией производной
    Eigen::Vector2d vel_err_xy = vel_err.head(2);
    Eigen::Vector2d d_vel_err_xy_raw = (vel_err_xy - state.prev_vel_err_xy) / dt;
    state.d_vel_err_xy = cfg.d_filter_alpha * state.d_vel_err_xy + (1 - cfg.d_filter_alpha) * d_vel_err_xy_raw;
    state.prev_vel_err_xy = vel_err_xy;

    Eigen::Vector2d acc_cmd_xy = acc_des.head(2) 
                               + params.Kp_vel_xy * vel_err.head(2) 
                               + params.Kd_vel_xy * state.d_vel_err_xy;

    // Z: PD с фильтрацией
    double vel_err_z = vel_err.z();
    double d_vel_err_z_raw = (vel_err_z - state.prev_vel_err_z) / dt;
    state.d_vel_err_z = cfg.d_filter_alpha * state.d_vel_err_z + (1 - cfg.d_filter_alpha) * d_vel_err_z_raw;
    double acc_cmd_z = acc_des.z() + params.Kp_vel_z * vel_err_z + params.Kd_vel_z * state.d_vel_err_z;
    state.prev_vel_err_z = vel_err_z;

    // итоговый вектор ускорений
    Eigen::Vector3d acc_cmd(acc_cmd_xy.x(), acc_cmd_xy.y(), acc_cmd_z);
    acc_cmd.x() = clip(acc_cmd.x(), -cfg.ACC_MAX, cfg.ACC_MAX);
    acc_cmd.y() = clip(acc_cmd.y(), -cfg.ACC_MAX, cfg.ACC_MAX);
    acc_cmd.z() = clip(acc_cmd.z(), -cfg.ACC_MAX, cfg.ACC_MAX);

    // ---------- convert acc_cmd -> desired angles and thrust ----------
    Eigen::Vector3d thrust_direction = acc_cmd + Eigen::Vector3d(0, 0, cfg.g);
    double acc_total = thrust_direction.norm();
    if (acc_total < 1e-3) acc_total = 1e-3;
    thrust_direction /= acc_total;

    // Преобразуем в углы с учетом рыскания
    double cos_psi = std::cos(psi_des);
    double sin_psi = std::sin(psi_des);

    // Компоненты в телевой системе после поворота на рыскание
    double x_body = cos_psi * thrust_direction.x() + sin_psi * thrust_direction.y();
    double y_body = -sin_psi * thrust_direction.x() + cos_psi * thrust_direction.y();
    double z_body = thrust_direction.z();

    // Вычисляем углы крена и тангажа
    double theta_des = std::asin(std::clamp(x_body, -1.0, 1.0));
    double phi_des = std::atan2(-y_body, z_body);

    // Ограничиваем углы
    phi_des = clip(phi_des, -M_PI/4, M_PI/4);  // ±45°
    theta_des = clip(theta_des, -M_PI/4, M_PI/4);

    // Общая тяга
    F_total = cfg.m * acc_total;
    F_total = clip(F_total, cfg.F_MIN, cfg.F_MAX);

    // ---------- attitude control ----------
    double phi = quad_angles.x();
    double theta = quad_angles.y();
    double psi = quad_angles.z();
    
    double p = quad_ang_vel.x();
    double q = quad_ang_vel.y();
    double r = quad_ang_vel.z();
    
    double p_des = 0.0;
    double q_des = 0.0;
    double r_des = 0.0;

    double ex = phi_des - phi;
    double ey = theta_des - theta;
    double ez = psi_des - psi;
    
    double evx = p_des - p;
    double evy = q_des - q;
    double evz = r_des - r;

    M.x() = params.kp_phi * ex + params.kd_phi * evx;
    M.y() = params.kp_theta * ey + params.kd_theta * evy;
    M.z() = params.kp_psi * ez + params.kd_psi * evz;
}