/* Get readings from IMU and compute them to get the angles */ void computeIMU(){ /* Get Accel and Gyro readings */ IMU.getMotion6(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2]); /* Apply offsets */ for( int i=0 ; i<3 ; i++ ){ accel[i] += accelOffsets[i]; gyro[i] += gyroOffsets[i]; } /* Refresh time variables */ delta = micros() - auxtime; auxtime = delta + auxtime; if( delta > maxdelta ) maxdelta = delta; compute += delta; times++; if( times == 100 ){ avgDelta = compute/100; times = 0; compute = 0; } /* Compute offsets if needed */ if( !gyroOffReady[0] || !gyroOffReady[1] || !gyroOffReady[2] ) computeGyroOffsets(); if( !accelOffReady[0] || !accelOffReady[1] || !accelOffReady[2] ) computeAccelOffsets(); }
void loop() { // read raw accel/gyro measurements from device accelgyro.getMotion6(&accData[0], &accData[1], &accData[2], &gyrData[0], &gyrData[1], &gyrData[2]); CalculatedT(); ComplementaryFilter(accData, gyrData, &final_pitch, &final_roll); printf("pitch = %6f roll %6f dt = %lu\n", final_pitch, final_roll, elapsed); }
void CalculateOffsets(uint8_t gyroSamplingRate) { int numReadings = 0; long base_x_gyro = 0; long base_y_gyro = 0; long base_z_gyro = 0; long base_x_accel = 0; long base_y_accel = 0; long base_z_accel = 0; int16_t ax, ay, az, gx, gy, gz; delay(100); // Discard the first few reading unsigned long before = millis(); unsigned long now; do { mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); now = millis(); } while(now - before < 500/(gyroSamplingRate+1)); // ignore 500 frames of data before = millis(); do { mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // PrintMotion6Data(ax, ay, az, gx, gy, gz); base_x_gyro += gx; base_y_gyro += gy; base_z_gyro += gz; base_x_accel += ax; base_y_accel += ay; base_z_accel += az; numReadings++; now = millis(); } while(now - before < 1000/(gyroSamplingRate+1)); // collect average over 1000 frames of data base_x_gyro /= numReadings; gXOffset = base_x_gyro; base_y_gyro /= numReadings; gYOffset = base_y_gyro; base_z_gyro /= numReadings; gZOffset = base_z_gyro; base_x_accel /= numReadings; aXOffset = base_x_accel; base_y_accel /= numReadings; aYOffset = base_y_accel; base_z_accel /= numReadings; aZOffset = base_z_accel; }
bool GetMotion6(float angles[]) { int16_t accX, accY, accZ; int16_t gyroX, gyroY, gyroZ; mpu.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ); //400µs angles[0] = accX; angles[1] = accY; angles[2] = accZ; angles[3] = gyroX; angles[4] = gyroY; angles[5] = gyroZ; return true; }
void loop() { // read raw accel/gyro measurements from device accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // these methods (and a few others) are also available //accelgyro.getAcceleration(&ax, &ay, &az); //accelgyro.getRotation(&gx, &gy, &gz); // display accel/gyro x/y/z values printf("a/g: %6hd %6hd %6hd %6hd %6hd %6hd\n",ax,ay,az,gx,gy,gz); }
/* Initialize the IMU and I2C communication */ void IMUInit(){ /* Intialize communication I2C */ Wire.begin(); /* Initialize IMU. Default resolution is set to minimum, no offsets */ IMU.initialize(); /* Get first values for accel and gyro */ IMU.getMotion6(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2]); }
JNIEXPORT void JNICALL Java_com_nhl_spindp_sensors_I2C_loopI2c (JNIEnv *env, jobject thisObj) { jclass dataCls = env->FindClass("com/nhl/spindp/sensors/I2C$I2CData"); if (env->ExceptionCheck()) return; jfieldID dataField = env->GetFieldID( env->GetObjectClass(thisObj), "data", "Lcom/nhl/spindp/sensors/I2C$I2CData;"); if (env->ExceptionCheck()) return; jobject dataObj = env->GetObjectField( thisObj, dataField); if (env->ExceptionCheck()) return; if(adc) { jfieldID adcVal0Field = env->GetFieldID( dataCls, "adcVal0", "S"); jfieldID adcVal1Field = env->GetFieldID( dataCls, "adcVal1", "S"); if (env->ExceptionCheck()) return; short spanning = readADC(0); //spanning short stroom = readADC(1); //stroom env->SetShortField( dataObj, adcVal0Field, spanning); env->SetShortField( dataObj, adcVal1Field, stroom); if (env->ExceptionCheck()) return; } if(gyro) { jfieldID accDataXField = env->GetFieldID( dataCls, "accDataX", "S"); jfieldID accDataYField = env->GetFieldID( dataCls, "accDataY", "S"); jfieldID accDataZField = env->GetFieldID( dataCls, "accDataZ", "S"); //jfieldID tmpField = env->GetFieldID(dataCls, "tmp", "S"); jfieldID gyroXField = env->GetFieldID( dataCls, "gyroX", "S"); jfieldID gyroYField = env->GetFieldID( dataCls, "gyroY", "S"); jfieldID gyroZField = env->GetFieldID( dataCls, "gyroZ", "S"); if (env->ExceptionCheck()) return; int16_t gx, gy, gz, ax, ay, az; gyroscope.getMotion6( &ax, &ay, &az, &gx, &gy, &gz); env->SetShortField( dataObj, accDataXField, (short) ax);//readWord(0x68,0x3B) env->SetShortField( dataObj, accDataYField, (short) ay);//readWord(0x68,0x3D) env->SetShortField( dataObj, accDataZField, (short) az);//readWord(0x68,0x3F) //env->SetShortField(dataObj, tmpField, (result[ 6] << 8) | result[ 7]); env->SetShortField( dataObj, gyroXField, (short) gx);//readWord(0x68,0x43) env->SetShortField( dataObj, gyroYField, (short) gy);//readWord(0x68,0x45) env->SetShortField( dataObj, gyroZField, (short) gz);//readWord(0x68,0x47) if (env->ExceptionCheck()) return; } env->SetObjectField( thisObj, dataField, dataObj); if (env->ExceptionCheck()) return; }
void compute_imu(float _looptime) { looptime = _looptime; // get sensor readings mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); gx -= imu_offset[3]; gy -= imu_offset[4]; gz -= imu_offset[5]; mx = mpu.getExternalSensorWord(0); my = mpu.getExternalSensorWord(2); mz = mpu.getExternalSensorWord(4); /* computations */ compute_state_imu(); }
void atualizaSensors(int deltaT){ accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); /* Calcula os angulos (em graus) de X e Y */ accYangle = (atan2(ax,az)+PI)*RAD_TO_DEG; accXangle = (atan2(ay,az)+PI)*RAD_TO_DEG; double gyroXrate = (double)gx/131.0; double gyroYrate = -((double)gy/131.0); /* Calcula os angulos X e Y do Giroscopio*/ gyroXangle += gyroXrate*((double)deltaT/1000); gyroYangle += gyroYrate*((double)deltaT/1000); /* Aplica o Filtro Complementar*/ compAngleX = (0.93*(compAngleX+(gyroXrate*(double)deltaT/1000)))+(0.07*accXangle); compAngleY = (0.93*(compAngleY+(gyroYrate*(double)deltaT/1000)))+(0.07*accYangle); // DEBUG // if (debugSerial && debugMPU6050){ Serial.print(accXangle); Serial.print("\t"); Serial.print(accYangle); Serial.print("\t"); Serial.print(gyroXangle); Serial.print("\t"); Serial.print(gyroYangle); Serial.print("\t"); Serial.print(compAngleX); Serial.print("\t"); Serial.print(compAngleY); Serial.print("\t"); } delay(1); }
// This function reads acceleration form the MPU 6050 and returns impacts // 255-1000; void Accel_read(int *Accel) { int16_t ax, ay, az; int16_t gx, gy, gz; static int x; static int lax, lay, laz; int s1,s2,s3; // read raw accel/gyro measurements from device accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // display accel/gyro x/y/z values s1= abs(ax-lax) / 100; s2 = abs(ay-lay) / 100; s3 = abs(az-laz) / 100 ; //get maxval if((s1 > s2) && (s1 > s3)) *Accel = s1; else if((s2 > s1) && (s1 > s3)) *Accel = s2; else *Accel =s3; //fflush(stdout); //if(max > 254) // printf("\n%d",max); //update last vals laz = az; lay = ay; lax = ax; // > 255 would be a servere jolt LimitInt(Accel,0,1000); }//END ACCEL_Read
void calibrate_imu(){ delay(1000); // allow IMU to settle int total = 300; long imu_total[6] = {0, 0, 0, 0, 0, 0}; int count = 0; int i; while(count <= total){ count++; mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); imu_total[0] += ax; imu_total[1] += ay; imu_total[2] += az; imu_total[3] += gx; imu_total[4] += gy; imu_total[5] += gz; } Serial.print("Calibrated offsets :"); for(i = 0; i < 6; i++){ imu_offset[i] = imu_total[i] / total; Serial.print(imu_offset[i]); Serial.print(" "); } Serial.println(""); }
int main(int argc, char const *argv[]) { AccelVector gravity; MPU6050 accelgyro; accelgyro.initialize(); accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); int16_t x=-3940, y=-0110, z=00326; int16_t gx, gy, gz; // accelgyro.getAcceleration(&x, &y,&z); gravity.x = x; gravity.y = y; gravity.z = z; float alpha = 0.5f, gyralpha = 0.9f; // for(int i=0; i<100; i++){ // accelgyro.getAcceleration(&x, &y,&z); // gravity.x = gravity.x * alpha + x * (1 - alpha); // gravity.y = gravity.y * alpha + y * (1 - alpha); // gravity.z = gravity.z * alpha + z * (1 - alpha); // usleep(50000); // } AccelVector vec, gyr; accelgyro.getMotion6(&x, &y,&z, &gx, &gy, &gz); gyr.x = gx; gyr.y = gy; gyr.z = gz; while(1) { accelgyro.getMotion6(&x, &y,&z, &gx, &gy, &gz); //accelgyro.getAcceleration(&x, &y,&z,); vec.x = vec.x * alpha + x * (1 - alpha); vec.y = vec.y * alpha + y * (1 - alpha); vec.z = vec.z * alpha + z * (1 - alpha); gyr.x = gyr.x * gyralpha + gx * (1 - gyralpha); gyr.y = gyr.y * gyralpha + gy * (1 - gyralpha); gyr.z = gyr.z * gyralpha + gz * (1 - gyralpha); printf("Total gforce now %.3f g's gyro len %.3f | Now: %05.0f %05.0f %05.0f Gyro: %05.0f %05.0f %05.0f\n", vec.getMagnitude()/16000, gyr.getMagnitude(), vec.x, vec.y, vec.z, gyr.x, gyr.y, gyr.z); usleep(50000); } // MPU control/status vars /*bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector MPU6050 accelgyro; accelgyro.initialize(); // load and configure the DMP printf("Initializing DMP...\n"); devStatus = accelgyro.dmpInitialize(); // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready printf("Enabling DMP...\n"); accelgyro.setDMPEnabled(true); // enable Arduino interrupt detection //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); //attachInterrupt(0, dmpDataReady, RISING); mpuIntStatus = accelgyro.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it printf("DMP ready!\n"); dmpReady = true; // get expected DMP packet size for later comparison packetSize = accelgyro.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) printf("DMP Initialization failed (code %d)\n", devStatus); return 0; } while(1){ fifoCount = accelgyro.getFIFOCount(); if (fifoCount == 1024) { // reset so we can continue cleanly accelgyro.resetFIFO(); printf("FIFO overflow!\n"); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (fifoCount >= 42) { // read a packet from FIFO accelgyro.getFIFOBytes(fifoBuffer, packetSize); // display Euler angles in degrees accelgyro.dmpGetQuaternion(&q, fifoBuffer); accelgyro.dmpGetGravity(&gravity, &q); accelgyro.dmpGetYawPitchRoll(ypr, &q, &gravity); printf("ypr %7.2f %7.2f %7.2f \n", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI); } } */ return 0; }
// Acquire a data record. void acquireData(data_t* data) { data->time = micros(); mpu.getMotion6(&data->ax, &data->ay, &data->az, &data->gx, &data->gy, &data->gz); }
bool IntegrateGyro() { // Set the full scale range of the gyro uint8_t FS_SEL = 0; int16_t accX, accY, accZ; int16_t gyroX, gyroY, gyroZ; mpu.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ); //Set Starting angles unsigned long now = millis(); float dt =(now - Before)/1000.0; Before = now; //mpu.setFullScaleGyroRange(FS_SEL); // get default full scale value of gyro - may have changed from default // function call returns values between 0 and 3 uint8_t READ_FS_SEL = mpu.getFullScaleGyroRange(); // Serial.print("FS_SEL = "); // Serial.println(READ_FS_SEL); GYRO_FACTOR = 131.0/(READ_FS_SEL + 1); // get default full scale value of accelerometer - may not be default value. // Accelerometer scale factor doesn't reall matter as it divides out uint8_t READ_AFS_SEL = mpu.getFullScaleAccelRange(); // Serial.print("AFS_SEL = "); // Serial.println(READ_AFS_SEL); //ACCEL_FACTOR = 16384.0/(AFS_SEL + 1); // Remove offsets and scale gyro data float fgyroX, fgyroY, fgyroZ; fgyroX = (gyroX - gXOffset)/GYRO_FACTOR; fgyroY = -(gyroY - gYOffset)/GYRO_FACTOR; fgyroZ = -(gyroZ - gZOffset)/GYRO_FACTOR; accX = accX; // - base_x_accel; accY = accY; // - base_y_accel; accZ = accZ; // - base_z_accel; const double Q_angle = 0.001; const double Q_gyroBias = 0.003; const double R_angle = 0.03; AccelAngleY = atan2(accX, sqrt(pow(accY,2) + pow(accZ,2)))*RADIANS_TO_DEGREES; AccelAngleX = atan2(accY, sqrt(pow(accX,2) + pow(accZ,2)))*RADIANS_TO_DEGREES; AccelAngleZ = accZ; #ifdef UNFILTERED // Compute the (filtered) gyro angles fAngleX = fgyroX*dt + fLastGyroAngleX; fAngleY = fgyroY*dt + fLastGyroAngleY; fAngleZ = fgyroZ*dt + fLastGyroAngleZ; #endif #ifdef FILTERED // Apply the complementary filter to figure out the change in angle - choice of alpha is // estimated now. Alpha depends on the sampling rate... const float alpha = 0.9; float gyroAngleX = fgyroX*dt + fLastGyroAngleX; float gyroAngleY = fgyroY*dt + fLastGyroAngleY; float gyroAngleZ = fgyroZ*dt + fLastGyroAngleZ; fAngleX = alpha*gyroAngleX + (1.0 - alpha)*AccelAngleX; fAngleY = alpha*gyroAngleY + (1.0 - alpha)*AccelAngleY; fAngleZ = gyroAngleZ; //Accelerometer doesn't give z-angle #endif fLastGyroAngleX = fAngleX; fLastGyroAngleY = fAngleY; fLastGyroAngleZ = fAngleZ; return true; }
int main(int argc, char **argv) { printf("MPU6050 3-axis acceleromter example program\n"); I2Cdev::initialize(); MPU6050 accelgyro ; int16_t ax, ay, az; int16_t gx, gy, gz; accelgyro.initialize(); if ( accelgyro.testConnection() ) printf("MPU6050 connection test successful\n") ; else { fprintf( stderr, "MPU6050 connection test failed! something maybe wrong, continuing anyway though ...\n"); //return 1; } // use the code below to change accel/gyro offset values /* printf("Updating internal sensor offsets...\n"); // -76 -2359 1688 0 0 0 printf("%i \t %i \t %i \t %i \t %i \t %i\n", accelgyro.getXAccelOffset(), accelgyro.getYAccelOffset(), accelgyro.getZAccelOffset(), accelgyro.getXGyroOffset(), accelgyro.getYGyroOffset(), accelgyro.getZGyroOffset()); accelgyro.setXGyroOffset(220); accelgyro.setYGyroOffset(76); accelgyro.setZGyroOffset(-85); printf("%i \t %i \t %i \t %i \t %i \t %i\n", accelgyro.getXAccelOffset(), accelgyro.getYAccelOffset(), accelgyro.getZAccelOffset(), accelgyro.getXGyroOffset(), accelgyro.getYGyroOffset(), accelgyro.getZGyroOffset()); */ printf("\n"); printf(" ax \t ay \t az \t gx \t gy \t gz:\n"); while (true) { accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // printf(" %d \t %d \t %d \t %d \t %d \t %d\n", ax, ay, az, gx, gy, gz); // accelgyro.getAcceleration(&ax, &ay, &az); // printf(" %d \t %d \t %d \r", ax, ay, az); float axs = ax/16384.0; float ays = ay/16384.0; float azs = az/16384.0; float dxz = sqrt( axs*axs+azs*azs); float dyz = sqrt( ays*ays+azs*azs); float rotX = atan2(axs,dyz); float rotY = atan2(ays,dxz); printf(" %f \t %f \r", 180*rotX/3.14159, 180*rotY/3.14159); fflush(stdout); // fflush(stdout); bcm2835_delay(100); } return 1; }
virtual void run() { int16_t accData[3]; int16_t gyrData[3]; CTimer tm(TIMER0); tm.second(DT); tm.enable(); myPID.SetMode(AUTOMATIC); myPID.SetOutputLimits(-100.0, 100.0); myPID.SetSampleTime(PID_SAMPLE_RATE); m_roll = 0.0f; m_pitch = 0.0f; double output; #if USE_AUTO_TUNING double sp_input, sp_output, sp_setpoint, lastRoll; PID speedPID(&sp_input, &sp_output, &sp_setpoint, 35, 1, 2, DIRECT); speedPID.SetMode(AUTOMATIC); speedPID.SetOutputLimits(-config.roll_offset, config.roll_offset); speedPID.SetSampleTime(PID_SAMPLE_RATE); sp_input = 0; sp_setpoint = 0; lastRoll = 0; #endif // // loop // while (isAlive()) { // // wait timer interrupt (DT) // if (tm.wait()) { // // read sensors // m_mxMPU.lock(); m_mpu->getMotion6(&accData[0], &accData[1], &accData[2], &gyrData[0], &gyrData[1], &gyrData[2]); m_mxMPU.unlock(); // // filter // ComplementaryFilter(accData, gyrData, &m_pitch, &m_roll); #if USE_AUTO_TUNING sp_input = (lastRoll - m_roll) / PID_SAMPLE_RATE; // roll speed lastRoll = m_roll; speedPID.Compute(); m_setpoint = -sp_output; // tune output angle #else m_roll -= config.roll_offset; #endif m_input = m_roll; myPID.Compute(); if ( m_output > config.skip_interval ) { LEDs[1] = LED_ON; LEDs[2] = LED_OFF; output = map(m_output, config.skip_interval, 100, config.pwm_min, config.pwm_max); m_left->direct(DIR_FORWARD); m_right->direct(DIR_FORWARD); } else if ( m_output<-config.skip_interval ) { LEDs[1] = LED_OFF; LEDs[2] = LED_ON; output = map(m_output, -config.skip_interval, -100, config.pwm_min, config.pwm_max); m_left->direct(DIR_REVERSE); m_right->direct(DIR_REVERSE); } else { LEDs[1] = LED_OFF; LEDs[2] = LED_OFF; output = 0; m_left->direct(DIR_STOP); m_right->direct(DIR_STOP); } // // auto power off when fell. // if ( abs(m_roll)>65 ) { output = 0; } // // output // m_left->dutyCycle(output * config.left_power); m_right->dutyCycle(output * config.right_power); } }
void accelgyro_handler_planA() { // TODO - Debug Operation Plan A //accelgyro.getAcceleration(&x, &y,&z); accelgyro.getMotion6(&x, &y,&z, &gx, &gy, &gz); // accel_low.x = accel_low.x * gravity_alpha + x * (1 - gravity_alpha); // accel_low.y = accel_low.y * gravity_alpha + y * (1 - gravity_alpha); // accel_low.z = accel_low.z * gravity_alpha + z * (1 - gravity_alpha); gyro_low.x = gyro_low.x * gyro_alpha + gx * (1 - gyro_alpha); gyro_low.y = gyro_low.y * gyro_alpha + gy * (1 - gyro_alpha); gyro_low.z = gyro_low.z * gyro_alpha + gz * (1 - gyro_alpha); angle = accel_low.getAngleTo(gravity); gyroLen = gyro_low.getMagnitude(); // if((gyroLenTemp >= lastGyroLen - thresholdGyroLen) && (gyroLenTemp <= lastGyroLen + thresholdGyroLen)) // { // contGyro++; // if(contGyro >= DEBOUNCE_GYRO) // { // gyroLen = gyroLenTemp; // } // } // else // { // contGyro = 0; // } // lastGyroLen = gyroLenTemp; // Angle peak detection - OW GLORY! // if(angle < last_angle) // { // if(cont_max_angle == 0) // { // first_max_angle = last_angle; // } // cont_max_angle++; // if(cont_max_angle >= CONT_ANGLE_PEAK) // { // cont_max_angle = 0; // max_angle = (int)first_max_angle; // DEBUG_PRINTF(V_CRITICAL, "MAXANGLE %d\n", max_angle); // } // } // else // { // cont_max_angle = 0; // } // last_angle = angle; switch(accelGyroState) { case ACCELGYRO_WAIT_STOP: { if(gyroLen <= zero) { contGyro++; if(contGyro >= DEBOUNCE_GYRO) { accelGyroState = ACCELGYRO_STOP; DEBUG_PRINTF(V_CRITICAL, "Accelgyro detected zero angle threshold.\n"); contGyro = 0; } } else { contGyro = 0; } break; } case ACCELGYRO_STOP: { timeout_watchdog = 0; if(gyroLen >= start) { contGyro++; if(contGyro >= DEBOUNCE_GYRO) { DEBUG_PRINTF(V_CRITICAL, "Accelgyro detected start angle threshold.\n"); accelGyroState = ACCELGYRO_MOVE; operation_state = START_TEMPORIZATION; //START_PHOTO_CAPTURE; operation_event_flag = 1; start_operation_timer(OPERATION_TICK, 0); contGyro = 0; } } else { contGyro = 0; } break; } case ACCELGYRO_MOVE: { // Normal operation without errors or timeout if(gyroLen <= network) // TODO: Put a cont_timer_operation for minimal temporization? { contGyro++; if(contGyro >= DEBOUNCE_GYRO) { if(videoGenerated == true) // Normal operation. { accelGyroState = ACCELGYRO_WAIT_STOP; operation_state = SEND_FILE; operation_event_flag = 1; DEBUG_PRINTF(V_CRITICAL, "Accelgyro detected network angle threshold.\n"); } else if(shouldStopPhotoTimer == false) { shouldStopPhotoTimer = true; DEBUG_PRINTF(V_CRITICAL, "Accelgyro detected network angle threshold and video wasn't generated. Stop taking photos and generate video.\n"); } contGyro = 0; } } else if(operation_timeout_reached == true) // Didn't reach the angle yet and reached timeout. { if(videoGenerated == true) { accelGyroState = ACCELGYRO_WAIT_STOP; operation_state = SEND_FILE; operation_event_flag = 1; DEBUG_PRINTF(V_CRITICAL, "Accelgyro didn't detect network angle threshold and video was generated. Send file.\n"); } else if(shouldStopPhotoTimer == false) { shouldStopPhotoTimer = true; DEBUG_PRINTF(V_CRITICAL, "Accelgyro didn't detect network angle threshold and video wasn't generated. Stop taking photos and generate video.\n"); } contGyro = 0; } else { contGyro = 0; } break; } } return; }