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;
}
}
