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
}
