00001 00010 #include "math.h" 00011 #include "mex.h" 00012 #include "string.h" 00013 00014 #include "Datas.h" 00015 #include "dataFillings.h" 00016 00017 00018 #ifndef PI_DEF 00019 #define pi 3.141592653589793 00020 #define PI 3.141592653589793 00021 #endif 00022 00023 00024 00025 //init datas computable without P.para nor state: 00026 void data_init(datas* d) 00027 { 00028 d->g = 9.81; 00029 00030 d->Long = 18.0; 00031 00032 d->tanCal_left = 0.042; //Max LD 00033 d->tanCal_right = 0.042; 00034 d->tanCal_pitch = 0.0; 00035 00036 d->L_mast = 27.0; 00037 d->A_sail = 165.0; 00038 d->Baume = 10.0; 00039 d->A_structure = 50.0; 00040 d->CD_structure = 0.0; 00041 00042 d->Ix = 4.0e5; //2e5 00043 d->Iy = 2.0e5; 00044 d->Iz = 3.5e5; 00045 d->M = 7.0e3; 00046 00047 d->E_foils = 6.5; 00048 d->Chord = 0.5; 00049 00050 //Min & max chord for the hydrofoils 00051 d->Chord_min = 0.2; 00052 d->Chord_max = 2.5; 00053 d->epsilon = 1.0e-2; 00054 00055 d->rho = 1.0e3; 00056 d->rho_air = 1.293; 00057 00058 d->E_yaw = 3.6; //2; 00059 00060 d->Env_pitch = 2.0; 00061 00062 d->Chord_yaw = 0.3; 00063 d->Chord_pitch = 0.3; 00064 00065 d->Wind = 10; 00066 00067 d->Wave_angle0 = pi/4.0; 00068 d->Wave_amp0 = 0.6; 00069 d->V_wave0 = 2.0;//0.0; 00070 00071 d->Fac_update = 1.0; 00072 00073 00074 00075 d->A_pitch = d->Env_pitch * d->Chord_pitch; 00076 00077 d->L_nose = 3.0; 00078 00079 d->target_x = 0.0f; 00080 d->target_y = 0.0f; 00081 d->Vslider_x = 0.0f; 00082 d->Hslider_x = 0.0f; 00083 } 00084 00085 //Update datas computable from state 00086 void data_update_state(datas* d, double *state) 00087 { 00088 d->x = state[0]; 00089 d->y = state[1]; 00090 d->z = state[2]; 00091 00092 d->phi = state[3]; 00093 d->theta = state[4]; 00094 d->psi = state[5]; 00095 00096 d->dx = state[6]; 00097 d->dy = state[7]; 00098 d->dz = state[8]; 00099 00100 d->dphi = state[9]; 00101 d->dtheta = state[10]; 00102 d->dpsi = state[11]; 00103 00104 /*d->x_cam = state[12]; //unused camera dynamic stuff 00105 d->y_cam = state[13]; 00106 d->z_cam = state[14]; 00107 00108 d->dx_cam = state[15]; 00109 d->dy_cam = state[16]; 00110 d->dz_cam = state[17];*/ 00111 } 00112 00113 00114 //Update datas computable from parameters 00115 void data_update_param(datas* d, parameters *P) 00116 { 00117 d->x_foils = P->param[0]; 00118 d->tanCal_yaw0 = P->param[1]; 00119 d->tanAngle_sail = P->param[4]; 00120 d->G_shift = 0.0;//P->param[5]; 00121 d->alpha_foils = P->param[6]; 00122 d->Wind_angle = P->param[8]; 00123 d->z_foils = P->param[9]; 00124 00125 d->alpha_left = d->alpha_foils*pi/180.0; 00126 d->alpha_right = d->alpha_foils*pi/180.0; 00127 00128 d->y_foil_left = 12.0- d->G_shift; 00129 d->y_foil_right = -12.0- d->G_shift; 00130 00131 d->x_sail = d->x_foils; 00132 d->y_sail = 0.0 - d->G_shift; 00133 d->z_sail = d->z_foils + d->L_mast/3.0; 00134 00135 00136 d->x_yaw = -0.5*d->Long; 00137 d->y_yaw = 0 - d->G_shift; 00138 d->z_yaw = d->z_foils-1.5; //E_yaw/2 - 5; 00139 00140 d->x_pitch = -0.5*d->Long; 00141 d->y_pitch = 0 - d->G_shift; 00142 d->z_pitch = d->z_yaw - d->E_yaw; 00143 00144 d->Wind_x = -d->Wind*cos(d->Wind_angle); 00145 d->Wind_y = -d->Wind*sin(d->Wind_angle); 00146 00147 d->Env = d->y_foil_left - d->y_foil_right; 00148 d->Lambda0 = 1.7*d->Env; 00149 00150 d->N = 2; //length(Lambda); 00151 00152 d->tanCal_yaw = d->tanCal_yaw0; 00153 00154 d->Fac_update = 1.0; 00155 00156 00157 d->Lambda[0] = d->Lambda0/3.0; 00158 d->Lambda[1] = d->Lambda0/5.0; 00159 d->Wave_angle[0] = d->Wave_angle0 + 0.5*pi/4.0; 00160 d->Wave_angle[1] = d->Wave_angle0 + pi/4.0; 00161 d->Wave_amp[0] = d->Wave_amp0/2.0; 00162 d->Wave_amp[1] = d->Wave_amp0/4.0; 00163 d->V_wave[0] = d->V_wave0/2.0; 00164 d->V_wave[1] = d->V_wave0/4.0; 00165 00166 d->Lambda1 = d->Lambda[0]; 00167 d->Lambda2 = d->Lambda[1]; 00168 d->Wave_amp1 = d->Wave_amp[0]; 00169 d->Wave_amp2 = d->Wave_amp[1]; 00170 d->V_wave1 = d->V_wave[0]; 00171 d->V_wave2 = d->V_wave[1]; 00172 d->Wave_angle1 = d->Wave_angle[0]; 00173 d->Wave_angle2 = d->Wave_angle[1]; 00174 00175 //store neutral cal angles: 00176 d->Cal_right_neutral = 0.5*(atan(d->tanCal_left)+atan(d->tanCal_right)); 00177 d->Cal_left_neutral = 0.5*(atan(d->tanCal_left)+atan(d->tanCal_right)); 00178 d->Cal_pitch_neutral = 0.0;//atan(d->tanCal_pitch); 00179 d->Cal_yaw_neutral = 0.0;//atan(d->tanCal_yaw); 00180 00181 //init baume and girouette: 00182 d->max_abs_angle_baume = atan(d->tanAngle_sail); 00183 d->angle_girouette = atan(d->tanAngle_sail); 00184 } 00185 00186 00187 void angles_update(datas *d) 00188 { 00189 float debat = 0.1745; // max cal angle variation (0.1745==10deg) 00190 00191 d->tanCal_right = tan(d->Cal_right_neutral - debat*d->target_x + debat*d->Vslider_x); 00192 d->tanCal_left = tan(d->Cal_left_neutral + debat*d->target_x + debat*d->Vslider_x); 00193 00194 d->tanCal_pitch = tan(d->Cal_pitch_neutral + debat*d->target_y); 00195 d->tanCal_yaw = tan(d->Cal_yaw_neutral + debat*d->Hslider_x); 00196 d->tanCal_yaw0 = d->tanCal_yaw; 00197 }
1.4.6-NO