More precise ACC calibration for AltHold and level mode

Post Reply
alexmos
Posts: 108
Joined: Tue Aug 09, 2011 12:12 pm

More precise ACC calibration for AltHold and level mode

Post by alexmos »

Hi! I have spent a lot of time debugging my own AltHold implementation using baro, sonar and acc and found, that badly calibrated ACC tend to wrong AltHold in fast flight.

Currently we have pretty simple ACC calibration in MultiWii - it good caibrate Z-axis, but zero on X,Y axis strongly depend on orientation of sensors in the moment of calibration.
It working good for hover but not for fast flight: after long time of flight gyro vector follows wrong acc vector, and just after stopping flight leveling is not good.

But for AltHold mode, it is much more important to get uniform ACC vector for full range of inclanation angle. Even small error (2-3 units) will give 1m of altitude error.

The ACC data may be not uniform due to wrong zero calibration and different sensivity for axis.

My idea is pretty simple: measure 6 limits for 3 axis (negative and positive for each) and than calculate Zero and Scale.
My implementation replaces current procedure, but compatible with it: if we calibrate only Z axis in step 1, old method is used.

Code: Select all

void ACC_Common() {
  static int32_t a[3];
  static uint8_t curAxis = YAW;
  int8_t curLimit, axis;
  static int16_t limits[3][2] = { {0,0}, {0,0}, {0,0} };
 
  if (calibratingA>0) {
    for (axis = 0; axis < 3; axis++) {
      // Reset a[axis] at start of calibration
      if (calibratingA == 400) {
         // Find wich axis to calibrate?
         if(abs(accADC[axis]) > abs(accADC[(axis+1)%3]) + abs(accADC[(axis+2)%3])) {
            curAxis = axis;
            accScale[curAxis] = 0; // re-calibrate scale, too
         }

         a[axis]=0;
      }
         
      // Sum up 400 readings
      a[axis] +=accADC[axis];
    }

    // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
    if (calibratingA == 1) {
      curLimit = a[curAxis] > 0 ? 0 : 1; // positive - 0, negative = 1
      limits[curAxis][curLimit] = a[curAxis]/400;
     
      // If we get 2 limits for one axis, we can found precize zero and scale
      if(limits[curAxis][0] > 0 && limits[curAxis][1] < 0) {
         accZero[curAxis] = (limits[curAxis][0] + limits[curAxis][1])/2;
         accScale[curAxis] = ((int32_t)acc_1G) * 2000 / (limits[curAxis][0] - limits[curAxis][1]);
      }
      // Old (not precize) calibration for YAW only
      else if(curAxis == YAW && curLimit == 0) {
         accZero[YAW]   = a[YAW]/400 - acc_1G;
         accZero[ROLL]  = a[ROLL]/400;
         accZero[PITCH] = a[PITCH]/400;
       }

      accTrim[ROLL]   = 0;
      accTrim[PITCH]  = 0;
      writeParams(); // write accZero in EEPROM
    }
    calibratingA--;
  }
 
   // Calibrate
   for(axis=0;axis<3;axis++) {
      if(accScale[axis]) {
         accADC[axis] =  ((int32_t)(accADC[axis] - accZero[axis])) * accScale[axis] / 1000;
      } else {
         accADC[axis]-=  accZero[axis];
      }
   }
}


Also, we need to store AccScale to EEPROM (or recalibrate each time before flight)

How to use:

1. Set quad to ground and press CALIB_ACC button in GUI (or send command from RX by standard stick combination)
2. Turn upside/down (level it as presise as possible) and run calibration again
3. Rotate 90 degree (for instance, PITCH direction up) and run calibration
4. Rotate 180 degree (PITCH directed down) and run calibration
5,6. the same for ROLL.

To re-calibrate by old method, just run step 1 and skip other, and all precise data will be erased.

After this calibration, I get 1G=255 for every angle for every axis, and my AltHold working much better now!

Noctaro
Posts: 280
Joined: Thu Sep 08, 2011 11:15 am
Contact:

Re: More precise ACC calibration for AltHold and level mode

Post by Noctaro »

May this also kill the level drift over time that occurs on my quad? As it feels as some variables (pitch and roll) do constantly drift off over time. Could this be related to the vector misbehaviour? (ITG600 [orig. WMP+], BMA180, BMP055, HMC5883l )
Wich version of multiwii do you use for this mod, as i would like to give it a try!

greetz Noc

capt
Posts: 54
Joined: Wed Jan 19, 2011 10:54 pm

Re: More precise ACC calibration for AltHold and level mode

Post by capt »

Error when compiling.
Attachments
acc.JPG

alexmos
Posts: 108
Joined: Tue Aug 09, 2011 12:12 pm

Re: More precise ACC calibration for AltHold and level mode

Post by alexmos »

Noctaro wrote:May this also kill the level drift over time that occurs on my quad? As it feels as some variables (pitch and roll) do constantly drift off over time. Could this be related to the vector misbehaviour? (ITG600 [orig. WMP+], BMA180, BMP055, HMC5883l )
Wich version of multiwii do you use for this mod, as i would like to give it a try!

greetz Noc


No, time drift is not corrected by this mod. May be it's temperature drift?

This mod for early 1.9. In latest dev release only "InflightAccCalibration" was added, so if you replace acc_common by mine version, you loose it.

alexmos
Posts: 108
Joined: Tue Aug 09, 2011 12:12 pm

Re: More precise ACC calibration for AltHold and level mode

Post by alexmos »

capt wrote:Error when compiling.


Sorry, of course you need to add global variable accScale:
in MultiWii.pde:
replace

Code: Select all

static int16_t accZero[3]  = {0,0,0};


with

Code: Select all

static int16_t accZero[3]  = {0,0,0};
static uint16_t accScale[3]  = {0,0,0};  // sensivity correction (1000 for acc_1G)


To write accScale to EEPROM, in EEPROM.pde:

replace

Code: Select all

, {&accZero, sizeof(accZero)}


with

Code: Select all

, {&accZero, sizeof(accZero)}
, {&accScale, sizeof(accScale)}


and change config version

Code: Select all

static uint8_t checkNewConf = 2;


First time you run it, all you settings will be lost (because store format changed), so just before uploading new version, load setting into GUI and write after uploading.

Noctaro
Posts: 280
Joined: Thu Sep 08, 2011 11:15 am
Contact:

Re: More precise ACC calibration for AltHold and level mode

Post by Noctaro »

Ok, thanks for the info!
The drift that occurs at my quad, seems to be temperature undepended as it will also constanty drift(over time) if acclimated . Thats why i did hope that its depended to the vector misbehaviour.
But i will give it a try, just to see what it does :)

greetz Noc

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: More precise ACC calibration for AltHold and level mode

Post by marbalon »

alexmos wrote:After this calibration, I get 1G=255 for every angle for every axis, and my AltHold working much better now!


I'm working now on Alt. hold, so have a question. What altitude hold method are you using? Old 1.9, new from dev or own ?

alexmos
Posts: 108
Joined: Tue Aug 09, 2011 12:12 pm

Re: More precise ACC calibration for AltHold and level mode

Post by alexmos »

marbalon wrote:
alexmos wrote:After this calibration, I get 1G=255 for every angle for every axis, and my AltHold working much better now!


I'm working now on Alt. hold, so have a question. What altitude hold method are you using? Old 1.9, new from dev or own ?


I use my own, but it is very simular to ziss_dm method that was in old 1.9. My code does not looks like in 1.9, but principles are the same: baro + acc.
I have posted some tests into main Alt Hold thread: viewtopic.php?f=7&t=363&p=9284#p9284

Post Reply