void find_post_variable_value_fluid(np_t *np, long l, gl_t *gl,
long varnum, double *varvalue){
spec_t w,nu;
double eta,kappa;
*varvalue=0.0;
if (is_node_valid(np[l],TYPELEVEL_FLUID)){
if (varnum<ns) *varvalue=_w(np[l],varnum);
if (varnum>=ns && varnum<ns+nd) *varvalue=_V(np[l],varnum-ns);
//assert(is_node_resumed(np[l]));
find_w(np[l],w);
find_nuk_eta_kappa(w, _rho(np[l]), _T(np[l],gl), nu, &eta, &kappa);
switch (varnum) {
case nd+ns+0: *varvalue=_rho(np[l]); break;
case nd+ns+1: *varvalue=_P(np[l],gl); break;
case nd+ns+2: *varvalue=_Pstar(np[l],gl); break;
case nd+ns+3: *varvalue=_T(np[l],gl); break;
case nd+ns+4: *varvalue=_Tv(np[l]); break;
case nd+ns+5: *varvalue=_a(np[l],gl); break;
case nd+ns+6: *varvalue=_eta(np[l],gl); break;
case nd+ns+7: *varvalue=_etastar(np,l,gl); break;
case nd+ns+8: *varvalue=_kappastar(np,l,gl); break;
case nd+ns+9: *varvalue=_k(np[l]); break;
case nd+ns+10: *varvalue=_eps(np[l],gl); break;
case nd+ns+11: *varvalue=_omega(np[l],gl); break;
case nd+ns+12: *varvalue=_gamma(np[l],gl); break;
}
}
}
void
AP_AHRS_HIL::setHil(float _roll, float _pitch, float _yaw,
float _rollRate, float _pitchRate, float _yawRate)
{
roll = _roll;
pitch = _pitch;
yaw = _yaw;
_omega(_rollRate, _pitchRate, _yawRate);
roll_sensor = ToDeg(roll)*100;
pitch_sensor = ToDeg(pitch)*100;
yaw_sensor = ToDeg(yaw)*100;
_dcm_matrix.from_euler(roll, pitch, yaw);
}
void
AP_DCM::_accel_adjust(void)
{
_accel_vector(1) += accel_scale((ground_speed / 100) * _omega(2)); // Centrifugal force on Acc_y = GPS_speed * GyroZ
_accel_vector(2) -= accel_scale((ground_speed / 100) * _omega(1)); // Centrifugal force on Acc_z = GPS_speed * GyroY
}
