hitechnic Gyro sensor (NGY1044) noise

[HaWe]

Another issue - now it's becoming really weird:

After FindGyroOffset it's a stable GyroIntegral and GyroHeading. (Offset set to ~5)
Now I'm turning the robot for 2 sec and stop them, reset the GyroIntegral to 0.

But the GyroIntegral and Heading are running insanely fast off limits now! (2° per second!!)

Now after again running FindGyroOffset => Offset recalibrated to ~3, and all is stable again.

What's that??

Code: Select all

// Navigator Robot, TriBot architecture
// for NXC, tested with BricxCC 3.3.8.10 (2011-July-03)
// version 0.93
// based on:
// Odometry
// Compass
// Gyro sensor
// (still untuned)

//--------------------------------------------------
// mechanics, motors + sensors
//--------------------------------------------------

#define Menc(a) MotorRotationCount(a)

#define WheelDiameter   7.7    // Raddurchmesser
#define WheelGauge     28.8    // Achsbreite (Mittellinie Antriebsräder)
#define WheelGearRatio  0.5    // Untersetzungsgetriebe 1:2


                               // Radumfang
float WheelCircumference   = PI*WheelDiameter;   // 2*PI*r = PI*d

                               // Abrollstrecke pro 1° MotorEncoderTick
float WheelLegPerEncDegree = (PI*WheelDiameter*WheelGearRatio)/360;

float fMencHdgMath;           // Heading to Odometry, mathematical sense
long  MencHdg;                // Heading to Odometry, geographical sense

//--------------------------------------------------

#define GYRO    S2
#define COMPASS S3

long  CompHdg, CompOffset;
float GyroHdg, GyroValue, GyroOffset=0.0, GyroIntegral;

//--------------------------------------------------
// IO functions, sensors
//--------------------------------------------------

#define Beep(f,d) PlayTone(f,d)

#define printf1( _x, _y, _format1, _value1) { \
  string sval1 = FormatNum(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}

const char LCDline[]={56,48,40,32,24,16,8,0};

const string clrln="                 ";

inline bool btnhit(){
   return ( ButtonPressed(BTN1, false) || ButtonPressed(BTN2, false)
         || ButtonPressed(BTN3, false) || ButtonPressed(BTN4, false));
}


void PressToContinue(char Button) {
   string msg;
   if (Button==BTNCENTER)   msg="press BtnCntr...";
   else
   if (Button==BTNEXIT)     msg="press BtnExit...";
   else
   if (Button==BTNRIGHT)    msg="press BtnRight...";
   else
   if (Button==BTNLEFT)     msg="press BtnLeft...";

   printf1(0,LCDline[7],"%s", msg);
   while (!ButtonPressed(Button, false)); while (btnhit());
}

//--------------------------------------------------
// math
//--------------------------------------------------

#define min(a,b) (a<b?a:b)

#define max(a,b) (a>b?a:b)


inline long round(float f)
{
  if (f>=0) return (f + 0.5);
  else  return (f - 0.5);
}

float ArrayDegreesMean (float src[], int len) {
   int i;
   for (i=0; i<len; ++i) {
     if (src[i]>180) src[i]=(src[i]-360);
   }
   return (ArrayMean(src, NA, NA));
}

inline float ArrayMedianF(float src[], int len)
{
  float ftemp[];

  ArraySort(ftemp, src, NA, NA)
  return ftemp[(len-1)/2];
}

inline long ArrayMedianI(long src[], int len)
{
  long itemp[];

  ArraySort(itemp, src, NA, NA)
  return itemp[(len-1)/2];
}


inline void ArrayPushF(float &src[], float _new, int len)
{
  for (int i=len; i>0; --i) {src[i]=src[i-1];} // shift up
  src[0]=_new;
}


inline void ArrayPushI(long &src[], long _new, int len)
{
  for (int i=len; i>0; --i) {src[i]=src[i-1];} // shift up
  src[0]=_new;
}


//--------------------------------------------------
// sensor calibration
//--------------------------------------------------

int OldBatteryLevel;

void FindGyroOffset()
{
    float tot = 0;
    OldBatteryLevel=BatteryLevel();
    for (int i=0; i<500; ++i)
    {
       tot += SensorHTGyro(GYRO, 0); // tot var is 500 gyro readings
       if (!(i%50)) Beep(880,10);
       Wait(5);                     // 5ms between analog readings
    }
    Beep(440,100);
    GyroOffset=tot/500;
}


//------------------------------------------------------------------------------

float fxpos=0, fypos=0;
float gyrodata[5];

task Navigator(){

  long  encLeft, encRight, encLeftOld, encRightOld, dEncLeft, dEncRight;
  float fxold=0, fyold=0, Urk, Ulk, Uk, dMHdgRad, MencHdgRad;

  encLeft =Menc(OUT_B);
  encRight=Menc(OUT_C);
  fMencHdgMath=0;

  //-------------------------------------------//

  CompHdg=CompOffset=SensorHTCompass(COMPASS);

  //-------------------------------------------//
  long  GyroCalib=10000,      // must be calibrated once!
        itemp;

  GyroIntegral=0;
  GyroHdg=0;

     //-------------------------------------------//

  while(1) {
                                             //----------------------------//
     CompHdg=SensorHTCompass(COMPASS);       // Compass heading            //
                                             //----------------------------//


     //-------------------------------------------//
     

     GyroValue=SensorHTGyro(GYRO,0)- GyroOffset ;
     ArrayPushF(gyrodata, GyroValue, 5);
     GyroValue= (ArrayMean(gyrodata, 0, 5));

     GyroIntegral+=GyroValue;

     GyroHdg=(GyroIntegral*(360.0/GyroCalib));

     if (ButtonPressed(BTNLEFT, false)) {   // press  BTNLEFT:
        GyroIntegral=0;                     // reset GyroIntegral
        while (btnhit()); Beep(1000,10);
                                            // now turn robot 360°
     }                                      // then press BTNRIGHT:
     if (ButtonPressed(BTNRIGHT, false)) {  // reset GyroCalib after 360° turn
        GyroCalib=abs(GyroIntegral);
        while (btnhit());  Beep(1000,10);
     }

                                             //----------------------------//
     while (GyroHdg>=360) GyroHdg-=360;      // Gyro heading               //
     while (GyroHdg<  0)  GyroHdg+=360;      //----------------------------//


     //-------------------------------------------//
     
     encLeftOld =encLeft;                    // save old encoder values
     encRightOld=encRight;
     encLeft =Menc(OUT_B);                   // get new  encoder values
     encRight=Menc(OUT_C);

     dEncLeft=encLeft-encLeftOld;
     Ulk= WheelCircumference*dEncLeft;       // left wheel leg way
     
     dEncRight=encRight-encRightOld;
     Urk= WheelCircumference*dEncRight;      // right wheel leg way

     Uk=(Urk+Ulk)/2;                         // intermedium leg way

     dMHdgRad = atan2((Urk - Ulk), WheelGauge);     // delta orientation

     fxold=fxpos; fyold=fypos;               // save old pos (x,y)

     MencHdgRad=fMencHdgMath*PI/180;              // Hdg in deg => rad

     fxpos=fxold + Uk*cos(MencHdgRad + (dMHdgRad /2));    // new xpos
     fypos=fyold + Uk*sin(MencHdgRad + (dMHdgRad /2));    // new ypos

     MencHdgRad = MencHdgRad + dMHdgRad;               // new motor enc Heading
     fMencHdgMath=MencHdgRad*180/PI;
     MencHdg=round(-fMencHdgMath);
     
                                              //----------------------------//
     while (MencHdg >=360) MencHdg-=360;      // odometry heading           //
     while (MencHdg < 0)  MencHdg+=360;       //----------------------------//
     

     Wait(20);
  }
}


task DisplayValues() {
  while (1) {
     printf1(0, 56, "mot_B%6d", Menc(OUT_B));
     printf1(0, 48, "mot_C%6d", Menc(OUT_C));
     
     printf1(0, 40, "GValu%6.1f", GyroValue);
     printf1(0, 32, "GIntg%6.1f", GyroIntegral);
     printf1(0, 24, "GOffs%6.1f", GyroOffset);

     printf1(0, 16, "MotoH%6d",   MencHdg);

     printf1(0,  8, "CompH%6d",   CompHdg);
     printf1(0,  0, "GyroH%6.1f", GyroHdg);
     Wait(10);
  }
}



task main(){

  SetSensorHTGyro(GYRO);
  SetSensorLowspeed(COMPASS);

  start DisplayValues;
  ArrayInit(gyrodata,0,5);
  FindGyroOffset();           // calibrating Gyro Offset => ~5.0

  start Navigator;
  
  while(!btnhit());
  
  OnFwd(OUT_C,80); OnRev(OUT_B,80);

  Wait(2000);
  Off(OUT_BC);
  GyroIntegral=0;
  Wait(5000);              // GyroIntegral unstable (decreasing fast, ~2° per SECOND!)

  FindGyroOffset();        // Recalibrating => Offset ~3.0
                           // now GyroIntegral stable

  while(1);
}

(integrated odometry, don't mind!)

[afanofosc]

Doc, I can't help you with your last couple of questions but I can help you with ArrayPushF. Run the following program for a bit and you'll see what I mean.

Code: Select all

inline void ArrayPushF(float &src[], float _new)
{
  for (int i=ArrayLen(src); i>0; --i)
    src[i]=src[i-1]; // shift up
  src[0]=_new;
}

inline void ArrayPushF2(float &src[], float _new)
{
  float tmp[];
  ArraySubset(tmp, src, 1, NA);
  ArrayBuild(src, _new, tmp);
  ArrayInit(tmp, 0, 0);
}

task main()
{
  float gyrodata[];
  int j = 5;
  while (j <= 100)
  {
    NumOut(0, LCD_LINE1, j);
    ArrayInit(gyrodata, 0, j);
    unsigned long tick = CurrentTick();
    for (int i=0; i < 1000; i++)
    {
       ArrayPushF(gyrodata, i);
    }
    NumOut(0, LCD_LINE2, CurrentTick()-tick);

    tick = CurrentTick();
    for (int i=0; i < 1000; i++)
    {
       ArrayPushF2(gyrodata, i);
    }
    NumOut(0, LCD_LINE3, CurrentTick()-tick);
    j++;
    Wait(SEC_1);
  }
}

John Hansen

[HaWe]

hmmm, I don't get it...

do you mean just the speed?
or is my code faulty?

[afanofosc]

Just the speed and executable code size. Nothing important.

John Hansen

[HaWe]

ok, I'll change my code.

But before that, I urgently need a reliable Gyro Integral function, insensitive to external and internal disturbances...

[aswin0]

Doc, only you can make THE integral function better. Here are some tips.

Very often you can calibrate the offset of the gyro while the robot is in action. This gives better input to your function. The way to go is to make assumptions. Is the robot going straight? The compass can tell. Or maybe the tacos from the wheel. If so, then you know how much the used offset is off the real offset. It equals the offset corrected reading from the gyro. This value you can use as error for a PI-controller. The P value will correct the integrated result. The I value will correct the offset.
(of course, once you do this you might make assumptions on how fast the robot is turning based on compass or tacho. Ten you can use this as wel to correct the gyro.)

Don't use too high a sample rate, this gives timing errors. 3 msec could be 3.5 msec. This problem is less with a lower sample rate.

Avoid vibrations on your sensor.

Use floats or doubles instead of integers for offset and odometry.

[HaWe]

(edited)
aswin,
thx, but did you read my code?
I'm already using floats for the Gyro.
NXC hasn't got doubles at all ( to my very regret)^^ (but in this case the errors are so big that it wouldn't help anyway)

I still didn't tune the Gyro readings to navigation yet, I will try to do that in future if I once will have reliable Gyro readings.

My Offset calibration readings are just every 5 ms, in the navigation thread the gyro readings are more than 20ms .

I can't make assumptions about movements because of heavy slip of the wheels (driving outdoors on the grass / lawn or indoors on slippery parquet or 1 wheel on parquet and the other on a carpet) or passive movements if hit by or at sth.
These are actually the kind of odometry errors I want to erase by gyro readings while using odometry simultaneosly.

All what I need now is, what I already wrote:
I urgently need a reliable Gyro Integral and offset recalibration function, insensitive to external and internal disturbances, like e.g. BatteryLevel.
(edited)

[HaWe]

aswin,
I also tried your own formula to calulate gyroOffset:

Code: Select all

  GyroOffset= 0.004981 * BatteryLevel() + 558.7103   

  GyroValue=SensorHTGyro(GYRO,0) + GyroOffset ;
  // alternatively: 
  // GyroValue=SensorHTGyro(GYRO,0) - GyroOffset ;

but both lead to a drift 0f >300° per second !!

[HaWe]

I have a workaround:
I'm running this at the start, after that GyroInegral seems to be quite constant.

Code: Select all

void InitGyro() {
    int Hdg;
    
    GyroOffset=5.0;

    OnFwd(OUT_C,80); OnRev(OUT_B,80);
    Wait(2000);
    Off(OUT_BC);
    Wait(1000);

    FindGyroOffset();
    
    OnFwd(OUT_C,80); OnRev(OUT_B,80);
    Wait(2000);
    Off(OUT_BC);
    Wait(1000);

    FindGyroOffset();

}

I will have to see how long that will stay fine...

[aswin0]

aswin,
I also tried your own formula to calulate gyroOffset:

Code: Select all

  GyroOffset= 0.004981 * BatteryLevel() + 558.7103   

  GyroValue=SensorHTGyro(GYRO,0) + GyroOffset ;
  // alternatively: 
  // GyroValue=SensorHTGyro(GYRO,0) - GyroOffset ;

but both lead to a drift 0f >300° per second !!

Doc,

The 558.7103 is specific to a gyro and situation. You should really calculate this value for yourself.