06.5 Code

推力(force)

// force = 电机的转速 * ｜电机的转速｜（带符号的转速）* 常数（用于产生拉力的气动常数） ===>>>> `T_i`
  double force = real_motor_velocity * real_motor_velocity * motor_constant_;

转速怎么来的？

// 转速 = motor_rot_vel_ （从gazbo中订阅得到到的）* 系数（为什么要系数？出于仿真器的可视化角度来说：比如转速很高，如80r/s，在gazebo中，界面的刷新率到不了这么高，fps可能20...；图像的刷新会出问题）
  double real_motor_velocity = motor_rot_vel_ * rotor_velocity_slowdown_sim_;

//从gazbo的关节（能旋转的东西，电机）得到的转速
 motor_rot_vel_ = joint_->GetVelocity(0);

推力的应用

// 实际scalar复杂，与不同的旋翼、飞行的方向都有关；这里简化（当你速度为25m/s时，scalar为0：25m/s时，拉力为0，速度越大，拉力越小）
double scalar = 1 - vel / 25.0; // at 50 m/s the rotor will not produce any force anymore
// Apply a force to the link. (link：三维软件中很常见，四旋翼可以认为是机体的link)
// 给机臂加了一个力 
// 力是矢量，给link_ 加上的是3维的向量
// scalar 系数；关于前进比
  link_->AddRelativeForce(ignition::math::Vector3d(0, 0, force * scalar));

旋翼的气动阻力

//未加风速
  //ignition::math::Vector3d body_velocity = link_->WorldLinearVel();
  // ignition::math::Vector3d body_velocity_perpendicular = body_velocity - (body_velocity * joint_axis) * joint_axis;
//加上风速
  // 风速 = 机体速度 - 风速
  ignition::math::Vector3d relative_wind_velocity = body_velocity - wind_vel_;
  ignition::math::Vector3d body_velocity_perpendicular = relative_wind_velocity - (relative_wind_velocity.Dot(joint_axis)) * joint_axis;
// air_drag旋翼的气动阻力沿桨平面方向，与飞行方向相反，是阻力
// 转速 * 系数 * v垂直（若无风，v是相对地面的速度；若有风，v是相对风的速度。v垂直是旋翼平面的分量（垂直于旋翼法向量的方向，即旋翼平面的方向））
  ignition::math::Vector3d air_drag = -std::abs(real_motor_velocity) * rotor_drag_coefficient_ * body_velocity_perpendicular;
// air_drag旋翼的气动阻力；忽略机身气动阻力即对应 =====>>>>> `H_i`
// Apply air_drag to link.
  link_->AddForce(air_drag);

空气阻力矩

// 声明时就定义，用于偏航的空气阻力矩
// drag_torque 空气阻力矩；方向是垂直降平面  ====>>>> `M_D,i`
// 方向 * 拉力 * 系数
  ignition::math::Vector3d drag_torque(0, 0, -turning_direction_ * force * moment_constant_);

滚转力矩

  // - \omega * \mu_1 * V_A^{\perp}
  // 转速 * 气动系数 * v垂直
  rolling_moment = -std::abs(real_motor_velocity) * rolling_moment_coefficient_ * body_velocity_perpendicular;
// 赋值给刚体  
  parent_links.at(0)->AddTorque(rolling_moment);

电机的转速

for (int i = 0; i < input_reference_.size(); i++) {
    if (armed) {
    // 先可认作电机转速input_reference_
    // 电机转速 
    // zero_position_armed_[i]：刚一解锁就开始转了，可认为是纵截距
    // input_scaling_[i] 自己的系数，油门要乘的系数
    // actuator_controls[input_index_[i]] Pixhawk输出给电调的 油门（0，1） ；若是舵机（-1，1）/ input_offset_[i]：偏置
    // 刚解锁时
      input_reference_[i] = (actuator_controls[input_index_[i]] + input_offset_[i])
          * input_scaling_[i] + zero_position_armed_[i];
      // std::cout << input_reference_ << ", ";
    } else {
      input_reference_[i] = zero_position_disarmed_[i];
      // std::cout << input_reference_ << ", ";
    }
  }
}

//...

// set joint positions
  for (int i = 0; i < input_reference_.size(); i++) {
    if (joints_[i] || joint_control_type_[i] == "position_gztopic") {
      double target = input_reference_[i];
      if (joint_control_type_[i] == "velocity")
      {
        double current = joints_[i]->GetVelocity(0);
        double err = current - target;
        // 模仿了电调对于电机旋转的反馈控制
        // 希望电机的转速对应到电调实际给电机的转矩，再把该转矩给到真正的刚体轴
        // gazbo会去模拟，给到这样一个力矩后，电机的轴和悬疑怎么转；之后还会反馈回来；然后不断完成更新……
        double force = pids_[i].Update(err, _dt);
        // SetForce 设置了滞后
        joints_[i]->SetForce(0, force);
      }
// 对应旋翼的转速
``