Restart gyro calibration during startup while gyros and accels not stable

This was posted before on the NG issues list: 
http://code.google.com/p/arducopter/issues/detail?id=94

On Kinderkram's suggestion I've added the issue again here. You may also want 
to look at a similar solution by Dror - dcaspi on RCG: 
http://www.rcgroups.com/forums/showpost.php?p=17455791&postcount=4992

The code below restarts the calibration each time the gyros or accels change 
significantly. This gives the user more time to put down the quad after 
connecting the battery, and avoids taking off with miscalibrated gyros. As a 
bonus, the leds will cycle quickly while the calibration is restarting, and 
slowly if the calibration is continuing normally. You may want to tweak the 
threshold.

Sample code for Sensors.pde; the mods are #ifdef'ed for identification.

#define WAIT_STABLE_CALIBRATION 5
void calibrateSensors(void) {
...
#ifdef WAIT_STABLE_CALIBRATION  
  float aux_float[6];
#else  
  float aux_float[3];
#endif
...
  aux_float[0] = gyro_offset_roll;
  aux_float[1] = gyro_offset_pitch;
  aux_float[2] = gyro_offset_yaw;
#ifdef WAIT_STABLE_CALIBRATION  
  aux_float[3] = acc_offset_x;
  aux_float[4] = acc_offset_y;
  aux_float[5] = acc_offset_z;
#endif

  // Take the gyro offset values
  for(i=0;i<600;i++)
  {
    Read_adc_raw();   // Read sensors
#ifdef WAIT_STABLE_CALIBRATION  
    // restart calibration while gyros or accels unstable
    for(gyro = GYROZ; gyro <= ACCELZ; gyro++)   
    {
      float delta = AN[gyro] - aux_float[gyro];
      if (delta < -WAIT_STABLE_CALIBRATION || delta > WAIT_STABLE_CALIBRATION)
        i = 0; // restart
      aux_float[gyro] += delta * 0.2;
    }
#else  
    for(gyro = GYROZ; gyro <= GYROY; gyro++)   
      aux_float[gyro] = aux_float[gyro] * 0.8 + AN[gyro] * 0.2;     // Filtering  
#endif
...
}

Original issue reported on code.google.com by patrick....@gmail.com on 20 Feb 2011 at 11:18

imawolfking / ardupirates

Restart gyro calibration during startup while gyros and accels not stable #26