int main(int argc, char** argv) { if(!parse_options(argc, argv)) return 0; init(); for(;;) { bool a, g, m; if((a = read_accel())) { if(!a_log->overflow) write_log(a_log); adjust_accel(); } if((g = read_gyro())) { if(!g_log->overflow) write_log(g_log); adjust_gyro(); } if((m = read_mag())) { if(include_mag) { if(!m_log->overflow) write_log(m_log); adjust_mag(); } } if(!a && !g && !m) usleep(1000); } return 0; }
int main(void) { SystemInit(); STM32F4_Discovery_LEDInit(LED3); //Orange STM32F4_Discovery_LEDInit(LED4); //Green STM32F4_Discovery_LEDInit(LED5); //Red STM32F4_Discovery_LEDInit(LED6); //Blue STM32F4_Discovery_PBInit(BUTTON_USER, BUTTON_MODE_GPIO); USBD_Init(&USB_OTG_dev,USB_OTG_FS_CORE_ID,&USR_desc,&USBD_CDC_cb,&USR_cb); SystemCoreClockUpdate(); // inicjalizacja dystrybucji czasu procesora init_I2C1(); // na podstawie: http://eliaselectronics.com/stm32f4-tutorials/stm32f4-i2c-mastertutorial/ //acc I2C_start(I2C1, LSM303DL_A_ADDRESS, I2C_Direction_Transmitter); I2C_write(I2C1,0x20); // LSM303_CTRL_REG1_A 0x20 I2C_write(I2C1,0x27); // Enable Accelerometer // 0x27 = 0b00100111 // Normal power mode, all axes enabled I2C_stop(I2C1); // stop the transmission //acc //mag I2C_start(I2C1, LSM303DL_M_ADDRESS, I2C_Direction_Transmitter); I2C_write(I2C1,0x02); //LSM303_MR_REG_M 0x02 I2C_write(I2C1,0x00); // Enable Magnetometer // 0x00 = 0b00000000 // Continuous conversion mode I2C_stop(I2C1); //mag //gyro I2C_start(I2C1, LSM303DL_G_ADDRESS, I2C_Direction_Transmitter); I2C_write(I2C1, 0x20); //L3G_CTRL_REG1 0x20 I2C_write(I2C1, 0x0F); // 0x0F = 0b00001111 // Normal power mode, all axes enabled I2C_stop(I2C1); //gyro char start='0'; while(1) { Delay(5); read_acc(); read_mag(); read_gyro(); start='0'; while(1) { start = usb_cdc_getc(); if(start=='1') { break; } } } /*while (1){ if(usb_cdc_kbhit()){ char c, buffer_out[15]; c = usb_cdc_getc(); switch(c){ case '3': STM32F4_Discovery_LEDToggle(LED3); sprintf(buffer_out,"LED%c = %u\r\n",c,GPIO_ReadInputDataBit(GPIOD,LED3_PIN)); usb_cdc_printf(buffer_out); break; case '4': STM32F4_Discovery_LEDToggle(LED4); sprintf(buffer_out,"LED%c = %u\r\n",c,GPIO_ReadInputDataBit(GPIOD,LED4_PIN)); usb_cdc_printf(buffer_out); break; case '5': STM32F4_Discovery_LEDToggle(LED5); sprintf(buffer_out,"LED%c = %u\r\n",c,GPIO_ReadInputDataBit(GPIOD,LED5_PIN)); usb_cdc_printf(buffer_out); break; case '6': STM32F4_Discovery_LEDToggle(LED6); sprintf(buffer_out,"LED%c = %u\r\n",c,GPIO_ReadInputDataBit(GPIOD,LED6_PIN)); usb_cdc_printf(buffer_out); break; } } button_sts = STM32F4_Discovery_PBGetState(BUTTON_USER); if(button_sts){ STM32F4_Discovery_LEDOff(LED3); STM32F4_Discovery_LEDOff(LED5); STM32F4_Discovery_LEDOff(LED3); STM32F4_Discovery_LEDOff(LED5); } }*/ }
void lsm303::handle::read() { read_acc(); read_mag(); }
int main (int argc, char **argv) { int file; int16_t temp; uint8_t data[2] = {0}; Triplet a_bias = {0}, g_bias = {0}, m_bias = {0}; FTriplet m_scale; int opt, option_index, help = 0, option_dump = 0; OptionMode option_mode = OPTION_MODE_ANGLES; float declination = 0.0; while ((opt = getopt_long(argc, argv, "d:hm:u", long_options, &option_index )) != -1) { switch (opt) { case 'd' : declination = atof (optarg); break; case 'm' : if (strcmp (optarg, "sensor") == 0) option_mode = OPTION_MODE_SENSOR; else if (strcmp (optarg, "angles") == 0) option_mode = OPTION_MODE_ANGLES; else help = 1; break; case 'u' : option_dump = 1; break; default: help = 1; break; } } if (help || argv[optind] != NULL) { printf ("%s [--mode <sensor|angles>] [--dump]\n", argv[0]); return 0; } if (!read_bias_files (&a_bias, &g_bias, &m_bias, &m_scale)) return 1; file = init_device (I2C_DEV_NAME); if (file == 0) return 1; if (option_dump) { dump_config_registers(file); printf ("\n"); } init_gyro(file, GYRO_SCALE_245DPS); init_mag(file, MAG_SCALE_2GS); init_acc(file, ACCEL_SCALE_2G); // temperature is a 12-bit value: cut out 4 highest bits read_bytes (file, XM_ADDRESS, OUT_TEMP_L_XM, &data[0], 2); temp = (((data[1] & 0x0f) << 8) | data[0]); printf ("Temperature: %d\n", temp); printf ("Temperature: %d\n", temp); if (option_mode == OPTION_MODE_SENSOR) printf (" Gyroscope (deg/s) | Magnetometer (mGs) | Accelerometer (mG)\n"); else printf (" Rotations (mag + acc):\n"); while (1) { FTriplet gyro, mag, acc, angles1; usleep (500000); read_gyro (file, g_bias, GYRO_SCALE_245DPS, &gyro); read_mag (file, m_bias, m_scale, MAG_SCALE_2GS, &mag); read_acc (file, a_bias, ACCEL_SCALE_2G, &acc); if (option_mode == OPTION_MODE_SENSOR) { printf ("gyro: %4.0f %4.0f %4.0f | ", gyro.x, gyro.y, gyro.z); printf ("mag: %4.0f %4.0f %4.0f | ", mag.x*1000, mag.y*1000, mag.z*1000); printf ("acc: %4.0f %4.0f %5.0f\n", acc.x*1000, acc.y*1000, acc.z*1000); } else { calculate_simple_angles (mag, acc, declination, &angles1); printf ("pitch: %4.0f, roll: %4.0f, yaw: %4.0f\n", angles1.x, angles1.y, angles1.z); } } return 0; }
/************************************************************************************************* Main **************************************************************************************************/ void main(void) { HAL_BOARD_INIT(); UART_init(); U0CSR &= ~0x04; ENABLE_RX(); /*setup sensors*/ sensors_init(); sensor_int_init(); /* Setup LED's */ P1DIR |= BV(0); P0DIR |= BV(4); P1_0 = 0; start_gyro(); //start the gyro init_acc(); //start the accelerometer start_mag(); start_baro(); //zero_mag(); EA = 1; SLEEPCMD |= 0x02; //pm 2 uint8 flag; uint8 IDbyte; uint8 baro_stage = 1; while(1){ if (RXin) { //If it is a cal data request if ( active_sensors & BV(4) ) { flag = 0x03; flush_data(&flag); baro_read_cal(); //End of line char flag = 0x00; flush_byte(&flag); } else if ( active_sensors > 0 ) { // P0_4 = 1; /**************************************************************************/ /* Read and transmit sensor data */ flag = 0x04; flush_byte(&flag); flush_byte(&active_sensors); /*---------------------------------------------------------------------*/ //Read accelerometer and gyro if ( active_sensors & BV(0) ) { IDbyte = BV(0); flush_data(&IDbyte); while( !( acc_int_status() ) ); //wait for interrupt read_acc(); //read the accelerometer while( !(gyro_int_status() & BV(0) ) ); //wait for interrupt read_gyro(); } /*---------------------------------------------------------------------*/ //Read Magnetometer //start_interrupts(MAG_INT); if ( active_sensors & BV(1) ) { IDbyte = BV(1); flush_data(&IDbyte); //PCON |= 1; while( !(mag_status() & 0x08 ) ); read_mag(); // mag_sleep(TRUE); } /*---------------------------------------------------------------------*/ //Barometer //uint16 delay_ticks; uint8 baro_res = 2; if ( active_sensors & BV(2) ) { IDbyte = BV(2); flush_data(&IDbyte); if (active_sensors & 0x40) { //delay_ticks = 0xFA00; baro_capture_press(baro_res); //while(delay_ticks--); baro_read_press(TRUE); //delay_ticks = 0xFA00; baro_capture_temp(); //while(delay_ticks--); baro_read_temp(TRUE); }else{ uint8 nullbyte = 3; switch (baro_stage) { case 1 : baro_capture_press(baro_res); baro_read_press(FALSE); baro_read_temp(FALSE); baro_stage++; break; case 2 : baro_read_press(TRUE); baro_read_temp(FALSE); baro_stage++; break; case 3 : baro_capture_temp(); baro_read_press(FALSE); baro_read_temp(FALSE); baro_stage++; break; case 4 : baro_read_press(FALSE); baro_read_temp(TRUE); baro_stage = 1; break; } } //baro_shutdown(); } /*---------------------------------------------------------------------*/ //Humidity if ( active_sensors & BV(3) ) { IDbyte = BV(3); flush_data(&IDbyte); humid_init(); humid_read_humidity(TRUE); } /*---------------------------------------------------------------------*/ //End of line char flag = 0x00; flush_byte(&flag); } if ( !(active_sensors & BV(5)) ) //if autopoll is off { P0_4 = 1; RXin = 0; //clear the RX flag }else{ P0_4 = 0; } } IEN0 |= 0x04; U0CSR &= ~0x04; } }