/*
NAME: process_accel()
INPUT: RawxyzAccel - Data from CAN msg which has been put into the float of the fXYZ structure
OUTPUT: Result stored in file scope variable: AccelAngularPosition (Pitch,Roll) & AccelAngularVelocity
DESCRIPTION: Calculates the dot product of the incoming measurement
against the reference vector for each of the x=0, y=0, and
z=0 planes.
*/
void process_accel( BOOL ShowData )
{
// TIME STAMP :
prev_accel_timestamp = latest_accel_timestamp;
gettimeofday( &latest_accel_timestamp, NULL );
accel_time_delta = calc_time_delta( &prev_accel_timestamp, &latest_accel_timestamp );
if (ShowData)
printf("*** ACCEL TIME DELTA=%10.4f\n", accel_time_delta );
// Update State Variables:
AccelAngularPositionPrev = AccelAngularPosition;
AccelAngularVelocityPrev = AccelAngularVelocity;
// If near 1g, then stationary or rotational only!
// (yes there is a loophole here: freefall & 1g horizontal to name 1)
// (gyro can verify this however. If not validated, then...)
float mult = 1.0/One_g_scale2g; // printf("mult=%6.6f\n", mult);
scale( &RawxyzAccel, mult, &AccelScaled );
float tolerance = 6.0*Gravity_variance; // defined in fuse_ag.h from observed values.
if (compare_to_1g(&AccelScaled, tolerance) !=0 )
{
if (ShowData) printf("Linear Acceleration detected!\n");
LinearAccelerationDetected = TRUE;
}
else
{
LinearAccelerationDetected = FALSE;
}
if (ShowData) printf("\n");
// Compute new Angles/Deltas:
calc_stationary_angles( cast_f &AccelScaled ); // updates "AccelAngularPosition"
compute_accel_angular_velocity(); // updates "AccelAngularVelocity"
if (ShowData)
{
print_accel_angles();
print_accel_velocity();
}
}
// Do the timestep, outer and inner iterations
void iterate(void)
{
unsigned int num_groups_todo;
int outer_done;
double t1 = omp_get_wtime();
// Timestep loop
for (unsigned int t = 0; t < timesteps; t++)
{
unsigned int tot_outers = 0;
unsigned int tot_inners = 0;
global_timestep = t;
// Calculate data required at the beginning of each timestep
zero_scalar_flux();
zero_flux_moments_buffer();
// Outer loop
outer_done = false;
for (unsigned int o = 0; o < outers; o++)
{
// Reset the inner convergence list
int inner_done = false;
for (unsigned int g = 0; g < ng; g++)
{
groups_todo[g] = g;
}
num_groups_todo = ng;
tot_outers++;
calc_total_cross_section();
calc_scattering_cross_section();
calc_dd_coefficients();
calc_time_delta();
calc_denominator();
printf("calced\n");
// Compute the outer source
calc_outer_source();
printf("calc outer\n");
// Save flux
store_scalar_flux(old_outer_scalar);
printf("store scalar\n");
// Inner loop
for (unsigned int i = 0; i < inners; i++)
{
tot_inners++;
// Compute the inner source
calc_inner_source();
printf("calc_inner\n");
// Save flux
store_scalar_flux(old_inner_scalar);
zero_edge_flux_buffers();
#ifdef TIMING
double t1 = omp_get_wtime();
#endif
// Sweep
perform_sweep(num_groups_todo);
#ifdef TIMING
double t2 = omp_get_wtime();
printf("sweep took: %lfs\n", t2-t1);
#endif
// Scalar flux
reduce_angular();
#ifdef TIMING
double t3 = omp_get_wtime();
printf("reductions took: %lfs\n", t3-t2);
#endif
// Check convergence
store_scalar_flux(new_scalar);
#ifdef TIMING
double t4 = omp_get_wtime();
#endif
inner_done = check_convergence(old_inner_scalar, new_scalar, epsi, groups_todo, &num_groups_todo, true);
#ifdef TIMING
double t5 = omp_get_wtime();
printf("inner conv test took %lfs\n",t5-t4);
#endif
if (inner_done)
{
break;
}
}
// Check convergence
outer_done = check_convergence(old_outer_scalar, new_scalar, 100.0*epsi, groups_todo, &num_groups_todo, false);
if (outer_done && inner_done)
{
break;
}
}
printf("Time %d - %d outers, %d inners.\n", t, tot_outers, tot_inners);
// Exit the time loop early if outer not converged
if (!outer_done)
{
break;
}
}
double t2 = omp_get_wtime();
printf("Time to convergence: %.3lfs\n", t2-t1);
if (!outer_done)
{
printf("Warning: did not converge\n");
}
PRINT_PROFILING_RESULTS;
}
