config.cpp - master - Logrus/CopterControl

config.cpp
98 строк3 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:1799ef28c
#include "pch.h"
#include "config.h"
//#include <yaml-cpp/yaml.h>
//#include <iostream>
#include <stdexcept>
using namespace std;
// Определение глобальной переменной
Config cfg;

Config Config::loadFromFile(const std::string& filename) {
    YAML::Node config = YAML::LoadFile(filename);

    Config c;

    // === Физические константы ===
    const auto& physics = config["physics"];
    c.g = physics["g"].as<double>();
    c.m = physics["m"].as<double>();
    c.m_p = physics["m_p"].as<double>();
    c.l = physics["l"].as<double>();
    c.rc = physics["rc"].as<double>();
    c.k = physics["k"].as<double>();
    c.b = physics["b"].as<double>();
    c.dt = physics["dt"].as<double>();

    // === Ограничения ===
    const auto& limits = config["limits"];
    c.OMEGA_MAX = limits["OMEGA_MAX"].as<double>();
    c.ACC_MAX = limits["ACC_MAX"].as<double>();
    c.F_MAX = 4.0 * c.k * c.OMEGA_MAX * c.OMEGA_MAX;
    c.F_MIN = 0.0;

    // === Позиционный регулятор ===
    const auto& pos = config["position"];
    c.Kp_pos_xy = pos["Kp_pos_xy"].as<double>();
    c.Kp_pos_z = pos["Kp_pos_z"].as<double>();
    c.Ki_pos_z = pos["Ki_pos_z"].as<double>();

    // === Ориентационный регулятор ===
    const auto& att = config["attitude"];
    c.kp_phi = att["kp_phi"].as<double>();
    c.kp_theta = att["kp_theta"].as<double>();
    c.kp_psi = att["kp_psi"].as<double>();
    c.kd_phi = att["kd_phi"].as<double>();
    c.kd_theta = att["kd_theta"].as<double>();
    c.kd_psi = att["kd_psi"].as<double>();


    // === Регулятор скорости ===
    const auto& vel = config["velocity"];
    c.Kp_vel_xy = vel["Kp_vel_xy"].as<double>();
    c.Kd_vel_xy = vel["Kd_vel_xy"].as<double>();
    c.Kp_vel_z = vel["Kp_vel_z"].as<double>();
    c.Kd_vel_z = vel["Kd_vel_z"].as<double>();

    // === Фильтры ===
    const auto& filt = config["filters"];
    c.d_filter_alpha = filt["d_filter_alpha"].as<double>();
    c.alpha = filt["alpha"].as<double>();

    // === Интегратор по Z ===
    const auto& integ = config["integrator"];
    c.int_z_min = integ["int_z_min"].as<double>();
    c.int_z_max = integ["int_z_max"].as<double>();

    // === Waypoints ===
    if (config["waypoints"]) {
        for (const auto& wp_node : config["waypoints"]) {
            if (wp_node.size() != 3) {
                throw std::runtime_error("Each waypoint must have 3 elements [x, y, z]");
            }
            Eigen::Vector3d wp;
            wp.x() = wp_node[0].as<double>();
            wp.y() = wp_node[1].as<double>();
            wp.z() = wp_node[2].as<double>();
            c.waypoints.push_back(wp);
        }
    } else {
        throw std::runtime_error("Missing 'waypoints' section in config.yaml");
    }

    // === t_points (опционально) ===
    if (config["t_points"]) {
        for (const auto& t_node : config["t_points"]) {
            c.t_points.push_back(t_node.as<double>());
        }
        // Проверка: t_points должно быть waypoints.size() + 1
        if (c.t_points.size() != c.waypoints.size() + 1) {
            throw std::runtime_error("t_points must have N+1 elements for N waypoints");
        }
    }

    return c;
}
// Функция инициализации (вызывайте её один раз в main)
void initConfig(const std::string& filename) {
    cfg = Config::loadFromFile(filename);
}