FXAS21002C Integrating angluar rate

cancel
Showing results for 
Show  only  | Search instead for 
Did you mean: 

FXAS21002C Integrating angluar rate

2,136 Views
def152
Contributor I

I'm sucessfully reading angular rate from FXAS21002C at 100Hz at FSR 250dps. At the begining I do a simple calibration ( read 100 values, sum them up, divide by 100). I substract this value from the reading and then multiply by sensitivity sepcified in datasheet, in my case it is 0.0078125dps/bit. Next step is a simple trapezoidal integration ((PastReading + AcutialReading)/2.0)0.01. When I rotate gyro by ~90deg I get about 20deg reading.

I could get proper sensitivity value by trial and error method but I think this is not the best scenario :smileysilly:

Also I have question regarding to self-test process. Firstly I read some samples from gyro then I set ST bit in CTRL_REG1 and again read  from OUT_x_xxx?

This reading gives me const value of 8191 on all axis.

Labels (1)
Tags (2)
0 Kudos
7 Replies

1,120 Views
def152
Contributor I

Thanks for your suggestions. I will use interrupts to get data.

I meant acc+mag (I forgot to mention the model in previous post) FXOS700CQ which can't be connected to any other SPI device because it doesn't tri-state MISO pin.

0 Kudos

1,120 Views
anthonyduhamel
NXP Employee
NXP Employee

This problem can be easily fixed  with a tri-state buffer.

For example:

Capture.PNG

0 Kudos

1,120 Views
def152
Contributor I

As i mentioned before I read 100 samples in self-test mode and they all have the same value. Yours are changing so I think I should try another gyro :smileysilly:

At this moment i can't give you data from logic analyzer but I will get it in a few days.

I need only Z axis data but since it is recommended to read all 3 axis I do it.

Here is the code:

#define FXAS21002C_H_OUT_X_MSB        0x01

#define FXAS21002C_H_WHO_AM_I         0x0C

#define FXAS21002C_H_CTRL_REG0        0x0D

#define FXAS21002C_H_CTRL_REG1        0x13

enum gyroODR {

  GODR_800HZ,

  GODR_400HZ,

  GODR_200HZ,

  GODR_100HZ,

  GODR_50HZ,

  GODR_12_5HZ,

  GODR_6_25HZ,

  GODR_1_56HZ

};

enum gyroFSR {

  GFS_2000DPS,

  GFS_1000DPS,

  GFS_500DPS,

  GFS_250DPS

};

enum gyroMODE {

  MODE_STANDBY,

  MODE_READY,

  MODE_ACTIVE

};

typedef struct

{

   int16_t x;

   int16_t y;

   int16_t z;

} raw_data;

uint8_t SpiRxBuffer[6];

raw_data raw;

float prev_z;

int16_t cal_z;

void GyroInit(){

  /*

  Gyro is now in Standby mode every register

  changes should be made in this mode

  */

  // check connection by reading whoami register, it should always be 0xD7

  if(SpiRead(FXAS21002C_H_WHO_AM_I, 1) != 0xD7){

       return;

  }

  HAL_Delay(1);

  SpiWrite(FXAS21002C_H_CTRL_REG0, (GFS_250DPS)); // set FSR

  SpiWrite(FXAS21002C_H_CTRL_REG1, (GODR_100HZ |  MODE_ACTIVE));  // set ODR and switch to active mode

  HAL_Delay(100);

}    

void GyroReadData(){

  prev_z = raw.z;

  SpiRead(FXAS21002C_H_OUT_X_MSB, 6);

  raw.x = (int16_t)(SpiRxBuffer[0] << 8 | SpiRxBuffer[1]);

  raw.y = (int16_t)(SpiRxBuffer[2] << 8 | SpiRxBuffer[3]);

  raw.z = (int16_t)(SpiRxBuffer[4] << 8 | SpiRxBuffer[5]);

}

void GyroCalibrate(){

  for(int i = 0; i < 100; i++){

       GyroReadData();

       cal_z += raw.z;

       HAL_Delay(10);

  }

  cal_z = cal_z/100;

}

float GyroGetAngle(float dt, float a, float b){

  float a1 = 0; // current angle

  a1 = ((((a-cal_z)+(b-cal_z))/2.0)*dt*0.0078125) + a1; // cal_z is calibration offset

  return a1;

};

int main(void){

  float sum = 0;

  GyroInit();

  GyroCalibrate()

  //self-test mode

  //SpiWrite(FXAS21002C_H_CTRL_REG1, (GODR_100HZ | (1<<5) |MODE_ACTIVE));

     while(1){

           GyroReadData();

           sum += GyroGetAngle(0.01, raw.z, prev_z); //args: Ts, current, prev

           printf("%f\r\n", sum);

           HAL_Delay(10);

     }

}

It's a very good gryo but there is a one little catch. You can't read from both gyro and acc using SPI Smiley Sad I was wondering why everyone use I2C rather than SPI :smileygrin:

0 Kudos

1,120 Views
anthonyduhamel
NXP Employee
NXP Employee

Hi Karol,

Thanks for sharing.

Your code looks weird...

  • First, you didn't assign the enumeration values. For the MODE & FSR, it works, but not for the ODR

enum gyroODR { 

  // DR = CTRL_REG1[4:2]

  GODR_800HZ   = (0x00<<2),  

  GODR_400HZ   = (0x01<<2),  

  GODR_200HZ   = (0x02<<2),  

  GODR_100HZ   = (0x03<<2),  

  GODR_50HZ    = (0x04<<2),

  GODR_12_5HZ  = (0x05<<2),   

  GODR_6_25HZ  = (0x06<<2),  

  GODR_1_56HZ  = (0x07<<2), 

}; 

 

enum gyroFSR { 

  // FR = CTRL_REG0[1:0]

  GFS_2000DPS = 0x00, 

  GFS_1000DPS = 0x01,  

  GFS_500DPS  = 0x02,  

  GFS_250DPS  = 0x03, 

}; 

 

enum gyroMODE { 

  // STANDBY/READY =CTRL_REG1[0] ; STANDBY/ACTIVE = CTRL_REG1[1]

  MODE_READY    = 0x01,

  MODE_STANDBY  = 0x00,   

  MODE_ACTIVE   = 0x02, 

};

  • As I mentioned in an old comment, you have to use a timer to know how many time elapsed between two data collection

int main(void){ 

  float sum = 0;

  float timeElapsed = 0; 

  GyroInit();  

  GyroCalibrate() 

  while(1)

  { 

    GyroReadData(); 

    Timer_Stop();

    timeElapsed = Timer_GetTime();

    sum += GyroGetAngle(timeElapsed, raw.z, prev_z); //args: Ts, current, prev

    Timer_Reset();

    Timer_Start();

    printf("%f\r\n", sum);  

    HAL_Delay(10); 

  } 

  • I suggest to remove the printf function in your loop because it take a lot a time and that's why your integration does not work... I have two examples to improve your solution:
    • The easy one:

uint8_t counter =0;

while(1)

  { 

    GyroReadData(); 

    Timer_Stop();

    timeElapsed = Timer_GetTime();

    sum += GyroGetAngle(timeElapsed, raw.z, prev_z); //args: Ts, current, prev

    Timer_Reset();

    Timer_Start();

    if(counter%100 == 0)

      printf("%f\r\n", sum);

       

    HAL_Delay(10); 

    counter++

  } 

    • The more efficient (but it takes a RTOS):

Mutex mux;

float sum;

void xTaskDataCollection()

{

 

while(1)

  { 

    GyroReadData(); 

    Timer_Stop();

    timeElapsed = Timer_GetTime();

    Mutex_Lock(mux);

    sum += GyroGetAngle(timeElapsed, raw.z, prev_z); //args: Ts, current, prev

    Mutex_Unlock(mux);

    Timer_Reset();

    Timer_Start();

    if(counter%100 == 0)

     

    HAL_Delay(10); 

    counter++

  } 

}

void xTaskDisplay()

{

  float angle;

  while(1)

  {

   Mutex_Lock(mux);

   angle = sum;

   Mutex_Unlock(mux);

   printf("%f\r\n", angle);

    HAL_Delay(1000); 

  }

}

As i mentioned before I read 100 samples in self-test mode and they all have the same value. Yours are changing so I think I should try another gyro

Try with the new code. But that's strange it does not work...

I need only Z axis data but since it is recommended to read all 3 axis I do it.

Z axis in enough.

It's a very good gryo but there is a one little catch. You can't read from both gyro and acc using SPI  I was wondering why everyone use I2C rather than SPI

Yes you can use the same SPI bus for serveral sensors. That's why there is a chip select pin (/CS).

Anthony

0 Kudos

1,120 Views
anthonyduhamel
NXP Employee
NXP Employee

Hello Karol,

For the integration:

I'm not sure about your formula.

I would use this one:

α0 = 0 // Initialization. Can be assigned from a magnetometer data to get the absolute position

Rectangular integration

α1  = (ωz1-ωzOff)*K*Ts + α0

Trapezoidal integration

α1  = ((ωz1-ωzOff) + (ωz0-ωzOff) /2)  *K*Ts + α0

ωz1    : current Gyro Velocity (count)

ωz0    : previous Gyro Velocity (count)

ωzOff  :Gyro Velocity Offset (count)

K        :  sensitivity (°/s/count)

α1        : current angle (°)

α0        : previous anglular position (°)

Ts      : sampling time (s)

To improve the accuracy I suggest you to increase the sensor data rate. You can also compute the sampling time using a timer: it can be more accurate than using a static value (0.01).

For the SelfTest

Please take a look at the table below. The value you have looks to be okay.

Capture.PNG

Anthony

0 Kudos

1,120 Views
def152
Contributor I

Hello Anthony,

Thanks for info about Self-Test. I wasn't sure if it should be constant value.

About integration:

I'm using similar method, P is the area of individual trapeze, a is just sum of these areas and in my case ωz is yours ωz - ωzOff

The problem is "a" does not reflect reality. As i mentioned before it gives me 20 deg when i clearly rotate of approximately 90deg.

integr.png

I'm a beginner in gyro world :smileysilly:

Also another question. I know the gyro has 800Hz ODR, but can I read from it at 1kHz rate and get useful data?

0 Kudos

1,120 Views
anthonyduhamel
NXP Employee
NXP Employee

Hello Karol,

I'm using similar method, P is the area of individual trapeze a is just sum of these areas and in my case ωz is yours ωz - ωzOff

Yes, you perform the "offset calibration" as you mentioned in your 1st message "At the begining I do a simple calibration ( read 100 values, sum them up, divide by 100). I substract this value from the reading and then multiply by sensitivity sepcified in datasheet" .

So yes, your integration should be good.

The problem is "a" does not reflect reality. As i mentioned before it gives me 20 deg when i clearly rotate of approximately 90deg.

I understood your issue. Can you share your source code with me? It's really hard to know what is not okay. Maybe the sensor is not well configured. You can all try to extend the gyroscope range up to 2000dps.

I did a test with our RD-KL25-AGMP01 Datalogger board, recording data at 100Hz. I got pretty good results with the trapezoidal integration. I attached an excel spreadsheet if you want to look at the accuracy and formulas.

Thanks for info about Self-Test. I wasn't sure if it should be constant value.

Yes Karol, it should change each time. Take a look at the plots below:

with.PNGCapture.PNG

                                             With Self-Test                                                                                                                                                      Without Self-Test                                       

Here an example code:

bool bFXAS21002_GetRawGyroData(tRawGyroData* __pt_data)

{

  static uint8_t __au8_Buffer[6];

  int16_t __i16_SingleData;

  //_bFXAS21002_Write(FXAS21002_CTRL_REG1,0x27); //selftest

  if(_bFXAS21002_ReadBlock(FXAS21002_OUT_X_MSB,__au8_Buffer,6))

  {

  __i16_SingleData = (int16_t)( (__au8_Buffer[0]<<8) + __au8_Buffer[1]);

  __pt_data->i16X =  __i16_SingleData;

  __i16_SingleData = (int16_t)( (__au8_Buffer[2]<<8) + __au8_Buffer[3]);

  __pt_data->i16Y =  __i16_SingleData;

  __i16_SingleData = (int16_t)( (__au8_Buffer[4]<<8) + __au8_Buffer[5]);

  __pt_data->i16Z = __i16_SingleData;

  return TRUE;

  }

  return FALSE;

}

Also another question. I know the gyro has 800Hz ODR, but can I read from it at 1kHz rate and get useful data?

Yes you can do it, but you will have some identical data. I suggest you to synchronize the data collection with the sensor data rate by enabling the data ready interruption (SRC_DRDY) to avoid that.

Anthony

0 Kudos