83 lines
3.7 KiB
Markdown
83 lines
3.7 KiB
Markdown
# 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)
|
||
|
||
|
||
```rust
|
||
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
|
||
```rust
|
||
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]]);
|
||
|
||
}
|
||
``` |