2020.01.16向峻宏

这个代码还是挺难懂的，因为他的那个三部分的飞机模型的理论知识我没学过，如果能了解到它的运行原理，对于代码理解要好很多，如果哪位同学有四旋翼无人机的原理知识，就告诉我一下吧！

include "Quadrotor.h"
include "utils.h"
include <Eigen/Geometry>

Quadrotor::Quadrotor()
{
    physics = RigidBody();
    prop_radius = 0.099;
    kf = 5.55e-8;
    km = 0.07*(2*prop_radius)*kf;
    arm_length = 0.17;
    motor_time_constant = 1.0/20;
    min_rpm = 1200;
    max_rpm = 7000;/*定义了旋翼半径，桨叶推力系数kf，桨叶时间系数km（不懂啥意思，从下面的代码推出来的），旋翼机臂长，动机持续时间，最大和最小转速*/

    type = QUAD_MOTOR_CROSS;/*旋翼类型，就是下面的图展示的*/
    /* bug record: does not initialize here */
    external_force = Eigen::Vector3d::Zero();
    external_torque = Eigen::Vector3d::Zero();/*这好像是刚体的力和转矩，函数名和刚体里面的代码名一样*/
}

void Quadrotor::sim_step(double dt)/*四旋翼仿真的一步*/
{
    Eigen::Array4d motor_rpm_dot;
    Eigen::Array4d motor_rpm_sq = motor_rpm.array().square();

    double thrust = kf*motor_rpm_sq.sum();/*旋翼的推力计算公式*/
    Eigen::Vector3d force;
    Eigen::Vector3d torque;
    Eigen::Quaterniond my_attitude = physics.get_attitude();
    /* different torque if we have different type of quadrotor */
    if (type == QUAD_MOTOR_CROSS)
    {
        /*
               1
               |
               |
        2 ----------- 3
               |
               |
               0
        */
        torque(0) = kf*(motor_rpm_sq(2) - motor_rpm_sq(3))*arm_length;
        torque(1) = kf*(motor_rpm_sq(1) - motor_rpm_sq(0))*arm_length;
        torque(2) = km*(motor_rpm_sq(0) + motor_rpm_sq(1) -
                    motor_rpm_sq(2) - motor_rpm_sq(3));/*计算三个转矩，前2个是当前平面内的x，y轴的转矩，第三个我没懂*/
    }
    //TODO:add QUAD_MOTOR_X
    /*
            1             0
             \           /
               \       /
                  \ /
                  / \
               /       \
             /           \
            2             3

     */

    force = my_attitude.toRotationMatrix().col(2)*thrust + external_force;
    /* only for debug */
    //force = thrust*Eigen::Vector3d::UnitZ() + external_force;
    torque = torque + external_torque;/*计算总的力和转矩*/

    /* physics simulation */
    physics.set_force(force);
    physics.set_torque(torque);
    physics.sim_step(dt);/*刚体rigid body进行仿真的一步*/

    motor_rpm_dot = (target_motor_rpm - motor_rpm)/motor_time_constant;/*单位时间要增加的动机转速*/
    motor_rpm += motor_rpm_dot*dt;/*动机转速调整*/

}

void Quadrotor::set_motor_rpms(double m1, double m2, double m3, double m4)
{
    target_motor_rpm(0) = double_limit(m1, min_rpm, max_rpm);
    target_motor_rpm(1) = double_limit(m2, min_rpm, max_rpm);
    target_motor_rpm(2) = double_limit(m3, min_rpm, max_rpm);
    target_motor_rpm(3) = double_limit(m4, min_rpm, max_rpm);/*设置四个动机转速*/
}
void Quadrotor::set_external_force(Eigen::Vector3d _force)
{
    external_force(0) = _force(0);
    external_force(1) = _force(1);
    external_force(2) = _force(2);/*设置三个外力*/
}
void Quadrotor::set_external_torque(Eigen::Vector3d _torque)
{
    external_torque(0) = _torque(0);
    external_torque(1) = _torque(1);
    external_torque(2) = _torque(2);/*设置三个外部转矩*/
}

Eigen::Quaterniond Quadrotor::get_attitude()
{
    return physics.get_attitude();/*这里应该是调用刚体的姿态函数获取刚体的姿态*/
}
void Quadrotor::set_attitude(Eigen::Quaterniond _attitude)
{
    physics.set_attitude(_attitude);/*设置目标姿态*/
}


Eigen::Vector3d Quadrotor::get_position()
{
    return physics.get_position();/*获取位置信息*/
}

void Quadrotor::set_position(Eigen::Vector3d setting_vec)
{
    physics.external_set_position(setting_vec);/*设置目标位置*/
}

Eigen::Vector3d Quadrotor::get_velocity()
{
    return physics.get_velocity();/*获取速度,以下类推*/
}

double Quadrotor::get_mass()
{
    return physics.get_mass();
}

Eigen::Vector3d Quadrotor::get_angularVelocity() const
{
    return physics.get_angularVelocity();
}

Eigen::Matrix3d Quadrotor::get_inertia() const
{
    return physics.get_inertia();
}

double Quadrotor::get_propeller_thrust_coefficient(void) const
{
    return kf;
}

double Quadrotor::get_propeller_moment_coefficient(void) const
{
    return km;
}

double Quadrotor::get_arm_length(void) const
{
    return arm_length;
}

QUAD_MOTOR_TYPE Quadrotor::get_type(void) const
{
    return type;
}