int* get_new_data(int g, unsigned long* t_1, unsigned long* t_2)
{
//check if the new data flag is on
if ( (read_acc(0x00) & 0b00001000) == 0 )
{
printf("No new data available!\n");
}
//read in data
char buffer[6]; int i;
for (i = 0; i < 6; i++)
buffer[i] = read_acc(i+1);
//two bytes to one two-byte storage
static int raw_data[3];
for (i = 0; i < 3; i++)
{
raw_data[i] = 0;
raw_data[i] |= (buffer[2*i])<<8 | (buffer[2*i + 1]);
raw_data[i] = raw_data[i]<<16;
raw_data[i] = raw_data[i]>>16;
raw_data[i] = raw_data[i]>>2;
}
*t_1 = *t_2;
*t_2 = time_now();
return raw_data;
}
int active_g(int g)
{
if ( g != 2 && g != 4 && g != 8 )
{
printf("Value of passed argument must be 2, 4 or 8\n");
return -1;
}
//try to put the device into standby mode
if (!standby())
return -1;
//chooses the g mode
char addr = 0x0E;
char n = 0b11111100;
write_acc(addr, read_acc(addr) & n);
if ( g == 4 )
{
n = 0b00000001;
write_acc(addr, read_acc(addr) | n);
}
else if ( g == 8 )
{
n = 0b00000010;
write_acc(addr, read_acc(addr) | n);
}
//try to put the device into active mode
if (!active())
return -1;
return g;
}
int standby()
{
char n = 0b11111110;
//control register address
char addr = 0x2A;
//change just the devices active bit in control register
write_acc(addr, n & read_acc(addr));
//return the status of the operation
if ((read_acc(addr) & ~n) == 0)
return 1;
else
return 0;
}
int active()
{
char n = 0x01;
//control register address
char addr = 0x2A;
//change just the devices active bit in control register
write_acc(addr, n | read_acc(addr));
//return the status of the operation
if ( (read_acc(addr) & n) != 0)
return 1;
else
return 0;
}
int low_noise(int on)
{
//noise bit location
char n = 0b00000100;
//control register address
char addr = 0x2A;
//change just the devices active bit in control register
if (on)
write_acc(addr, n | read_acc(addr));
else
write_acc(addr, ~n & read_acc(addr));
//return the status of the operation
if ((read_acc(addr) & n) != 0)
return on;
else
return !on;
}
void MPU9250::getMotion6(float *ax, float *ay, float *az, float *gx, float *gy, float *gz)
{
read_acc();
read_gyro();
*ax = accelerometer_data[0];
*ay = accelerometer_data[1];
*az = accelerometer_data[2];
*gx = gyroscope_data[0];
*gy = gyroscope_data[1];
*gz = gyroscope_data[2];
}
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();
}
/**
* @brief Application task and state machine
*/
static void app_task(void)
{
switch (node_status)
{
case IDLE:
{
button_id_t button;
static uint32_t current_time;
static uint32_t previous_button_time;
uint8_t num_records = 1;
static button_id_t previous_button;
keyboard_input_desc_t *joystick_desc;
zid_report_data_record_t zid_report_data_joystick[2];
uint8_t report_data_buffer_joystick[80];
uint8_t *msg_ptr = &report_data_buffer_joystick[0];
if(button_mode == BUTTON_GAME_MODE)
{
/* Create the input report for the ppt control */
zid_report_data_joystick[0].report_type = INPUT;
zid_report_data_joystick[0].report_desc_identifier = KEYBOARD;
zid_report_data_joystick[0].report_data = (void *)msg_ptr;
joystick_desc = (keyboard_input_desc_t *)msg_ptr;
joystick_desc->modifier_keys = 0x00;
joystick_desc->key_code[0] = 0x00;
joystick_desc->key_code[1] = 0x00;
joystick_desc->key_code[2] = 0x00;
joystick_desc->key_code[3] = 0x00;
joystick_desc->key_code[4] = 0x00;
joystick_desc->key_code[5] = 0x00;
uint16_t x_temp = x_val;
uint16_t y_temp = y_val;
if (y_temp > (ADC_val + APPX_5_DEGREE_VALUE))
{
joystick_desc->key_code[2] = 1;//down y-axis negative
}
else if (y_temp < (ADC_val - APPX_5_DEGREE_VALUE))
{
joystick_desc->key_code[2] = 2;//up y-axis positive
}
else if (x_temp > (ADC_val + APPX_5_DEGREE_VALUE))
{
joystick_desc->key_code[2] = 3;//left x-axis negative
}
else if (x_temp < (ADC_val - APPX_5_DEGREE_VALUE))
{
joystick_desc->key_code[2] = 4;//right x-axis positive
}
else
{
//
}
}
/* Scan the button events */
button = button_scan();
/* Check if any valid vents occurred */
if ((button != BUTTON_UNKNOWN) && (button == previous_button))
{
/* Check time to previous transmission. */
current_time= sw_timer_get_time();
if ((current_time - previous_button_time) < zid_interframe_duration)
{
return;
}
else
{
/* Check if the key press mode is mouse mode */
if(BUTTON_MOUSE_MODE == button_mode)
{
if(!((key_mapping_mouse[b_state] <= BUTTON_UP_E) &&
(BUTTON_RIGHT_E >= key_mapping_mouse[b_state])))
{
/* Inter-frame duration for mouse will be less than the actual key events */
if((current_time - previous_button_time) < INTER_FRAME_DURATION_MOUSE_US)
{
return;
}
}
}
/* Store current time */
previous_button_time = current_time;
}
/* Get the key index value for the actual event */
b_state = get_zid_keyrc_button(button);
/* Check any mode change event occurred */
switch(key_mapping_media[b_state])
{
case BUTTON_MOUSE_MODE:
/* Configure for the mouse mode */
button_mode = BUTTON_MOUSE_MODE;
LED_On(LED_2);
LED_Off(LED_3);
LED_Off(LED_4);
LED_Off(LED_5);
zid_interframe_duration = INTER_FRAME_DURATION_MOUSE_US;
return;
break;
case BUTTON_PPT_MODE:
/* Configure for the ppt mode */
button_mode = BUTTON_PPT_MODE;
LED_Off(LED_2);
LED_On(LED_3);
LED_Off(LED_4);
LED_Off(LED_5);
zid_interframe_duration = INTER_FRAME_DURATION_US;
return;
break;
case BUTTON_GAME_MODE:
/* Configure for the game controller mode. */
button_mode = BUTTON_GAME_MODE;
LED_Off(LED_2);
LED_Off(LED_3);
LED_On(LED_4);
LED_Off(LED_5);
read_acc(&x_val,&y_val,&z_val,&ADC_val);
app_calculate_offset();
Correct_x_offset(&x_val,x_offset);
Correct_y_offset(&y_val,y_offset);
sw_timer_start(APP_TIMER_ACC_READ,ACCELEROMETER_SAMPLE_TIME,SW_TIMEOUT_RELATIVE,
(FUNC_PTR)acc_read_cb,NULL);
zid_interframe_duration = INTER_FRAME_DURATION_US;
return;
break;
case BUTTON_MEDIA_MODE:
/* Configure for the media player control mode */
button_mode = BUTTON_MEDIA_MODE;
LED_Off(LED_2);
LED_Off(LED_3);
LED_Off(LED_4);
LED_On(LED_5);
zid_interframe_duration = INTER_FRAME_DURATION_US;
return;
break;
default:
LED_On(LED_1);
break;
}
if(button_mode == BUTTON_MEDIA_MODE)
{
/* get the valid key inputs for the media player control */
zid_report_data_record_t zid_report_data[2];
uint8_t report_data_buffer[80];
uint8_t *msg_ptr = &report_data_buffer[0];
if(key_mapping_media[b_state] == BUTTON_INVALID)
{
return;
}
/* Create the input report for the media player control */
zid_report_data[0].report_type = INPUT;
zid_report_data[0].report_desc_identifier = KEYBOARD;
zid_report_data[0].report_data = (void *)msg_ptr;
keyboard_input_desc_t *keyboard_input_desc;
keyboard_input_desc = (keyboard_input_desc_t *)msg_ptr;
keyboard_input_desc->modifier_keys = 0x00;
keyboard_input_desc->key_code[0] = 0x00;
keyboard_input_desc->key_code[1] = 0x00;
keyboard_input_desc->key_code[2] = 0x00;
keyboard_input_desc->key_code[3] = 0x00;
keyboard_input_desc->key_code[4] = (uint8_t)key_mapping_media[b_state];
keyboard_input_desc->key_code[5] = (uint8_t)(key_mapping_media[b_state] >> 8);
num_records = 1;
if (zid_report_data_request(pairing_ref,num_records, zid_report_data, TX_OPTIONS
#ifdef RF4CE_CALLBACK_PARAM
,(FUNC_PTR)zid_report_data_confirm
#endif
))
{
node_status = TRANSMITTING;
}
}
else if(button_mode == BUTTON_PPT_MODE)
{
/* get the valid key inputs for the ppt mode */
zid_report_data_record_t zid_report_data[2];
uint8_t report_data_buffer[80];
uint8_t *msg_ptr = &report_data_buffer[0];
if(key_mapping_ppt[b_state] == BUTTON_INVALID)
{
return;
}
/* Create the input report for the ppt control */
zid_report_data[0].report_type = INPUT;
zid_report_data[0].report_desc_identifier = KEYBOARD;
zid_report_data[0].report_data = (void *)msg_ptr;
keyboard_input_desc_t *keyboard_input_desc;
keyboard_input_desc = (keyboard_input_desc_t *)msg_ptr;
keyboard_input_desc->modifier_keys = 0x00;
keyboard_input_desc->key_code[0] = key_mapping_ppt[b_state];
keyboard_input_desc->key_code[1] = 0x00;
keyboard_input_desc->key_code[2] = 0x00;
keyboard_input_desc->key_code[3] = 0x00;
keyboard_input_desc->key_code[4] = 0x00;
keyboard_input_desc->key_code[5] = 0x00;
num_records = 1;
if (zid_report_data_request(pairing_ref,num_records, zid_report_data, TX_OPTIONS
#ifdef RF4CE_CALLBACK_PARAM
,(FUNC_PTR)zid_report_data_confirm
#endif
))
{
node_status = TRANSMITTING;
b_state = BUTTON_INVALID;
}
}
else if(button_mode == BUTTON_MOUSE_MODE)
{
/* get the valid key inputs for the mouse mode */
zid_report_data_record_t zid_report_data[2];
uint8_t report_data_buffer[80];
uint8_t *msg_ptr = &report_data_buffer[0];
mouse_desc_t *mouse_desc;
if(key_mapping_mouse[b_state] == BUTTON_INVALID)
{
return;
}
/* Create the input report for the mouse control */
zid_report_data[0].report_type = INPUT;
zid_report_data[0].report_desc_identifier = MOUSE;
zid_report_data[0].report_data = (void *)msg_ptr;
mouse_desc = (mouse_desc_t *)msg_ptr;
mouse_desc->button0 = 0x00;
mouse_desc->button1 = 0x00;
mouse_desc->button2 = 0x00;
mouse_desc->x_coordinate = 0x00;
mouse_desc->y_coordinate = 0x00;
switch(key_mapping_mouse[b_state])
{
case BUTTON_UP_E:
mouse_desc->y_coordinate = MOUSE_NEGATIVE_DISPLACEMENT;
break;
case BUTTON_LEFT_E:
mouse_desc->x_coordinate = MOUSE_NEGATIVE_DISPLACEMENT;
break;
case BUTTON_RIGHT_E:
mouse_desc->x_coordinate = MOUSE_POSITIVE_DISPLACEMENT;
break;
case BUTTON_DOWN_E:
mouse_desc->y_coordinate = MOUSE_POSITIVE_DISPLACEMENT;
break;
case BUTTON_LEFT_SINGLE_CLK:
mouse_desc->button0 = 0x01;
break;
case BUTTON_RIGHT_SINGLE_CLK:
mouse_desc->button1 = 0x01;
break;
case BUTTON_MIDDLE_CLK:
mouse_desc->button2 = 0x01;
break;
case BUTTON_SCROLL_UP:
mouse_desc->y_coordinate = 1;
mouse_desc->button2 = 0x80;
break;
case BUTTON_SCROLL_DOWN:
mouse_desc->x_coordinate = -1;
mouse_desc->button2 = 0x80;
break;
default:
break;
}
msg_ptr += sizeof(mouse_desc_t);
num_records = 1;
if (zid_report_data_request(pairing_ref, num_records, zid_report_data, TX_OPTIONS
#ifdef RF4CE_CALLBACK_PARAM
,(FUNC_PTR)zid_report_data_confirm
#endif
))
{
node_status = TRANSMITTING;
b_state = BUTTON_INVALID;
}
}
else if(button_mode == BUTTON_GAME_MODE)
{
if(key_mapping_gamepad[b_state] == BUTTON_INVALID)
{
return;
}
switch(key_mapping_gamepad[b_state])
{
case BUTTON_1:
case BUTTON_2:
case BUTTON_3:
case BUTTON_4:
joystick_desc->key_code[0]=key_mapping_gamepad[b_state];
break;
case BUTTON_THROTTLE_UP:
joystick_desc->key_code[1]=0x01;
break;
case BUTTON_THROTTLE_DOWN:
joystick_desc->key_code[1]=0x02;
default:
break;
}
num_records = 1;
}
}
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;
}
}