The problem is because you call that function in a while, and because of
for(counter = 1; counter < 51; counter++)
the counter will become again 51 (everytime the function is called) and then will enter again in that if (counter == 51)
.
To resolve that you could declare a global variable int ok = 0;
and use it like this:
void imu_raw_handler (const lcm_recv_buf_t *rbuf, const char *channel,
const imu_raw_t *msg, void *userdata) {
for(counter = 1; counter < 51; counter++)
{
gyro_roll = (gyro_roll + gyro_roll_old*(counter-1))/counter;
gyro_pitch = (gyro_pitch + gyro_pitch_old*(counter-1))/counter;
gyro_yaw = (gyro_yaw + gyro_yaw_old*(counter-1))/counter;
accel_x = (accel_x + accel_x_old*(counter-1))/counter;
accel_y = (accel_y + accel_y_old*(counter-1))/counter;
accel_z = (accel_z + accel_z_old*(counter-1))/counter;
}
if (ok == 0){ //will enter here once, only if you don't modify the ok anytime
// set zero points
axo = accel_x;
ayo = accel_y;
azo = accel_z;
gro = gyro_roll;
gpo = gyro_pitch;
gyo = gyro_yaw;
printf("Ready for Takeoff\n");
ok = 1; //this will be done once
}
}
I hope this is correct.
2
solved C: How to increment after entering an if statement