void readImu()
{
timestamp_old = timestamp;
timestamp = millis();
if (timestamp > timestamp_old)
G_Dt = (float)(timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else G_Dt = 0;
// Update sensor readings
read_sensors();
if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) // We're in calibration mode
{
check_reset_calibration_session(); // Check if this session needs a reset
}
else if (output_mode == OUTPUT__MODE_ANGLES) // Output angles
{
// Apply sensor calibration
compensate_sensor_errors();
// Run DCM algorithm
Compass_Heading(); // Calculate magnetic heading
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
}
}
// Main loop
void razor_loop(void)
{
razor_loop_input();
// Time to read the sensors again?
if ((timer_systime() - timestamp) >= OUTPUT__DATA_INTERVAL)
{
timestamp_old = timestamp;
timestamp = timer_systime();
if (timestamp > timestamp_old)
G_Dt = (double)MS2S(timestamp - timestamp_old); // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else
G_Dt = 0.0;
// Update sensor readings
read_sensors();
switch (output_mode)
{
case OUTPUT__MODE_CALIBRATE_SENSORS:
// We're in calibration mode
{
check_reset_calibration_session(); // Check if this session needs a reset
if (output_stream_on || output_single_on) output_calibration(curr_calibration_sensor);
break;
}
case OUTPUT__MODE_YPR_RAZOR:
{
// Apply sensor calibration
compensate_sensor_errors();
// Run DCM algorithm
Compass_Heading(); // Calculate magnetic heading
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
if (output_stream_on || output_single_on) output_angles();
break;
}
case OUTPUT__MODE_YPR_MADGWICK:
{
compensate_sensor_errors();
gyro[0] = GYRO_SCALED_RAD(gyro[0]);
gyro[1] = GYRO_SCALED_RAD(gyro[1]);
gyro[2] = GYRO_SCALED_RAD(gyro[2]);
// acc and mag already scaled in compensate_sensor_errors()
// Apply Madgwick algorithm
MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]);
//MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]);
//MadgwickAHRSupdate(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, magnetom[0], magnetom[1], magnetom[2]);
MadgwickYawPitchRoll(&yaw, &pitch, &roll);
if (output_stream_on || output_single_on) output_angles();
break;
}
case OUTPUT__MODE_YPR_FREEIMU:
{
compensate_sensor_errors();
gyro[0] = GYRO_SCALED_RAD(gyro[0]);
gyro[1] = GYRO_SCALED_RAD(gyro[1]);
gyro[2] = GYRO_SCALED_RAD(gyro[2]);
// acc and mag already scaled in compensate_sensor_errors()
// get sensorManager and initialise sensor listeners
//mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
//initListeners();
// wait for one second until gyroscope and magnetometer/accelerometer
// data is initialised then scedule the complementary filter task
//fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(), 1000, TIME_CONSTANT);
//public void onSensorChanged(SensorEvent event) {
//switch(event.sensor.getType()) {
//case Sensor.TYPE_ACCELEROMETER:
// copy new accelerometer data into accel array and calculate orientation
//System.arraycopy(event.values, 0, accel, 0, 3);
calculateAccMagOrientation();
//case Sensor.TYPE_GYROSCOPE:
// process gyro data
//gyroFunction(event);
gyroFunction();
//case Sensor.TYPE_MAGNETIC_FIELD:
// copy new magnetometer data into magnet array
//System.arraycopy(event.values, 0, magnet, 0, 3);
calculateFusedOrientation();
yaw = -fusedOrientation[0];
roll = -fusedOrientation[1];
pitch = fusedOrientation[2];
//if (output_stream_on || output_single_on) output_angles_freedom();
if (output_stream_on || output_single_on) output_angles();
break;
}
case OUTPUT__MODE_SENSORS_RAW:
case OUTPUT__MODE_SENSORS_CALIB:
case OUTPUT__MODE_SENSORS_BOTH:
{
if (output_stream_on || output_single_on) output_sensors();
break;
}
}
output_single_on = false;
#if DEBUG__PRINT_LOOP_TIME == true
//printf("loop time (ms) = %lu\r\n", timer_systime() - timestamp);
// Not really useful since RTC measures only 4 ms.
#endif
}
#if DEBUG__PRINT_LOOP_TIME == true
else
{
printf("waiting...\r\n");
}
#endif
return;
}
