3.7 KiB
3.7 KiB
yaw
p.dofs.ptte // Array of tower DOF indices contributing to QD2T-related linear accelerations of the tower nodes (point T) npte 塔架 Number of DOFs contributing to QD2T-related linear accelerations of the tower nodes (point T) 计数漂浮式基础自由度+塔架自由度
nptte // Number of tower DOFs contributing to QD2T-related linear accelerations of the tower nodes (point T)
for l in 0..p.dofs.nptte as usize{
// Loop through all active (enabled) tower DOFs that contribute to the QD2T-related linear accelerations of the yaw bearing (point O)
for i in l..p.dofs.nptte as usize{
// Loop through all active (enabled) tower DOFs greater than or equal to L
// [C(q,t)]T of YawBrMass
aug_mat[[p.dofs.ptte[i] as usize - 1, p.dofs.ptte[l] as usize - 1]] = p.yaw_br_mass *
dot_product(&rt_hs.plin_vel_eo.slice(s![p.dofs.ptte[i] as usize -1, 0, ..]).to_owned(),
&rt_hs.plin_vel_eo.slice(s![p.dofs.ptte[l] as usize -1, 0, ..]).to_owned());
// println!("{}",aug_mat[[p.dofs.ptte[i] as usize - 1, p.dofs.ptte[l] as usize - 1]]);
// println!("{}",p.dofs.ptte[i]);
// println!("{}",p.dofs.ptte[l]);
}
}
tmpvec1 = -p.yaw_br_mass * (p.gravity * coord_sys.z2.clone() + rt_hs.lin_acc_eot.clone());
for i in 0..p.dofs.nptte as usize{
// {-f(qd,q,t)}T + {-f(qd,q,t)}GravT of YawBrMass
aug_mat[[p.dofs.ptte[i] as usize - 1, p.naug as usize - 1]] = dot_product(&rt_hs.plin_vel_eo.slice(s![p.dofs.ptte[i] as usize -1, 0, ..]).to_owned(), &tmpvec1);
// println!("{}",aug_mat[[p.dofs.ptte[i] as usize - 1, p.naug as usize - 1]]);
// println!("{}",p.dofs.ptte[i]);
}
p.dofs.diag Array containing indices of SrtPS() associated with each enabled DOF srt_ps // Sorted version of PS(), from smallest to largest DOF index ps // Array of DOF indices to the active (enabled) DOFs/states
if p.dof_flag.as_mut().unwrap()[DOF_YAW as usize - 1]{
for i in p.dofs.diag[DOF_YAW as usize -1]..=p.dofs.n_actv_dof{
// [C(q,t)]N + [C(q,t)]R + [C(q,t)]G + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
aug_mat[[p.dofs.srt_ps[i as usize -1] as usize-1, DOF_YAW as usize-1]] = -1. * dot_product(&rt_hs.p_ang_vel_en.slice(s![DOF_YAW -1, 0, ..]).to_owned(),
&rt_hs.pmom_bnc_rt.slice(s![.., p.dofs.srt_ps[i as usize -1] -1]).to_owned());
// println!("{}", aug_mat[[p.dofs.srt_ps[i as usize -1] as usize-1, DOF_YAW as usize-1]]);
}
// {-f(qd,q,t)}N + {-f(qd,q,t)}GravN + {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}G + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
// + {-f(qd,q,t)}SpringYaw + {-f(qd,q,t)}DampYaw; NOTE: The neutral yaw rate, YawRateNeut, defaults to zero. It is only used for yaw control.
aug_mat[[DOF_YAW as usize-1, p.naug as usize -1]] = dot_product(&rt_hs.p_ang_vel_en.slice(s![DOF_YAW -1, 0, ..]).to_owned(),
&rt_hs.mom_bnc_rtt) + u.yaw_mom;
// println!("{}", aug_mat[[DOF_YAW as usize-1, p.naug as usize -1]]);
}