void sample_MiniMU9() //Main Loop
{
  if((millis()-timer)>=100) //20 // Main loop runs at 50Hz
  {
    counter++;
    timer_old = timer;
    timer=millis();
    if (timer>timer_old)
      G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
    else
      G_Dt = 0;
    
    // *** DCM algorithm
    // Data adquisition
    Read_Gyro();   // This read gyro data
    Read_Accel();     // Read I2C accelerometer
    
    if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)
      {
      counter=0;
      Read_Compass();    // Read I2C magnetometer
      Compass_Heading(); // Calculate magnetic heading  
      }
    
    // Calculations...
    Matrix_update(); 
    Normalize();
    Drift_correction();
    Euler_angles();
    // ***
   
    printdata();
  }
   
}
Esempio n. 2
0
void estimator_update_state_analog_imu( void ) {

  analog_imu_update();

  /* Offset is set dynamic on Ground*/
  Gyro_Vector[0]= -gyro_to_zero[G_ROLL]   + gyro[G_ROLL];
  Gyro_Vector[1]= -gyro_to_zero[G_PITCH]  + gyro[G_PITCH];
  Gyro_Vector[2]= -gyro_to_zero[G_PITCH]  + gyro[G_YAW];

  Accel_Vector[0] = accel[ACC_X];
  Accel_Vector[1] = accel[ACC_Y];
  Accel_Vector[2] = accel[ACC_Z];


  Matrix_update();
  Normalize();


  Drift_correction();
  Euler_angles();

  // return euler angles to phi and theta
  estimator_phi = euler.phi-imu_roll_neutral;
  //estimator_phi = angle[ANG_ROLL];
  estimator_theta = euler.theta-imu_pitch_neutral;
  //estimator_theta = angle[ANG_PITCH];
  estimator_psi = euler.psi;

}
Esempio n. 3
0
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();
	}
}
Esempio n. 4
0
static void calculations_heading(void *pvParameters)
{
	UARTInit(3, 115200); /* baud rate setting */
	printf("Maintask: Running\n");

	CLKPWR_ConfigPPWR(CLKPWR_PCONP_PCGPIO, ENABLE);
	spiInit();
	gyroInit();

	for (;;)
	{
		/*Block waiting for the semaphore to become available. */
		if (xSemaphoreTake( xSemaphore, 0xffff ) == pdTRUE)
		{
			/* It is time to execute. */;

			/* Turn the LED off if it was on, and on if it was off. */
			LPC_GPIO1->FIOSET = (1 << 1);

			static Bool initDone = TRUE;
			if (initDone)
			{

				static uint16_t initIteration = 0;
				if (initIteration++ == 33)//not 32 since first value is dummy =0,0
				{
					initDone = FALSE;
					imuInit_2();
				}
				else
					imuInit_1();

			}
			else
			{
				Matrix_update();
				Normalize();
				Drift_correction();
				Euler_angles();

				calculations_motor();

				static uint16_t counter = 0;
				counter++;
				if (counter == 4)
				{
					counter = 0;
					xSemaphoreGive(xSemaphoreTerminal);
				}

			}

			LPC_GPIO1->FIOCLR = (1 << 1);
		}
	}
}
Esempio n. 5
0
/**
 * \return estimator_phi, estimator_theta, estimator_psi as angle in rad
 */
void ahrs_update_fw_estimator( void )
{
  Euler_angles();

  //warning, only eulers written to ahrs struct so far
  //compute_body_orientation_and_rates();

  // export results to estimator
  estimator_phi   = ahrs_float.ltp_to_imu_euler.phi - ins_roll_neutral;
  estimator_theta = ahrs_float.ltp_to_imu_euler.theta - ins_pitch_neutral;
  estimator_psi   = ahrs_float.ltp_to_imu_euler.psi;
}
Esempio n. 6
0
int main(int argc, char**argv)
{
  ros::init(argc,argv,"imu");
  ros::NodeHandle n;
  ros::Publisher out_pub = n.advertise<sensor_msgs::Imu >("imuyrp", 1000);
  ros::NodeHandle nh;
  ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000);
  sensor_msgs::Imu imu_msg;
  std_msgs::Float64 msg;
   // Read sensors, init DCM algorithm
	reset_sensor_fusion();
	removegyrooff(); 
	float Y=0.0;
	int i=0;
  while(ros::ok())
	{
		
	  // Time to read the sensors again?
	  if((clock() - timestamp) >= OUTPUT__DATA_INTERVAL)
	  {
	    timestamp_old = timestamp;
	    timestamp = clock();
	    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;
	    //cout << G_Dt << endl;
	    // Update sensor readings
	    read_sensors();

      // Run DCM algorithm
      Compass_Heading(); // Calculate magnetic heading
      Matrix_update();
      Normalize();
      Drift_correction();
      Euler_angles();
      output_angles();

      imu_msg = sensor_msgs::Imu(); 
      imu_msg.header.stamp = ros::Time::now();
      imu_msg.header.frame_id = "imu";
      imu_msg.orientation.x = TO_DEG(pitch);
      imu_msg.orientation.y = TO_DEG(roll);
      imu_msg.orientation.z = TO_DEG(yaw);
      imu_msg.orientation.w = 0.0;
      imu_msg.orientation_covariance[0] = -1;
      imu_msg.angular_velocity.x = 0.0;
      imu_msg.angular_velocity.y = 0.0;
      imu_msg.angular_velocity.z = 0.0;
      imu_msg.angular_velocity_covariance[0] = -1;
      imu_msg.linear_acceleration.x = 0.0;
      imu_msg.linear_acceleration.y = 0.0;
      imu_msg.linear_acceleration.z = 0.0;
      imu_msg.linear_acceleration_covariance[0] = -1;

      msg.data = TO_DEG(-atan2(magnetom[1],magnetom[0]));
      chatter_pub.publish(msg);
      ROS_INFO("%s %f", "send an imu message",TO_DEG(-atan2(magnetom[1],magnetom[0])));
      //ros::spinOnce();
	  }
	}
}
Esempio n. 7
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;
}
Esempio n. 8
0
void vIMU_tasks()
{
	uint8_t time_count;
	portTickType ticks_now,ticks_old = 0;
	uint8_t IMU_update_count = 0;

   	IMU_setup();
   	Baro_init();

    if(RCC_GetFlagStatus(RCC_FLAG_SFTRST) == RESET)		//if it was a hardware reset
	{
		NVIC_SystemReset();								//force software reset so that MPU sensitivity returns to normal range
	}
    NVIC_Configuration();
	SD_Init();

//////////////////////////****************************//////////////////////////////
//	create_log_file();		//Uncomment to enable log on start, else log will start only when the button on GCS is pressed
//////////////////////////****************************//////////////////////////////

	for(int y=0; y<=5; y++)   // Read first initial ADC values for offset.
		AN_OFFSET[y] = 0;

    for(;;)
	{
    	WDT_status |= 1;		//update status for IMU tasks
    	if(log_init_flag)
		{
			log_data();			//append new data to buffer
		}
    	ticks_now = xTaskGetTickCount()*2;

    	if((ticks_now - ticks_old) >= 17)		//do the following every 17ms
    	{
    		G_Dt = (ticks_now - ticks_old)/1000.0;
    		ticks_old = ticks_now;

    		if((!calibrating_IMU) && (!calibrating_mag))	//update IMU only if not calibrating IMU or MAG
    		{
    			IMU_Update_ARDU();
    			HMC5883_calculate(roll,pitch);
    			Matrix_update();
    		    Normalize();
    		    Drift_correction();
    		    Euler_angles();
    		    update_variables();
    		}
    		else if(calibrating_mag)						//if calibrating MAG then do not process IMU, only get raw values and obtain MAG offsets
    		{
    		    if(IMU_update_count >= 5)
    			{
    		    	IMU_Update_ARDU();
    				IMU_update_count = 0;
    			}
    			else
    				IMU_update_count++;
    		}
			altitude = ((Baro_update(G_Dt*1000)) - ground_alt_offset);		//time in ms

			innerloop();									//set servo output
		    if(time_count == 5)								//do this every 5*17ms ~= 100ms
		    {
		    	send_attitude();
		    	send_attitude_RAW();
		    	time_count = 0;
		    }
		    else
		    	time_count++;
    	}
		vTaskDelay( 17/ portTICK_RATE_MS );					//3ms less to compensate for IMU update time
	}
}