void liftKernel(Tuple_float_float_float_float* v__11, Tuple_float_float_float_float* v__12, float v__13, float v__14, Tuple_Tuple_float_float_float_float_Tuple_float_float_float_float* v__22){
{
/* Static local memory */
/* Typed Value memory */
Tuple_float_float_float_float v__17;
/* Private Memory */
/* omp_map */
#pragma omp parallel for private(v__17)
for (int v_i_8 = 0;v_i_8<1000;v_i_8 = (1 + v_i_8)){
Tuple_float_float_float_float v_tmp_34 = (Tuple_float_float_float_float) { 0.0f, 0.0f, 0.0f, 0.0f };
v__17 = v_tmp_34;
/* reduce_seq */
for (int v_i_9 = 0;v_i_9<1000;v_i_9 = (1 + v_i_9)){
v__17 = calcAcc(v__11[v_i_8], v__11[v_i_9], v__14, v__13, v__17);
}
/* end reduce_seq */
/* map_seq */
/* iteration count is exactly 1, no loop emitted */
{
int v_i_10 = 0;
v__22[v_i_8] = update(v__11[v_i_8], v__12[v_i_8], v__14, v__17);
}
/* end map_seq */
}
/* end omp_map */
}}
void ReferenceFrame::update(double t, double dt)
{
SensorsManager& sm = SensorsManager::getSensorsManager();
sm.update(t);
//TODO: calculates compass data
const double* newNorthData = sm.getNorth();
this->north[0] = newNorthData[0];
this->north[1] = newNorthData[1];
this->north[2] = newNorthData[2];
//calculates acceleration (low-pass filter)
const double* new_acc = sm.getAcc();
this->acc[0] = calcAcc(this->buffer_acc_X, new_acc[0]);
this->acc[1] = calcAcc(this->buffer_acc_Y, new_acc[1]);
this->acc[2] = calcAcc(this->buffer_acc_Z, new_acc[2]);
buff_index = (++buff_index) % BUFFER_SIZE;
//calculates angular rate
const double* new_ang = sm.getAngleVel();
this->angle_vel[0] = new_ang[0];
this->angle_vel[1] = new_ang[1];
this->angle_vel[2] = new_ang[2];
double acc_accounted[] = { this->acc[0], this->acc[1], this->acc[2] + min(0.0, this->uBase/THRUST_G) };
//assigns Euler's angles
double* eulers = get_eulers(this->acc_ref, acc_accounted);
//TODO: assigns compass
eulers[1] = 0.0;
//calculates final angle with complementary filter
double coef = 0.49 / (0.49 + dt);
this->angle[0] = coef * (this->angle[0] + this->angle_vel[0] * dt) + (1-coef) * eulers[0];
this->angle[1] = coef * (this->angle[1] + this->angle_vel[1] * dt) + (1-coef) * eulers[2];
this->angle[2] = coef * (this->angle[2] + this->angle_vel[2] * dt) + (1-coef) * eulers[1];
//calculates current errors
this->error[0] = this->angle[0] - this->angle_ref[0];
this->error[1] = this->angle[1] - this->angle_ref[1];
this->error[2] = this->angle[2] - this->angle_ref[2];
delete[] eulers;
}
