/* 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;
}
