Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
Archivo: dcm.c Proyecto: skattoju/razr
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];
    } 
  }
}
Ejemplo n.º 3
0
// 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;
}