config.h - master - Logrus/CopterControl

config.h
121 строка4 KB
Nikolay Nosorev
upload config.h
04 окт 2025, 16:20
04 окт 2025, 16:208f16fd3
#pragma once
#include <string>

struct Config {
    // Физические константы
    double g;
    double m;
    double m_p;
    double l;
    double rc;
    double k;
    double b;
    double dt;

    // Параметры ограничений
    double OMEGA_MAX;
    double F_MAX;   // вычисляется
    double F_MIN;   // всегда 0.0
    double ACC_MAX;

    // Коэффициенты регуляторов — позиция
    double Kp_pos_xy;
    double Kp_pos_z;
    double Ki_pos_z;

   // Коэффициенты регуляторов — скорость
    double Kp_vel_xy;
    double Kd_vel_xy;
    double Kp_vel_z;
    double Kd_vel_z;
	 
    // Коэффициенты регуляторов — ориентация
    double kp_phi;
    double kp_theta;
    double kp_psi;
    double kd_phi;
    double kd_theta;
    double kd_psi;

    // Фильтры
    double d_filter_alpha;
    double alpha;

    // Интегратор
    double int_z_min;
    double int_z_max;

    // Waypoints и временные точки
    std::vector<Eigen::Vector3d> waypoints;
    std::vector<double> t_points; // может быть пустым

    // Параметры планирования
    double nominal_speed;
    double nominal_accel;

    static Config loadFromFile(const std::string& filename);
	     // Конвертация в vector для NLopt
    // std::vector<double> toVector() const {
        // return {
			   // g,m,m_p,l,rc,k,b,dt,
				// OMEGA_MAX,F_MAX,F_MIN,ACC_MAX,
            // Kp_pos_xy, Kp_pos_z, Ki_pos_z, 
            // Kp_vel_xy, Kd_vel_xy, Kp_vel_z, Kd_vel_z,
            // kp_phi, kp_theta, kp_psi,
            // kd_phi, kd_theta, kd_psi,
				// d_filter_alpha,alpha,
				// int_z_min,int_z_max
        // };
    // }
	std::vector<double> toVectorParams() const {
        return {
            Kp_pos_xy, Kp_pos_z, Ki_pos_z, 
            Kp_vel_xy, Kd_vel_xy, Kp_vel_z, Kd_vel_z,
            kp_phi, kp_theta, kp_psi,
            kd_phi, kd_theta, kd_psi
        };
    }
    
    // // Инициализация из vector
    // static Config fromVector(const std::vector<double>& v) {
        // return {
            // v[0], v[1], v[2],v[3], v[4], v[5], v[6],v[7], // Физические константы
				// v[8], v[9], v[10], v[11], // Параметры ограничений
				// v[12], v[13],v[14],// Коэффициенты регуляторов — позиция
				// v[15], v[16], v[17],v[18], // Коэффициенты регуляторов — скорость
            // v[19],v[20],v[21],v[22],v[23],v[24], // Коэффициенты регуляторов — ориентация
				// v[25],v[26],//Фильтры
				// v[27],v[28] // Интегратор
        // };
    // }
};
struct Params {
    double Kp_pos_xy, Kp_pos_z, Ki_pos_z ;//Kd_pos_z;
    double Kp_vel_xy, Kd_vel_xy, Kp_vel_z, Kd_vel_z;
    double kp_phi, kp_theta, kp_psi;
    double kd_phi, kd_theta, kd_psi;
    
    // Конвертация в vector для NLopt
    std::vector<double> toVector() const {
        return {
            Kp_pos_xy, Kp_pos_z, Ki_pos_z, 
            Kp_vel_xy, Kd_vel_xy, Kp_vel_z, Kd_vel_z,
            kp_phi, kp_theta, kp_psi,
            kd_phi, kd_theta, kd_psi
        };
    }
    
    // Инициализация из vector
    static Params fromVector(const std::vector<double>& v) {
        return {
            v[0], v[1], v[2], 		   // позиционные
            v[3], v[4], v[5], v[6],         // скоростные  
            v[7], v[8], v[9],             // угловые P
            v[10], v[11], v[12]             // угловые D
        };
    }
};
// Объявляем глобальную переменную (extern = определена где-то в .cpp)
extern Config cfg;
// Объявление функции инициализации (важно!)
void initConfig(const std::string& filename = "config.yaml");