uSimMarine：代码总结

目录

AUV速度

  AUV最大加速度：

AUV最大减速度：

推力映射关系

计算航向

AUV速度

受到影响因素：推力---->使用推力图进行查找得到速度

转弯速度损失---->可以使用速度转化率，代码中设置为0.85

// Procedure: propagateSpeed--->计算速度

void SimEngine::propagateSpeed(NodeRecordUVL& record, const ThrustMap& tmap,
			       double delta_time, double thrust,
			       double rudder, double max_accel, 
			       double max_decel)
{
  if(delta_time <= 0)
    return;

  double next_speed  = tmap.getSpeedValue(thrust);// 推力值得到的速度
  double prev_speed  = record.getSpeed();
  
  // Apply a slowing penalty proportional to the rudder/turn
  // 施加与方向舵/转弯成比例的减速惩罚
  rudder = vclip(rudder, -100, 100);
  double rudder_magnitude = fabs(rudder);// 返回绝对值
  double vpct = (rudder_magnitude / 100) * 0.85;// 转弯造成的速度损失 0.85可以改变（转弯速度的损失转化率）
  next_speed *= (1.0 - vpct);

  if(next_speed > prev_speed) {
    double acceleration = (next_speed - prev_speed) / delta_time;
    if((max_accel > 0) && (acceleration > max_accel))
      next_speed = (max_accel * delta_time) + prev_speed;
  }

  if(next_speed < prev_speed) {
    double deceleration = (prev_speed - next_speed) / delta_time;
    if((max_decel > 0) && (deceleration > max_decel))
      next_speed = (max_decel * delta_time * -1) + prev_speed;
  }// 限制一定的速度范围
  record.setSpeed(next_speed);
}

根据 delta_time进行修改（在USM_Model::propagate（）函数中-->USM_MOOSApp::Iterate()中）

计算步骤：SimEngine::propagateSpeed（）函数中

  AUV最大加速度：

max_acceleration—>在.moos配置文件中，读取值为0；

AUV最大减速度：

max_deceleration->在.moos配置文件中，读取值为0.5；

推力映射关系

TrustMap.cpp文件当中可以设置正向和负向推力最大值；
m_min_thrust = -100; m_max_thrust =  100;

在ThrustMap::addPair()函数中可以增加推力速度关系

在USM_Model::addThrustMapping（）函数中使用了增加推力映射关系，
在USM_MOOSApp::OnStartUp()中生成了推力速度关系

计算航向

在SimEngine::propagateHeading()函数中设置在USM_Model::propagate（）函数中-->USM_MOOSApp::Iterate()中

void SimEngine::propagateHeading(NodeRecordUVL& record,
				 double delta_time, 
				 double rudder,
				 double thrust,
				 double turn_rate,
				 double torque_theta)
{
  // 假设推进器和方向舵在同一个致动器上，例如，像皮划艇或典型的无人潜水器。旋转的推进器对转弯没有任何作用，除非道具在移动。
  double speed = record.getSpeed();
  if(speed == 0) 
    rudder = 0;

  // 即使速度为零，也需要在扭矩为零的情况下继续打开。
  rudder    = vclip(rudder, -100, 100);// 判断推力是否在-100~100之间小于或大于回到100和-100
  turn_rate = vclip(turn_rate, 0, 100);
  
  // Step 1: 计算航向的原始增量变化
  double delta_deg = rudder * (turn_rate/100) * delta_time;

  // Step 2: 计算航向变化因子推力
  delta_deg = (1 + ((thrust-50)/50)) * delta_deg;

  // Step 3:考虑外力计算航向变化
  delta_deg += (delta_time * torque_theta);

  // Step 4: 计算[0359]范围内的最终新航向
  double prev_heading = record.getHeading();
  double new_heading  = angle360(delta_deg + prev_heading);
  record.setHeading(new_heading);
  record.setYaw(-degToRadians(angle180(new_heading)));
}