Implementing Notch Filter and AA Filters for gyro and accelerometer

[sylvanoMTL]

I am working on improving the library. I have already implemented methods to set offsets (which allow to compensate the raw data directly on the chip) and now working on the Notch and AAF filters, p26-27.

I have written the snippet below, just in case (I have not tested it yet), it might need some verification on the bitshift operations

int ICM42688::setGyroNotchFilter(float gyroNFfreq_x,float gyroNFfreq_y,float gyroNFfreq_z,uint8_t gyro_nf_bw){
    setBank(3);
    // get clock div
    uint8_t reg;
    if (readRegisters(UB0_REG_GYRO_CONFIG0, 1, &reg) < 0) return -1;
    uint8_t clkdiv = reg & 0x3F;
    setBank(1);
    uint16_t nf_coswz;
    //uint8_t nf_coswz_sel = 0;
    uint8_t gyro_nf_coswz_low[3] = {0}; 
    uint8_t buff = 0;
    float Fdrv = 19200 /(clkdiv*10.0f); // in kHz  (19.2MHz = 19200 kHz)
    float fdesired[3] = {gyroNFfreq_x,gyroNFfreq_y,gyroNFfreq_z};  // in kHz - fesdeired between 1kz and 3 kHz
    float coswz = 0;
    for(size_t ii=0;ii<3;ii++){
      float coswz = cos(2*PI*fdesired[ii]/Fdrv);
      if(coswz <=0.875){
            nf_coswz = (uint16_t) round(coswz*256);
            gyro_nf_coswz_low = (uint8_t) (nf_coswz & 0x00FF);  //take lower part
            buff = buff & (((nf_coswz & 0xFF00)>>8)<<ii);       //take upper part and concatenate in the buffer
          }
          else{
            buff =  buff & (1<<(3+ii));//nf_coswz_sel =  nf_coswz_sel & (1<<(3+ii));
            if(coswz>0.875){
              nf_coswz = (uint16_t) round(8*(1-coswz)*256);
              gyro_nf_coswz_low = (uint8_t) (nf_coswz & 0x00FF);  //take lower part
              buff = buff & (((nf_coswz & 0xFF00)>>8)<<ii);       //take upper part and concatenate in the buffer
              }
            else if(coswz<-0.875){
              nf_coswz = (uint16_t) round(-8*(1-coswz)*256);
              gyro_nf_coswz_low = (uint8_t) (nf_coswz & 0x00FF);  //take lower part
              buff = buff & (((nf_coswz & 0xFF00)>>8)<<ii);       //take upper part and concatenate in the buffer}
              }
          }
    }
    // write to the Registers 
    if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC6, gyro_nf_coswz_low[0]) < 0) return -2;
    if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC7, gyro_nf_coswz_low[1]) < 0) return -2;
    if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC8, gyro_nf_coswz_low[2]) < 0) return -2;
    if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC9, buff < 0)) return -2;
    if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC10, gyro_nf_bw < 0)) return -2;
    //Set Bank 0 to allow data measurements
    setBank(0);
    return 1;
}   

[plusk01]

great! PRs welcome

[sylvanoMTL]

OK, will do it once I have made enough progress.

[sylvanoMTL]

I have implemented the notch filter on my branch, but I am struggling testing the effect. I have tried to generate vibrations with some equipment I have ICM on a fan frame and speeding the fan at a specific RPM to have a pure sinus excitation. If anyone could suggest a low cost experiment with affordable components, I would be please to have a try with it. (e.g. shaker + accelerometers) Otherwise, i guess the implementation I have made is correct. I am just not able to demonstrate the effect of the Notch filter.