static accelerometer_i quantize_accelerometer(accelerometer_d value, unsigned mask) { return (accelerometer_i) { .x = quantize(value.x, mask), .y = quantize(value.y, mask), .z = quantize(value.z, mask), .q = quantize(value.q, mask), }; } static vec3_i quantize_vec(vec3 value, unsigned mask) { return (vec3_i) { .x = quantize(value.x, mask), .y = quantize(value.y, mask), .z = quantize(value.z, mask), }; } static accelerometer_d add_accelerometer_noise(accelerometer_d value) { return (accelerometer_d) { .x = value.x + gaussian(accelerometer_sd.x), .y = value.y + gaussian(accelerometer_sd.y), .z = value.z + gaussian(accelerometer_sd.z), .q = value.q + gaussian(accelerometer_sd.q), }; } static vec3 vec_noise(vec3 value, vec3 sd) { return (vec3) { .x = value.x + gaussian(sd.x), .y = value.y + gaussian(sd.y), .z = value.z + gaussian(sd.z), }; } static void update_simulator(void) { trace_state("sim", &rocket_state, ", %4.1f kg, %c%c%c\n", mass, engine_burning ? 'B' : '-', drogue_chute_deployed ? 'D' : '-', main_chute_deployed ? 'M' : '-'); accelerometer_sensor(quantize_accelerometer(add_accelerometer_noise(accelerometer_measurement(&rocket_state)), 0xfff)); if(t % 2000 == 0) gyroscope_sensor(quantize_vec(vec_noise(gyroscope_measurement(&rocket_state), gyroscope_sd), 0xfff)); if(t % 100000 == 0) pressure_sensor(quantize(pressure_measurement(&rocket_state) + gaussian(pressure_sd), 0xfff)); if(t % 100000 == 25000) magnetometer_sensor(quantize_vec(vec_noise(magnetometer_measurement(&rocket_state), magnetometer_sd), 0xfff)); if(t % 100000 == 50000) gps_sensor(vec_noise(rocket_state.pos, gps_pos_sd), vec_noise(rocket_state.vel, gps_vel_sd)); if(!engine_ignited && t >= LAUNCH_TIME && last_reported_state() == STATE_ARMED) { trace_printf("Sending launch signal\n"); launch(); } if(engine_burning && t - engine_ignition_time >= ENGINE_BURN_TIME) { trace_printf("Engine burn-out.\n"); engine_burning = false; } if(last_reported_state() == STATE_PREFLIGHT) { trace_printf("Sending arm signal\n"); arm(); } if(engine_burning) mass -= FUEL_MASS * DELTA_T_SECONDS / (ENGINE_BURN_TIME / 1e6); geodetic pos = ECEF_to_geodetic(rocket_state.pos); if(pos.altitude <= initial_geodetic.altitude) { if(pos.altitude < initial_geodetic.altitude) { pos.altitude = initial_geodetic.altitude; rocket_state.pos = geodetic_to_ECEF(pos); } mat3 rot = make_LTP_rotation(pos); ground_clip(&rocket_state.vel, rot); } } static void init_rocket_state(struct rocket_state *rocket_state) { /* TODO: accept an initial orientation for leaving the tower */ mass = ROCKET_EMPTY_MASS + FUEL_MASS; rocket_state->pos = geodetic_to_ECEF(initial_geodetic); rocket_state->rotpos = make_LTP_rotation(initial_geodetic); rocket_state->acc = expected_acceleration(0, rocket_state); } int main(int argc, const char *const argv[]) { parse_trace_args(argc, argv); initial_geodetic = (geodetic) { .latitude = M_PI_2, .longitude = 0, .altitude = 0, }; init_atmosphere(LAYER0_BASE_TEMPERATURE, LAYER0_BASE_PRESSURE); init_rocket_state(&rocket_state); init(initial_geodetic, rocket_state.rotpos); while(last_reported_state() != STATE_RECOVERY) { update_rocket_state_sim(&rocket_state, DELTA_T_SECONDS, expected_acceleration, (double)t); t += DELTA_T; update_simulator(); tick(DELTA_T_SECONDS); } return 0; }
int user1_init_primitives(FTYPE (*prim)[NSTORE2][NSTORE3][NPR], FTYPE (*pstag)[NSTORE2][NSTORE3][NPR], FTYPE (*ucons)[NSTORE2][NSTORE3][NPR], FTYPE (*vpot)[NSTORE1+SHIFTSTORE1][NSTORE2+SHIFTSTORE2][NSTORE3+SHIFTSTORE3], FTYPE (*Bhat)[NSTORE2][NSTORE3][NPR], FTYPE (*panalytic)[NSTORE2][NSTORE3][NPR], FTYPE (*pstaganalytic)[NSTORE2][NSTORE3][NPR], FTYPE (*vpotanalytic)[NSTORE1+SHIFTSTORE1][NSTORE2+SHIFTSTORE2][NSTORE3+SHIFTSTORE3], FTYPE (*Bhatanalytic)[NSTORE2][NSTORE3][NPR], FTYPE (*F1)[NSTORE2][NSTORE3][NPR],FTYPE (*F2)[NSTORE2][NSTORE3][NPR],FTYPE (*F3)[NSTORE2][NSTORE3][NPR], FTYPE (*Atemp)[NSTORE1+SHIFTSTORE1][NSTORE2+SHIFTSTORE2][NSTORE3+SHIFTSTORE3]) { int whichvel, whichcoord; int initreturn; int i = 0, j = 0, k = 0, l; FTYPE r,th,X[NDIM],V[NDIM]; int normalize_densities(FTYPE (*prim)[NSTORE2][NSTORE3][NPR]); int normalize_field(FTYPE (*prim)[NSTORE2][NSTORE3][NPR], FTYPE (*pstag)[NSTORE2][NSTORE3][NPR], FTYPE (*ucons)[NSTORE2][NSTORE3][NPR], FTYPE (*vpot)[NSTORE1+SHIFTSTORE1][NSTORE2+SHIFTSTORE2][NSTORE3+SHIFTSTORE3], FTYPE (*Bhat)[NSTORE2][NSTORE3][NPR]); int init_dsandvels(int *whichvel, int *whichcoord, int i, int j, int k, FTYPE *p, FTYPE *pstag); int init_atmosphere(int *whichvel, int *whichcoord, int i, int j, int k, FTYPE *pr); int pl,pliter; /////////////////////////////////// // // Assign primitive variables // /////////////////////////////////// trifprintf("Assign primitives\n"); // assume we start in bl coords and convert to KSprim // so field defined when get to floor model (fixup) init_3dnpr_fullloop(0.0,prim); ////////////////////// // // assume we start in bl coords and convert to KSprim // ////////////////////// #pragma omp parallel private(i,j,k,initreturn,whichvel,whichcoord) OPENMPGLOBALPRIVATEFULL { OPENMP3DLOOPVARSDEFINE; //////// COMPFULLLOOP{ OPENMP3DLOOPSETUPFULL; #pragma omp for schedule(OPENMPSCHEDULE(),OPENMPCHUNKSIZE(blocksize)) OPENMP3DLOOPBLOCK{ OPENMP3DLOOPBLOCK2IJK(i,j,k); initreturn=init_dsandvels(&whichvel, &whichcoord,i,j,k,MAC(prim,i,j,k),MAC(pstag,i,j,k)); // request densities for all computational centers if(initreturn>0){ FAILSTATEMENT("init.c:init_primitives()", "init_dsandvels()", 1); } else MYFUN(transform_primitive_vB(whichvel, whichcoord, i,j,k, prim, pstag),"init.c:init_primitives","transform_primitive_vB()",0); } }// end parallel region ///////////////////////////// // // normalize density if wanted // /////////////////////////////// // at this point densities are still standard, so just send "prim" trifprintf("Normalize densities\n"); normalize_densities(prim); ///////////////////////////// // // Define an atmosphere if wanted // /////////////////////////////// if(DOEVOLVERHO||DOEVOLVEUU){ // normalized atmosphere trifprintf("Add atmosphere\n"); #pragma omp parallel private(i,j,k,initreturn,whichvel,whichcoord) OPENMPGLOBALPRIVATEFULL { OPENMP3DLOOPVARSDEFINE; /////// COMPZLOOP { OPENMP3DLOOPSETUPZLOOP; #pragma omp for schedule(OPENMPSCHEDULE(),OPENMPCHUNKSIZE(blocksize)) OPENMP3DLOOPBLOCK{ OPENMP3DLOOPBLOCK2IJK(i,j,k); initreturn=init_atmosphere(&whichvel, &whichcoord,i,j,k,MAC(prim,i,j,k)); if(initreturn>0){ FAILSTATEMENT("init.c:init_primitives()", "init_atmosphere()", 1); } else{ // transform from whichcoord to MCOORD if (bl2met2metp2v(whichvel, whichcoord,MAC(prim,i,j,k), i,j,k) >= 1){ FAILSTATEMENT("init.c:init()", "bl2ks2ksp2v()", 1); } } }// end 3D LOOP }// end parallel region }