Miniflyer:
I thought the calman filter was just to smooth out jittery results? Right now they are not jittery, they are not responding to my movement and drifting randomly. Tried some other start and end points for the delta t peasurement. All not working....
From the description of the problem you are having, I'd say the issue is on line 2356 of your code.
I put in code that allows me to read the raw values from the IMU on the screen, when I am troubleshooting:
void fGetIMU( void *pvParameters )
{
if ( fInitializeDevice( ) )
{
log_i( " SPI device init " );
if ( fInitializeAG() )
{
log_i( "AG init" );
if ( fDO_AG_ID() )
{
log_i( " AG self ID'd " );
if ( fInitializeM() )
{
log_i( "Init Magnetometer" );
if ( fDO_M_ID() )
{
log_i( " M self ID'd: &d", getMAG_ID_OK() );
fReboot();
vTaskDelay( 50 );
if ( fEnableGandA() == false ) // enable gyros and accelerometers
{
log_i( " Fail: Enable Gyro and Accelerometer " );
}
if ( fEnableM() == false ) // enable gyros and accelerometers
{
Serial.println( " Fail: Enable Magnetometer" );
}
if ( setupAccelScale( LSM9DS1_ACCELRANGE_8G ) )
{
log_i( " Fail: SetupAccelScale" );
}
if ( setupGyroScale ( LSM9DS1_GYROSCALE_2000DPS ) )
{
Serial.println( " Fail: setupGyroScale" );
}
if (setupMagScale( LSM9DS1_MAGGAIN_12GAUSS ) )
{
Serial.println( "FailL setupMagScale" );
}
} // if ( fDO_M_ID )
else
{
log_i( "fDO_M_ID, fail: %d", returnHighBits() ); //each LSMDS1 may have its own ID
}
} // if ( fInitializeM() )
} // if ( fDO_AG_ID() )
else
{
log_i( "fDO_AG_ID, fail: %d", returnHighBits() );
}
} // if ( fInitializeAG() )
} // if ( fInitializeDevice( ) )
boolean DidBias = false;
// boolean DidBias = true; // for testing
float TimePast = get_ESP32_uS();
float TimeNow = get_ESP32_uS();
TickType_t xLastWakeTime = xTaskGetTickCount();
TickType_t xFrequency = 25;
for (;;)
{
vTaskDelayUntil( &xLastWakeTime, xFrequency );
// read the sensor
if ( DidBias )
{
TimeNow = get_ESP32_uS();
deltat = ( TimeNow - TimePast);
if ( getLSM9DS1_ID_OK() && getMAG_ID_OK() ) // then do things
{
fReadAccelerometers();
fReadGyros();
fReadMagnetometer();
// fPrintIMU();
MahonyQuaternionUpdate ( get_aX(), get_aY(), get_aZ(), get_gX(), get_gY(), get_gZ(), get_mX(), get_mY(), get_mZ() );
yaw = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
pitch = ( (pitch * 180.0f) / PI);
yaw *= 180.0f / PI;
yaw -= 14.0f; // Declination
roll = ( (roll * 180.0f) / PI);
// Serial.print( roll );
// Serial.print( ", " );
// Serial.print( pitch );
// Serial.print( ", " );
// Serial.print( yaw );
// Serial.print( ", " );
// Serial.println( );
if ( xSemaphoreTake( sema_X, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, do not wait
{
xQueueOverwrite( xQ_X_INFO, (void *) &roll );
xEventGroupSetBits( eg, evtTweakServoX );
}
if ( xSemaphoreTake( sema_Y, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, do not wait
{
xQueueOverwrite( xQ_Y_INFO, (void *) &pitch );
xEventGroupSetBits( eg, evtTweakServoY );
}
TimePast = TimeNow;
} // if ( LSM9DS1_ID_OK && M_ID_OK ) // then do things
else
{
Serial.print ( " LSM9DS1_ID_NOT_OK ");
Serial.print ( getLSM9DS1_ID_OK() );
Serial.print ( " or MAG_ID_OK not OK " );
Serial.println( getMAG_ID_OK() );
}
} // if ( DidBias )
else
{
calibrate(); // accelerometer and gyro
// log_i( " aXbias = %f, aYbias = %f, aZbias = %f", get_aXbias(), get_aYbias(), get_aZbias() );
// log_i( " gXbias = %f, gYbias = %f, gZbias = %f", get_gXbias(), get_gYbias(), get_gZbias() );
fCalculateM_offset( );
// Serial.print( "calibration values ");
// Serial.print( ", ax ");
// Serial.print ( mpu9250.getAXbias(), 6 );
// Serial.print( ", ay ");
// Serial.print ( mpu9250.getAYbias(), 6 );
// Serial.print( ", az ");
// Serial.print ( mpu9250.getAZbias(), 6 );
// Serial.print( ", gx ");
// Serial.print ( mpu9250.getGXbias(), 6 );
// Serial.print( ", gy ");
// Serial.print ( mpu9250.getGYbias(), 6 );
// Serial.print( ", gz ");
// Serial.print ( mpu9250.getGZbias(), 6 );
// Serial.print( ", mx ");
// Serial.print ( mpu9250.getMXbias(), 6 );
// Serial.print( ", my ");
// Serial.print ( mpu9250.getMYbias(), 6 );
// Serial.print( ", mz ");
// Serial.print ( mpu9250.getMZbias(), 6 );
// Serial.println();
DidBias = true;
//// /* setting the interrupt */
xEventGroupSetBits( eg, evtLIDAR_ServoAspectChange ); // start the LIDAR scanning
}
// Serial.print( "fGetIMU " );
// Serial.print(uxTaskGetStackHighWaterMark( NULL ));
// Serial.println();
// Serial.flush();
xLastWakeTime = xTaskGetTickCount();;
} // for (;;)
vTaskDelete( NULL );
} // void fGetIMU( void *pvParameters )
If the input values are not changing the output values will not...
Anyways, consider your descriptions of your issues and how would the descriptions help someone else help you.
