void SensorFusion::matrixUpdate(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro) { Gyro_Vector(0) = GYRO_SCALED_RAD(gyro(0)); //gyro x roll Gyro_Vector(1) = GYRO_SCALED_RAD(gyro(1)); //gyro y pitch Gyro_Vector(2) = GYRO_SCALED_RAD(gyro(2)); //gyro z yaw Accel_Vector(0) = accel(0); Accel_Vector(1) = accel(1); Accel_Vector(2) = accel(2); Omega = Gyro_Vector + Omega_I; Omega_Vector = Omega + Omega_P; updateMatrix(0,0) = 0; updateMatrix(0,1) = -G_Dt*Omega_Vector(2);//-z updateMatrix(0,2) = G_Dt*Omega_Vector(1);//y updateMatrix(1,0) = G_Dt*Omega_Vector(2);//z updateMatrix(1,1) = 0; updateMatrix(1,2) = -G_Dt*Omega_Vector(0);//-x updateMatrix(2,0) = -G_Dt*Omega_Vector(1);//-y updateMatrix(2,1) = G_Dt*Omega_Vector(0);//x updateMatrix(2,2) = 0; temporaryMatrix = dcmMatrix * updateMatrix; dcmMatrix += temporaryMatrix; }
void Matrix_update(void) { Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw Accel_Vector[0]=accel[0]; Accel_Vector[1]=accel[1]; Accel_Vector[2]=accel[2]; Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term #if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction Update_Matrix[0][0]=0; Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z Update_Matrix[1][1]=0; Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; Update_Matrix[2][2]=0; #else // Use drift correction Update_Matrix[0][0]=0; Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z Update_Matrix[1][1]=0; Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x Update_Matrix[2][2]=0; #endif Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c int x,y; for(x=0; x<3; x++) //Matrix Addition (update) { for(y=0; y<3; y++) { DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; } } }
// 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; }