卫星姿态控制 C++ 可扩展仿真

#include 
#include 
#include 
#include 
using namespace std;

// 引入控制目标模块
#include "ControlTarget.h"

// 引入控制器模块
#include "PID_Controler.h"

// 引入敏感器模块，此部分也受空间环境影响
//#include "StarSensor.h"       
//#include "Gyroscope.h"

// 引入执行机构模块
//#include "FlyWheel.h"
//#include "Jet_Thruster.h"
//#include "MagneticTorqueConverter.h"

// 引入卫星动力学模块
#include "SatelliteDynamics.h"

// 工具模块
//#include "OrbitDescribe.hpp"
//#include "CoordinateTrans.hpp"
//#include "VecterMatrix.hpp"
#include "AttitudeDescribe.h"
#include "Constant.h"
using namespace m_Constant;

// 引入数据保存模块
//#include "PointDataList.h"


int main()
{
    // 姿态描述及转换工具
    AttitudeDescribe m_AttitudeDescribe;
    array m_q_start;

    // 初始状态设定
    double gamma0 = 20, fai0 = 42.7, psai0 = 5.6;
    double wib_bx0 = 0.1, wib_by0 = 0.2, wib_bz0 = 0.3;
    m_AttitudeDescribe.Euler_to_q(gamma0 * d2r, fai0 * d2r, psai0 * d2r, m_q_start);

    // 卫星初始运动状态
    array yn = { 0, 0, 0, 0, 0, 0, wib_bx0 * d2r, wib_by0 * d2r, wib_bz0 * d2r, m_q_start[0], m_q_start[1], m_q_start[2], m_q_start[3], 0 };

    // 卫星动力学
    shared_ptr pSatelliteDynamics(new SatelliteDynamics(yn));

    // PID控制器参数设定
    double wnx = 5, wny = 5, wnz = 5;
    array kp = { 2 * 1.25 / 0.515 * wnx * wnx * 0.6, 2 * 9.65 / 3.18 * wny * wny * 0.6, 2 * 9.65 / 1.59 * wnz * wnz * 0.6 };
    array ki = { 0, 0, 0 };
    array kd = { sqrt(0.6) * 2 * wnx * 1.25 / 0.515 * 2, sqrt(0.6) * 2 * wny * 9.65 / 3.18, sqrt(0.6) * 2 * wnz * 9.65 / 1.59 * 2 };
    
    // PID控制器
    shared_ptr pPID(new PID_Controler(kp, ki, kd, h));

    // 控制目标指令参数设定
    double gamma1 = 0, fai1 = 10, psai1 = 20;

    // 控制目标指令
    shared_ptr pControlTarget(new ControlTarget(gamma1 * d2r, fai1 * d2r, psai1 * d2r));

    // 陀螺仪测量
    array wob_b_mea;

    // 星敏感器测量
    array q_mea;

    // 姿态误差四元数
    array q_error;

    // 执行机构控制力矩
    array Mc = { 0, 0, 0 };  

    // 执行机构控制力
    array Fc = { 0, 0, 0 }; 

    // 数据保存到文件
    string FileName("Euler_Angle.txt");
    ofstream outFile(FileName, ios::out);
    if (outFile.fail()) cerr << "打开失败！" << endl;

    // 开始！
    for (; pSatelliteDynamics->Get_Time() < 9.99;)   // 仿真10秒
    {   
        // 卫星动力学推演
        pSatelliteDynamics->RungeKuttaGill(yn, Mc, Fc);

        // 陀螺仪的测量值 wob_b_mea, 控制器输入
        wob_b_mea = pSatelliteDynamics->Get_wob_b();

        // 星敏感器测量值 q_mea, 控制目标指令模块输入
        q_mea = pSatelliteDynamics->Get_Quaternion();

        // 控制目标指令模块输入 q_mea 输出 q_error，控制器输入
        pControlTarget->ControlCommand(q_mea, q_error);

        // 控制器输入 q_error、陀螺仪的测量值 wob_b_mea, 输出控制力矩 Mc, 输入到卫星动力学模块中
        pPID->AttitudeControl(q_error, wob_b_mea, Mc);

        // 保存数据到文件
        outFile << pSatelliteDynamics->Get_Time() << ",  "
                << pSatelliteDynamics->Get_EulerAngle()[0] * r2d << ",  " << pSatelliteDynamics->Get_EulerAngle()[1] * r2d << ",  " 
                << pSatelliteDynamics->Get_EulerAngle()[2] * r2d << ",  "<< pSatelliteDynamics->Get_wob_b()[0] * r2d << ",  " 
                << pSatelliteDynamics->Get_wob_b()[1] * r2d << ",  " << pSatelliteDynamics->Get_wob_b()[2] * r2d << ",  "
                << Mc[0] << ",  " << Mc[1] << ",  " << Mc[2] << endl;
    }

    // 关闭文件     
    outFile.close();

    return 0;
}

测试数据：

各个模块后续补充ing…