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 }