controller.h - master - Logrus/CopterControl

controller.h
56 строк2 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:1799ef28c
#pragma once
//#include "pch.h"
#include <eigen3/Eigen/Dense>
#include "config.h"
//#include "constants.h"

class Controller {
public:
    Controller(struct Params& params,double dt) :params(params),dt(dt) {}
//  Controller(std::Vector<double> params,double dt) :params(Params::fromVector(params)),dt(dt) {}
    void 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::Vector4d calculate_motor_speeds(double F_total, const Eigen::Vector3d& M) const;

private:
    // Внутреннее состояние контроллера
    struct ControlState {
        Eigen::Vector3d pos_des_prev = Eigen::Vector3d::Zero();
        double int_z = 0.0;
        Eigen::Vector2d prev_vel_err_xy = Eigen::Vector2d::Zero();
        Eigen::Vector2d d_vel_err_xy = Eigen::Vector2d::Zero();
        double prev_vel_err_z = 0.0;
        double d_vel_err_z = 0.0;
        double psi_prev = 0.0;
    };
    struct Params& params;
    ControlState state;
    double dt;
    
    // Вспомогательные функции
    double clip(double x, double min_val, double max_val) const;
    void calculate_commands(
        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_traj,
        const Eigen::Vector3d& acc_des_traj,
        double psi_des,
        const Eigen::Vector3d& ang_vel_des,
        double& F_total,
        Eigen::Vector3d& M
    );
};