quadcopter.cpp - master - Logrus/CopterControl

quadcopter.cpp
61 строка2 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:1799ef28c
#include "pch.h"
#include "quadcopter.h"
#include "config.h"
//#include <cmath>

Quadcopter::Quadcopter() 
    : pos(Eigen::Vector3d::Zero()),
      vel(Eigen::Vector3d::Zero()),
      angles(Eigen::Vector3d::Zero()),
      ang_vel(Eigen::Vector3d::Zero()),
      w1(0.0), w2(0.0), w3(0.0), w4(0.0)
{
    // Тензор инерции
    I << 0.4 * cfg.m * cfg.rc * cfg.rc + 2 * cfg.l * cfg.l * cfg.m_p, 0, 0,
         0, 0.4 * cfg.m * cfg.rc * cfg.rc + 2 * cfg.l * cfg.l * cfg.m_p, 0,
         0, 0, 0.4 * cfg.m * cfg.rc * cfg.rc + 4 * cfg.l * cfg.l * cfg.m_p;
    
    I_inv = I.inverse();
}

Eigen::Matrix3d Quadcopter::rotation_matrix(double phi, double theta, double psi) const {
    double c_phi = std::cos(phi), s_phi = std::sin(phi);
    double c_theta = std::cos(theta), s_theta = std::sin(theta);
    double c_psi = std::cos(psi), s_psi = std::sin(psi);
    
    return (Eigen::Matrix3d() <<
        c_psi*c_theta, c_psi*s_theta*s_phi - s_psi*c_phi, c_psi*s_theta*c_phi + s_psi*s_phi,
        s_psi*c_theta, s_psi*s_theta*s_phi + c_psi*c_phi, s_psi*s_theta*c_phi - c_psi*s_phi,
        -s_theta,      c_theta*s_phi,                   c_theta*c_phi).finished();
}

void Quadcopter::update_state(double F_total, const Eigen::Vector3d& M, double dt) {
    double phi = angles.x();
    double theta = angles.y();
    double psi = angles.z();
    
    Eigen::Matrix3d R = rotation_matrix(phi, theta, psi);
    Eigen::Vector3d thrust_vector_inertial = R * Eigen::Vector3d(0, 0, F_total);
    Eigen::Vector3d acc = thrust_vector_inertial / cfg.m - Eigen::Vector3d(0, 0, cfg.g);
    
    vel += acc * cfg.dt;
    pos += vel * cfg.dt;

    double p = ang_vel.x();
    double q = ang_vel.y();
    double r = ang_vel.z();
    
    // Гироскопический момент
    Eigen::Vector3d M_gyroscopic = ang_vel.cross(I * ang_vel);
    Eigen::Vector3d ang_acc = I_inv * (M - M_gyroscopic);
    
    ang_vel += ang_acc * cfg.dt;
    
    // Матрица преобразования body rates → эйлер-углы
    Eigen::Matrix3d T;
    T << 1, std::sin(phi)*std::tan(theta), std::cos(phi)*std::tan(theta),
         0, std::cos(phi),                -std::sin(phi),
         0, std::sin(phi)/std::cos(theta), std::cos(phi)/std::cos(theta);
    
    angles += (T * ang_vel) * cfg.dt;
}