Example #1
0
int drotek_marg2_init(drotek_marg2_t *marg2, i2c_bus_t *bus)
{
   THROW_BEGIN();
   THROW_ON_ERR(mpu6050_init(&marg2->mpu, bus, MPU6050_DLPF_CFG_94_98Hz, MPU6050_FS_SEL_500, MPU6050_AFS_SEL_4G));
   THROW_ON_ERR(hmc5883_init(&marg2->hmc, bus));
   THROW_END();
}
Example #2
0
int main()
{
	uint16_t a;
	int rv;
	uint8_t x;
		
	uart_init();	
	i2c_init();
	/*for compass*/
	uint8_t buf;
	double angle=0;
	uint16_t angle1=0;
	hmc5883_init();
	Prints("hmc5883_init ok!\n\r");
	while(1)
	{
		buf = 0x00;
		write_bytes(0x02,1,&buf);
		
		angle=CompassAngle(0,0,0);
		angle1=angle;
		Prints("Azimuth=");
		PrintInt(angle1/10);
		uart_putchar('.');
		PrintUchar(angle1%10);
		_delay_ms(300);
	}	
	return 0;
}
Example #3
0
int drotek_marg_init(drotek_marg_t *marg, i2c_bus_t *bus)
{
   THROW_BEGIN();
   THROW_ON_ERR(itg3200_init(&marg->itg, bus, ITG3200_DLPF_98HZ));
   THROW_ON_ERR(bma180_init(&marg->bma, bus, BMA180_RANGE_4G, BMA180_BW_40HZ));
   THROW_ON_ERR(hmc5883_init(&marg->hmc, bus));
   THROW_END();
}
Example #4
0
void main_thread()
{
	printf_(OS_WELCOME_MESSAGE);

	u32 res = hmc5883_init();

	while (1)
	{
		led_on(LED_1);
		hmc5883_read();
		//printf_("%u : %i %i %i\n", res, g_hmc5883.mx, g_hmc5883.my, g_hmc5883.mz);
		printf_("%u : %i \n", res, g_hmc5883.mz);
		led_off(LED_1);


		timer_delay_ms(100);
	}

	robot_main();
}
Example #5
0
int main(void)
{
   i2c_bus_t bus;
   int ret = i2c_bus_open(&bus, "/dev/i2c-3");
   if (ret < 0)
   {
      fatal("could not open i2c bus", ret);
      return EXIT_FAILURE;
   }

   /* ITG: */
   itg3200_dev_t itg;
itg_again:
   ret = itg3200_init(&itg, &bus, ITG3200_DLPF_42HZ);
   if (ret < 0)
   {
      fatal("could not inizialize ITG3200", ret);
      if (ret == -EAGAIN)
      {
         goto itg_again;   
      }
      return EXIT_FAILURE;
   }

   /* BMA: */
   bma180_dev_t bma;
   bma180_init(&bma, &bus, BMA180_RANGE_4G, BMA180_BW_10HZ);

   /* HMC: */
   hmc5883_dev_t hmc;
   hmc5883_init(&hmc, &bus);
   
   /* MS: */
   ms5611_dev_t ms;
   ret = ms5611_init(&ms, &bus, MS5611_OSR4096, MS5611_OSR4096);
   if (ret < 0)
   {
      fatal("could not inizialize MS5611", ret);
      return EXIT_FAILURE;
   }
   pthread_t thread;
   pthread_create(&thread, NULL, ms5611_reader, &ms);

   /* initialize AHRS filter: */
   madgwick_ahrs_t madgwick_ahrs;
   madgwick_ahrs_init(&madgwick_ahrs, STANDARD_BETA);

   interval_t interval;
   interval_init(&interval);
   float init = START_BETA;
   udp_socket_t *socket = udp_socket_create("10.0.0.100", 5005, 0, 0);

   /* kalman filter: */
   kalman_t kalman1, kalman2, kalman3;
   kalman_init(&kalman1, 1.0e-6, 1.0e-2, 0, 0);
   kalman_init(&kalman2, 1.0e-6, 1.0e-2, 0, 0);
   kalman_init(&kalman3, 1.0e-6, 1.0e-2, 0, 0);
   vec3_t global_acc; /* x = N, y = E, z = D */
   int init_done = 0;
   int converged = 0;
   sliding_avg_t *avg[3];
   avg[0] = sliding_avg_create(1000, 0.0);
   avg[1] = sliding_avg_create(1000, 0.0);
   avg[2] = sliding_avg_create(1000, -9.81);
   float alt_rel_last = 0.0;
   int udp_cnt = 0;
   while (1)
   {
      int i;
      float dt = interval_measure(&interval);
      init -= BETA_STEP;
      if (init < FINAL_BETA)
      {
         init = FINAL_BETA;
         init_done = 1;
      }
      madgwick_ahrs.beta = init;
      
      /* sensor data acquisition: */
      itg3200_read_gyro(&itg);
      bma180_read_acc(&bma);
      hmc5883_read(&hmc);
      
      /* state estimates and output: */
      euler_t euler;
      madgwick_ahrs_update(&madgwick_ahrs, itg.gyro.x, itg.gyro.y, itg.gyro.z, bma.raw.x, bma.raw.y, bma.raw.z, hmc.raw.x, hmc.raw.y, hmc.raw.z, 11.0, dt);
      
      quat_t q_body_to_world;
      quat_copy(&q_body_to_world, &madgwick_ahrs.quat);
      quat_rot_vec(&global_acc, &bma.raw, &q_body_to_world);
      for (i = 0; i < 3; i++)
      {
         global_acc.vec[i] -= sliding_avg_calc(avg[i], global_acc.vec[i]);
      }
      if (init_done)
      {
         kalman_in_t kalman_in;
         kalman_in.dt = dt;
         kalman_in.pos = 0;
         kalman_out_t kalman_out;

         kalman_in.acc = global_acc.x;
         kalman_run(&kalman_out, &kalman1, &kalman_in);
         kalman_in.acc = global_acc.y;
         kalman_run(&kalman_out, &kalman2, &kalman_in);
         kalman_in.acc = -global_acc.z;
         pthread_mutex_lock(&mutex);
         kalman_in.pos = alt_rel;
         pthread_mutex_unlock(&mutex);
         kalman_run(&kalman_out, &kalman3, &kalman_in);
         if (!converged)
         {
            if (fabs(kalman_out.pos - alt_rel) < 0.1)
            {
               converged = 1;   
               fprintf(stderr, "init done\n");
            }
         }
         if (converged) // && udp_cnt++ > 10)
         {
            if (udp_cnt++ == 10)
            {
               char buffer[1024];
               udp_cnt = 0;
               int len = sprintf(buffer, "%f %f %f %f %f %f %f", madgwick_ahrs.quat.q0, madgwick_ahrs.quat.q1, madgwick_ahrs.quat.q2, madgwick_ahrs.quat.q3,
                                                                 global_acc.x, global_acc.y, global_acc.z);
               udp_socket_send(socket, buffer, len);
            }
            printf("%f %f %f\n", -global_acc.z, alt_rel, kalman_out.pos);
            fflush(stdout);
         }
      }

      
   }
   return 0;
}
Example #6
0
/*
 * Application entry point.
 */
int main(void)
{
	enum led_status lstat = LST_INIT;
	EventListener el0;
	alert_status_t proto_st = ALST_INIT;
	alert_status_t bmp085_st = ALST_INIT;
	alert_status_t mpu6050_st = ALST_INIT;
	alert_status_t hmc5883_st = ALST_INIT;

	/*
	 * System initializations.
	 * - HAL initialization, this also initializes the configured device drivers
	 *   and performs the board-specific initializations.
	 * - Kernel initialization, the main() function becomes a thread and the
	 *   RTOS is active.
	 */
	halInit();
	chSysInit();

#ifdef BOARD_IMU_AHRF
	/* Clear DRDY pad */
	palClearPad(GPIOA, GPIOA_DRDY);

	/* Activates serial */
	sdStart(&SD1, NULL);
	sdStart(&SD2, NULL);

	/* Activate pwm */
	pwmStart(&PWMD1, &pwm1cfg);

	/* Activate i2c */
	i2cStart(&I2CD1, &i2c1cfg);

	/* Activate exti */
	extStart(&EXTD1, &extcfg);

#endif /* BOARD_IMU_AHRF */
#ifdef BOARD_CAPTAIN_PRO2

	/* Activates serial */
	sdStart(&SD3, NULL);
	sdStart(&SD4, NULL);

	/* Activate pwm */
	pwmStart(&PWMD3, &pwm3cfg);
	pwmStart(&PWMD4, &pwm4cfg);
	pwmStart(&PWMD5, &pwm5cfg);

	/* Activate i2c */
	i2cStart(&I2CD1, &i2c1cfg);

	/* Activate exti */
	extStart(&EXTD1, &extcfg);

	/* Activate adc */
	adcStart(&ADCD1, NULL);

#endif /* BOARD_CAPTAIN_PRO2 */

	/* alert subsys */
	chEvtInit(&alert_event_source);
	chEvtRegister(&alert_event_source, &el0, 0);

	/* init devices */
	pt_init();
	chThdSleepMilliseconds(10); /* power on delay */
#ifdef HAS_DEV_BMP085
	bmp085_init();
	chThdSleepMilliseconds(50); /* init delay */
#endif
#ifdef HAS_DEV_MS5611
	ms5611_init(&ms5611cfg);
	chThdSleepMilliseconds(50); /* init delay */
#endif
#ifdef HAS_DEV_MPU6050
	mpu6050_init(&mpu6050cfg);
	chThdSleepMilliseconds(250); /* give some time for mpu6050 configuration */
#endif
#ifdef HAS_DEV_HMC5883
	hmc5883_init(&hmc5883cfg);
#endif
#ifdef HAS_DEV_SERVOPWM
	servopwm_init(&servopwmcfg);
#endif
#ifdef HAS_DEV_NTC10K
	ntc10k_init();
#endif
#ifdef HAS_DEV_RPM
	rpm_init();
#endif

#ifdef BOARD_IMU_AHRF
	/* Set DRDY pad */
	palSetPad(GPIOA, GPIOA_DRDY);
#endif

	while (TRUE) {
		eventmask_t msk = chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(100));

		if (msk & EVENT_MASK(0)) {
			flagsmask_t fl = chEvtGetAndClearFlags(&el0);

			if (fl & ALERT_FLAG_PROTO)
				proto_st = pt_get_status();

#ifdef HAS_DEV_MPU6050
			if (fl & ALERT_FLAG_MPU6050)
				mpu6050_st = mpu6050_get_status();
#endif

#ifdef HAS_DEV_HMC5883
			if (fl & ALERT_FLAG_HMC5883)
				hmc5883_st = hmc5883_get_status();
#endif

#ifdef HAS_DEV_BMP085
			if (fl & ALERT_FLAG_BMP085)
				bmp085_st = bmp085_get_status();
#endif

#ifdef HAS_DEV_MS5611
			if (fl & ALERT_FLAG_BMP085)
				bmp085_st = ms5611_get_status();
#endif

			pt_set_sens_state(mpu6050_st, hmc5883_st, bmp085_st);
		}

		if (proto_st == ALST_FAIL || mpu6050_st == ALST_FAIL || hmc5883_st == ALST_FAIL || bmp085_st == ALST_FAIL)
			lstat = LST_FAIL;
		else if (proto_st == ALST_INIT || mpu6050_st == ALST_INIT || hmc5883_st == ALST_INIT || bmp085_st == ALST_INIT)
			lstat = LST_INIT;
		else if (proto_st == ALST_NORMAL && mpu6050_st == ALST_NORMAL && hmc5883_st == ALST_NORMAL && bmp085_st == ALST_NORMAL)
			lstat = LST_NORMAL;

		led_update(lstat);
	}
}