int regulator_start() { mpu6050_init(); mpu6050_run_selftest(); mpu6050_calibrate_gyroscope(); mpu6050_calibrate_accelerometer(); running = 1; pthread_create(®ulator_thread_id, NULL, regulator_thread, NULL); pthread_detach(regulator_thread_id); }
void init_block(void) { g_block_type = CLASS_SENSOR; g_block_sub_type = BLOCK_ACCELEROMETER_GYRO; mpu6050_init(); mpu_data_property_init(); uart0_recv_attach(sysex_process_online, NULL); set_rgb_led(0, 0, 128); }
void imu_init() { accel_x_filter = median_filter_new(11, 0); accel_y_filter = median_filter_new(11, 0); accel_z_filter = median_filter_new(11, 0); delay(300); mpu6050_init(); delay(300); }
int attiInit(char* conf) { if(attiLoadConfig(conf) < 0) { fprintf(stderr,"Attitude: Error loading config\n"); return -1; } if(mpu6050_init()) { fprintf(stderr, "init mpu6050 failed\n"); return -1; } attiBufferIndex = 0; attiInitData(); attiInitBaseValues(); attiInitDataBuffer(); return 0; }
int main() { /* Semaphore creation */ vSemaphoreCreateBinary(ahrs_task_semaphore); vSemaphoreCreateBinary(flight_control_task_semaphore); /* Peripheral initialization */ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); led_init(); debug_port_init(); usart3_init(57600); i2c1_init(); spi1_init(); timer1_init(); pwm_timer4_init(); pwm_timer5_init(); pwm_capture_timer2_init(); pwm_capture_timer3_init(); pwm_capture_timer8_init(); //Make sure all the peripheral is finished the initialization delay_ms(5); /* Device initialization */ while(mpu6050_init()); while(hmc5983_init()); nrf24l01_init(); motor_init(); /* Task creation */ //Attitude and Heading Reference System (AHRS) task xTaskCreate(ahrs_task, (portCHAR *)"AHRS task", 4096, NULL, tskIDLE_PRIORITY + 3, NULL); xTaskCreate(flight_control_task, (portCHAR *)"Flight control task", 4096, NULL, tskIDLE_PRIORITY + 2, NULL); #ifndef DEBUG_PRINT //USART plot task xTaskCreate(usart_plot_task, (portCHAR *)"USART plot task", 2048, NULL, tskIDLE_PRIORITY + 1, NULL); #endif /* Start schedule */ vTaskStartScheduler(); return 0; }
int main(void) { i2c_bus_t bus; int ret = i2c_bus_open(&bus, "/dev/i2c-0"); if (ret < 0) { printf("could not open i2c bus", ret); return EXIT_FAILURE; } mpu6050_dev_t mpu; mpu6050_init(&mpu, &bus, MPU6050_DLPF_CFG_94_98Hz, MPU6050_FS_SEL_500, MPU6050_AFS_SEL_4G); while (1) { mpu6050_read(&mpu); } return 0; }
void mpu6050_test() { uint16_t temp; mpu6050_gyro gyro; mpu6050_accel accel; mpu6050_init(); while(1) { temp = (uint16_t)mpu6050_get_temp(); accel = mpu6050_get_accel(); gyro = mpu6050_get_gyro(); //xprintf("the temperature is:%d\r\n", temp); //xprintf("accle_X:%d accel_Y:%d accel_Z:%d\r\n", accel.X, accel.Y, accel.Z); xprintf("gyro_X:%d gyro_Y:%d gyro_Z:%d\r\n", gyro.X, gyro.Y, gyro.Z); nrf_delay_ms(200); } }
int main() { /*uint32_t i;*/ uint8_t j = 0; uint8_t acc_data[14]; // Configure pins int8_t ret; ret = mpu6050_init(); leds_enable(); /*leds_on();*/ /*_delay_ms(1000);*/ /*leds_off();*/ /*nRF24_init();*/ /*nRF24_TXMode();*/ /*if(ret != 0){*/ /*sprintf(log.buffer, "Error initializing MPU6050!\n");*/ /*}*/ // Loop do { /*if(mpu6050_read(acc_data) != 0){*/ /*ret = 1;*/ /*}*/ /*if(mpu6050_test_connection() == 0){*/ /*ret = 1;*/ /*}*/ /*log.buffer[j++ % sizeof(log)] = 'A';*/ /*if(nRF24_TXPacket("AAAA", 4) == nRF24_MASK_MAX_RT){*/ /**//*if(nRF24_TXPacket("AAAA", 4) == nRF24_MASK_TX_DS){*/ if(ret == 0){ leds_toggle(); } _delay_ms(1000); /*for(i = 0; i < 78000; i++){*/ /*}*/ } while(1); }
int main() { /* Semaphore creation */ vSemaphoreCreateBinary(ahrs_task_semaphore); /* Peripheral initialization */ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); led_init(); debug_port_init(); usart3_init(57600); i2c1_init(); timer2_init(); //Make sure all the peripheral is finished the initialization delay_ms(1000); /* Device initialization */ while(mpu6050_init()); led_on(LED1); //Initialization is finished /* Task creation */ //Attitude and Heading Reference System (AHRS) task xTaskCreate(ahrs_task, (portCHAR *)"AHRS task", 4096, NULL, tskIDLE_PRIORITY + 2, NULL); //USART plot task xTaskCreate(usart_plot_task, (portCHAR *)"USART plot task", 2048, NULL, tskIDLE_PRIORITY + 1, NULL); /* Start schedule */ vTaskStartScheduler(); return 0; }
void mwii_init(void){ //soc_init(); time_init(); uart_init(0, 38400, uart_buffer[0], 64, uart_buffer[1], 64); // setup printf stuff (specific to avr-libc) fdev_set_udata(&uart_fd, uart_get_serial_interface(0)); stdout = &uart_fd; stderr = &uart_fd; gpio_init(); //spi_init(); avr_i2c_init(0); pwm_init(); //adc0_init_default(); sei(); /* // setup stdout and stderr (avr-libc specific) fdev_setup_stream(stdout, _serial_fd_putc, _serial_fd_getc, _FDEV_SETUP_RW); fdev_set_udata(stdout, uart_get_serial_interface(0)); fdev_setup_stream(stderr, _serial_fd_putc, _serial_fd_getc, _FDEV_SETUP_RW); fdev_set_udata(stderr, uart_get_serial_interface(0)); */ // first thing must enable interrupts //kprintf("BOOT\n"); gpio_configure(GPIO_MWII_LED, GP_OUTPUT); //gpio_set(GPIO_MWII_LED); gpio_configure(GPIO_MWII_MOTOR0, GP_OUTPUT); gpio_configure(GPIO_MWII_MOTOR1, GP_OUTPUT); gpio_configure(GPIO_MWII_MOTOR2, GP_OUTPUT); gpio_configure(GPIO_MWII_MOTOR3, GP_OUTPUT); gpio_configure(GPIO_MWII_RX0, GP_INPUT | GP_PULLUP | GP_PCINT); gpio_configure(GPIO_MWII_RX1, GP_INPUT | GP_PULLUP | GP_PCINT); gpio_configure(GPIO_MWII_RX2, GP_INPUT | GP_PULLUP | GP_PCINT); gpio_configure(GPIO_MWII_RX3, GP_INPUT | GP_PULLUP | GP_PCINT); // calibrate escs //mwii_calibrate_escs(); // set initial motor speeds mwii_write_motors(MINCOMMAND, MINCOMMAND, MINCOMMAND, MINCOMMAND); brd->gpio0 = gpio_get_parallel_interface(); brd->twi0 = avr_i2c_get_interface(0); /* // I2C scanner for(int c = 0; c < 255; c++){ uint8_t buf[2]; i2c_start_read(brd->twi0, c, buf, 1); if(i2c_stop(brd->twi0) == 1 && c & 1){ kprintf("Device %x@i2c\n", c >> 1); } delay_us(10000); } */ gpio_set(GPIO_MWII_LED); i2cblk_init(&brd->mpublk, brd->twi0, MPU6050_ADDR, 8, I2CBLK_IADDR8); mpu6050_init(&brd->mpu, i2cblk_get_interface(&brd->mpublk)); //kdebug("MPU6050: %s\n", ((mpu6050_probe(&brd->mpu))?"found":"not found!")); i2cblk_init(&brd->bmpblk, brd->twi0, BMP085_ADDR, 8, I2CBLK_IADDR8); bmp085_init(&brd->bmp, i2cblk_get_interface(&brd->bmpblk)); //kdebug("BMP085: found\n"); i2cblk_init(&brd->hmcblk, brd->twi0, HMC5883L_ADDR, 8, I2CBLK_IADDR8); hmc5883l_init(&brd->hmc, i2cblk_get_interface(&brd->hmcblk)); gpio_clear(GPIO_MWII_LED); hcsr04_init(&brd->hcsr, brd->gpio0, GPIO_MWII_HCSR_TRIGGER, GPIO_MWII_HCSR_ECHO); ASYNC_PROCESS_INIT(&brd->process, mwii_task); ASYNC_QUEUE_WORK(&ASYNC_GLOBAL_QUEUE, &brd->process); //mwii_calibrate_mpu6050(); // let the escs init as well delay_us(500000L); }
void sensor_init(void) { mpu6050_init(); hmc5883l_init(); bmp085_init(); }
void rt_init_thread_entry(void* parameter) { mpu6050_init(); }
int main(void) { DDRB=0x08; /* PB3 pin of PORTB is declared output (PWM1 pin of DC Motor Driver is connected) */ DDRD=0x80; /* PD7 pin of PORTD is declared output (PWM2 pin of DC Motor Driver is connected) */ DDRA=0x0f; /*PA0,PA1,PA2 and PA3 pins of PortC are declared output ( i/p1,i/p2,i/p3 and i/p4 pins of DC Motor Driver are connected)*/ set_timercounter0_mode(1); /*Timer counter 0 is set to Phase Correct pwm mode*/ set_timercounter0_prescaler(4); /*Timer counter 0 frequency is set to 3.90625KHz*/ set_timercounter0_output_mode(2); /*Timer counter 0 output mode is set to non-inverting mode*/ set_timercounter2_mode(1); /*Timer counter 2 is set to Phase Correct pwm mode*/ set_timercounter2_prescaler(4); /*Timer counter 2 frequency is set to 3.90625KHz*/ set_timercounter2_output_mode(2); /*Timer counter 2 output mode is set to non-inverting mode*/ #if MPU6050_GETATTITUDE == 0 int16_t ax = 0; int16_t ay = 0; int16_t az = 0; int16_t gx = 0; int16_t gy = 0; int16_t gz = 0; double axg = 0; double ayg = 0; double azg = 0; double gxds = 0; double gyds = 0; double gzds = 0; double accXangle = 0; double gyroXangle = 0; double Xangle = 0 ; double error = 0; double I_error = 0; double D_error = 0; double previous_error = 0 ; double outputspeed = 0; /*double initangle = 0;*/ #endif //init uart uart_init(UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU)); //init interrupt sei(); //init mpu6050 mpu6050_init(); _delay_ms(50); #if MPU6050_GETATTITUDE == 0 mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz); mpu6050_getConvData(&axg, &ayg, &azg, &gxds, &gyds, &gzds); accXangle = (atan2(ayg,azg)+PI)*RAD_TO_DEG; gyroXangle = accXangle; #endif for(;;) { #if MPU6050_GETATTITUDE == 0 mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz); mpu6050_getConvData(&axg, &ayg, &azg, &gxds, &gyds, &gzds); #endif accXangle = (atan2(ayg,azg)+PI)*RAD_TO_DEG; gyroXangle = accXangle + gxds*dt; Xangle = 0.98*gyroXangle + 0.02*accXangle; error = 180 - Xangle; I_error += (error)*dt; D_error = (error - previous_error)/*/dt*/; outputspeed = (P_GAIN * error) + (I_GAIN * I_error) + (D_GAIN * D_error); previous_error = error; /*Bang Bang Controller if((Xangle<=(180.01))&&(Xangle>=179.99)) { PORTA = 0x00; } else if (Xangle>(180.01)) { set_timercounter0_compare_value(255); set_timercounter2_compare_value(255); PORTA = 0x0a; } else if(Xangle<(179.99)) { set_timercounter0_compare_value(255); set_timercounter2_compare_value(255); PORTA = 0x05; } Bang Bang Controller*/ if((Xangle<=(180.1))&&(Xangle>=179.9)) { PORTA = 0x00; } else if (Xangle>(180.1)) { set_timercounter0_compare_value(abs(outputspeed)); set_timercounter2_compare_value(abs(outputspeed)); PORTA = 0x0a; } else if(Xangle<(179.9)) { set_timercounter0_compare_value(abs(outputspeed)); set_timercounter2_compare_value(abs(outputspeed)); PORTA = 0x05; } #if MPU6050_GETATTITUDE == 0 char itmp[10]; /*dtostrf(ax, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(ay, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(az, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gx, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gy, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gz, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(axg, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(ayg, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(azg, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gxds, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gyds, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gzds, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(accXangle, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gyroXangle, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(initangle, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(Xangle, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(error, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(I_error, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(D_error, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');*/ dtostrf(outputspeed, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); uart_puts("\r\n"); uart_puts("\r\n"); #endif } }
int main(void) { //programowy pwm DDRC |= (1<<PC0)|(1<<PC1); PORTC |= (1<<PC0); PORTC |= (1<<PC1); TCCR2B |= (1<<WGM21); // tryb CTC TCCR2B |= (1<<CS20); // preskaler = 1 OCR2B = 199; // dodatkowy podzia³ czêsttotliwoœci przez 200 TIMSK2 |= (1<<OCIE2B); /*TCCR2 |= (1<<WGM21); // tryb CTC TCCR2 |= (1<<CS20); // preskaler = 1 OCR2 = 199; // dodatkowy podzia³ czêsttotliwoœci przez 200 TIMSK |= (1<<OCIE2);*/ #if MPU6050_GETATTITUDE == 0 int16_t ax = 0; int16_t ay = 0; int16_t az = 0; int16_t gx = 0; int16_t gy = 0; int16_t gz = 0; double axg = 0; double ayg = 0; double azg = 0; double gxds = 0; double gyds = 0; double gzds = 0; #endif #if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2 //long *ptr = 0; double qw = 1.0f; double qx = 0.0f; double qy = 0.0f; double qz = 0.0f; double roll = 0.0f; double pitch = 0.0f; double yaw = 0.0f; #endif //uart_init(UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU)); sei(); mpu6050_init(); _delay_ms(50); int maxa=0,maxg=0,aax,pom; //init mpu6050 dmp processor #if MPU6050_GETATTITUDE == 2 mpu6050_dmpInitialize(); mpu6050_dmpEnable(); _delay_ms(10); #endif DDRD |= (1<<PD0); PORTD |= (1<<PD0); lcd_init(); int licz=1,suma=0,_ax,fi,bak; int i; int ii,jj; int sprawdz; //pid /*float wzmocnienieP=10; //Wzmocnienie float stalaI=0.4; //Sta³a czasowa ca³kowania float stalaD=12; //Sta³a czasowa ró¿niczkowania */ float wzmocnienieP=14; //Wzmocnienie float stalaI=0; //Sta³a czasowa ca³kowania float stalaD=0; float czas=0.15; //Czas zmian wielkoœci int predkosc=127; //Prêdkoœæ silników int predkosc_k=127; int sterowanie,uchyb,uchyb_pop=0; int calka=0; int lqr; //KF /*Matrix *x_post = matrix_alloc(4,1); for(ii=0;ii<4;ii++) { for(jj=0;jj<1;jj++) { x_post->matrix_entry[ii][jj]=0; } } Matrix *P_post = matrix_alloc(4,4); for(ii=0;ii<4;ii++) { for(jj=0;jj<4;jj++) { P_post->matrix_entry[ii][jj]=1; } } Matrix *V = matrix_alloc(4,4); for(ii=0;ii<4;ii++) { for(jj=0;jj<4;jj++) { if(ii==jj) V->matrix_entry[ii][ii]=0.8; else V->matrix_entry[ii][jj]=0; } } Matrix *W = matrix_alloc(2,2); W->matrix_entry[0][0]=0.02; W->matrix_entry[0][1]=0; W->matrix_entry[1][0]=0; W->matrix_entry[1][1]=4; //LQR Matrix *K_C = matrix_alloc(1,4); K_C->matrix_entry[0][0]=80; K_C->matrix_entry[0][1]=28.6388; K_C->matrix_entry[0][2]=-63.7184; K_C->matrix_entry[0][3]=-5.6401; */ for(;;) { lcd_cls(); #if MPU6050_GETATTITUDE == 0 mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz); mpu6050_getConvData(&axg, &ayg, &azg, &gxds, &gyds, &gzds); #endif #if MPU6050_GETATTITUDE == 1 mpu6050_getQuaternion(&qw, &qx, &qy, &qz); mpu6050_getRollPitchYaw(&roll, &pitch, &yaw); _delay_ms(10); #endif #if MPU6050_GETATTITUDE == 2 if(mpu6050_getQuaternionWait(&qw, &qx, &qy, &qz)) { mpu6050_getRollPitchYaw(qw, qx, qy, qz, &roll, &pitch, &yaw); } _delay_ms(10); #endif #if MPU6050_GETATTITUDE == 0 //char itmp[10]; /*ltoa(ax, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); ltoa(ay, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); ltoa(az, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); ltoa(gx, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); ltoa(gy, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); ltoa(gz, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); uart_puts("\r\n"); dtostrf(axg, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(ayg, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(azg, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gxds, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gyds, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gzds, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); uart_puts("\r\n"); uart_puts("\r\n");*/ /*lcd_cls(); lcd_str("a=("); lcd_locate(0,3); lcd_int(axg); lcd_locate(0,6); lcd_str(","); lcd_locate(0,7); lcd_int(ayg); lcd_locate(0,10); lcd_str(","); lcd_locate(0,11); lcd_int(azg); lcd_locate(0,14); lcd_str(")"); lcd_locate(1,0); lcd_str("g=("); lcd_locate(1,3); lcd_int(gxds); lcd_locate(1,6); lcd_str(","); lcd_locate(1,7); lcd_int(gyds); lcd_locate(1,10); lcd_str(","); lcd_locate(1,11); lcd_int(gzds); lcd_locate(1,14); lcd_str(")"); _delay_ms(100);*/ /*if(ax<0) ax*=-1; if(gx<0) gx*=-1; if(ax>maxa) maxa=ax; if(gx>maxg) maxg=gx; lcd_int(maxa); lcd_locate(1,0); lcd_int(maxg); _delay_ms(100);*/ /* lcd_int(ax); lcd_locate(1,0); lcd_int(gx); _delay_ms(100);*/ //REGULATOR PROPORCJONALNY /*int K=26; ax/=100; if(ax<0) ax *= -1; pom=-K*ax+255; if(pom<0) pom = 0; pwm1=pom; if(ax>=0) PORTC &= ~(1<<PC1); else PORTC |= (1<<PC1); licz++;*/ //if((licz%10000)==0) //{ /*lcd_cls(); lcd_int(pwm1); _delay_ms(1000); //} * */ //############################33 ax /= 100; suma+=ax; licz++; if(licz == 20) { suma /= 20; fi=suma; //lcd_cls(); //lcd_int(fi); //_delay_ms(500); // if(fi>=0) { PORTC &= ~(1<<PC1); } if(fi<0) { PORTC |= (1<<PC1); fi *= -1; } //REGULATOR PROPORCJONALNY //int K=80; //pom=K*fi; //if(pom>255) pom = 255; //pwm1=pom; //PID if(fi<0) { uchyb = -1*fi; } else { uchyb = fi; } calka += stalaI*fi; sterowanie = (int)(wzmocnienieP*fi)/* - (int)(stalaD*((fi - uchyb_pop)))*/ + (int)(calka); sprawdz = predkosc - sterowanie; if(sterowanie>255) sterowanie = 255; if(sterowanie<0) sterowanie = 0-sterowanie; if(sterowanie<-255) sterowanie = 255; //lcd_int(sterowanie); //_delay_ms(1000); pwm1=sterowanie; uchyb_pop=fi; suma=0; licz=1; } //LQR //Wypelnianie macierzy A /*Matrix *A; A = matrix_alloc(4,4); for(ii=0;ii<4;ii++) { for(jj=0;jj<4;jj++) { A->matrix_entry[ii][jj]=0; } } A->matrix_entry[0][1]=1; A->matrix_entry[2][3]=1; A->matrix_entry[1][1]=-0.1192; A->matrix_entry[1][2]=6.7359; A->matrix_entry[3][1]=-1.2764; A->matrix_entry[3][2]=177.1575; //Wypelnianie macierzy B Matrix *B; B = matrix_alloc(4,1); for(ii=0;ii<4;ii++) { B->matrix_entry[ii][0]=0; } B->matrix_entry[1][0]=1.1923; B->matrix_entry[3][0]=12.7640; //Wypelnianie macierzy C Matrix *C; C = matrix_alloc(2,4); for(ii=0;ii<2;ii++) { for(jj=0;jj<4;jj++) { C->matrix_entry[ii][jj]=0; } } C->matrix_entry[0][0]=1; C->matrix_entry[1][2]=1; //u=pwm1 Matrix *x; x = matrix_alloc(4,1); x->matrix_entry[0][0]=0.000628*licznik; //m ################################# //x->matrix_entry[1][0]=6.28/(-0.0588*pwm1+20); //m/s x->matrix_entry[1][0]=3.1416/(5000*pwm1); //m/s x->matrix_entry[2][0]=(ax*9)/1600; //16000 - 90 stopni x->matrix_entry[3][0]=gy; // Matrix *y = matrix_alloc(2,1); y->matrix_entry[0][0]=x->matrix_entry[0][0]; y->matrix_entry[1][0]=x->matrix_entry[2][0]; //lcd_cls(); //lcd_int((int)x->matrix_entry[2][0]); //_delay_ms(500); //FILTR KALMANA Matrix *Ax = matrix_alloc(4,1); Matrix *Bu = matrix_alloc(4,1); Matrix *x_pri = matrix_alloc(4,1); Matrix *AP = matrix_alloc(4,4); Matrix *AT = matrix_alloc(4,4); Matrix *APAT = matrix_alloc(4,4); Matrix *P_pri = matrix_alloc(4,4); Matrix *eps = matrix_alloc(2,1); Matrix *CX = matrix_alloc(2,1); // x(t+1|t) = Ax(t|t) + Bu(t) Ax=matrix_multiply(A, x_post); Bu->matrix_entry[0][0]=0; Bu->matrix_entry[1][0]=pwm1*B->matrix_entry[1][0]; Bu->matrix_entry[2][0]=0; Bu->matrix_entry[3][0]=pwm1*B->matrix_entry[3][0]; matrix_add(x_pri, Ax, Bu); // P(t+1|t) = AP(t|t)A^T + V AP=matrix_multiply(A, P_post); AT=matrix_transpose(A); APAT=matrix_multiply(AP, AT); matrix_add(P_pri, APAT, V); // eps(t) = y(t) - Cx(t|t-1) CX=matrix_multiply(C, x_pri); matrix_subtract(eps,y,CX); Matrix *CP = matrix_alloc(2,4); Matrix *CPCT = matrix_alloc(2,2); Matrix *CT = matrix_alloc(4,2); Matrix *S = matrix_alloc(2,2); Matrix *PCT = matrix_alloc(4,2); //Matrix *S1 = matrix_alloc(2,2); Matrix *K = matrix_alloc(4,2); Matrix *Keps = matrix_alloc(4,1); Matrix *KS = matrix_alloc(4,2); Matrix *KT = matrix_alloc(2,4); Matrix *KSKT = matrix_alloc(4,4); // S(t) = CP(t|t-1)C^T + W CP=matrix_multiply(C, P_pri); CT=matrix_transpose(C); CPCT=matrix_multiply(CP, CT); matrix_add(S, CPCT, W); // K(t) = P(t|t-1)C^TS(t)^-1 PCT=matrix_multiply(P_pri, CT); matrix_invert(S); K=matrix_multiply(PCT, S); //S^-1 // x(t|t) = x(t|t-1) + K(t)eps(t) Keps=matrix_multiply(K, eps); matrix_add(x_post, x_pri, Keps); // P(t|t) = P(t|t-1) - K(t)S(t)K(t)^T matrix_invert(S); KS=matrix_multiply(K, S);//S KT=matrix_transpose(K); KSKT=matrix_multiply(KS, KT); matrix_subtract(P_post, P_pri, KSKT); //LQR Matrix *KX = matrix_alloc(1,1); KX=matrix_multiply(K, x_post); lqr=(int)(KX->matrix_entry[0][0]); sprawdz=predkosc + lqr; if(sprawdz>255) sprawdz = 255; if(sprawdz<0) sprawdz = 0; pwm1=sprawdz; pwm1 = predkosc */ //############ //matrix_t *A = make_matrix( 4, 4 ); //put_entry_matrix( eqs, if1, if1, dx ); /*lcd_cls(); lcd_int(fi); lcd_locate(0,7); lcd_int(pwm1); _delay_ms(500);*/ //################################# /*for(i=255;i>=0;i--) { pwm1=i; _delay_ms(30); if((i%10)==0) { lcd_cls(); lcd_int(pwm1); } } for(i=0;i<=255;i++) { pwm1=i; _delay_ms(30); if((i%10)==0) { lcd_cls(); lcd_int(pwm1); } }*/ #endif #if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2 //quaternion /*ptr = (long *)(&qw); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); ptr = (long *)(&qx); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); ptr = (long *)(&qy); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); ptr = (long *)(&qz); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); //roll pitch yaw ptr = (long *)(&roll); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); ptr = (long *)(&pitch); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); ptr = (long *)(&yaw); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); uart_putc('\n');*/ //lcd_int((int)qw); //_delay_ms(1000); #endif } }
/* * 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); } }
int main(void) { /* Configure Watch dog timer as an interval timer. WDTIFG is * set upon expiration of selected time interval, and PUC is not * generated, so there is no reset of the device. Also, WDTIE bit * remains unchanged, so you don't have to reset the WDT interrupt. * * WDTCTL is 16 bits and always needs to be accessed with * the upper 8 bits as the WDT password, WDTPW (0x5A). * Use ACLK for WDTCNT - selected with the WDTSSEL bit * Set WDTTMSEL bit to 1 for interval timer mode. * WDTIS0 and WDTIS1 set the interval. * 00 = WDT clock source/32768 **This is the PUC value * 01 = WDT clock source/8192 * 10 = WDT clock source/512 * 11 = WDT clock source/64 * With ACLK = 1.5kHz and dividing it by 32768, we get ~21.8 seconds * With ACLK = 1.5kHz and dividing it by 64, we get 42.6mS * between WDT interrupts. */ WDTCTL = WDTPW + WDTHOLD; initClocks(); /* Configure Bluetooth module */ hc05_init(__baud_to_uca0br(9600)); hc05_transmit("HC05 init\r\n",11); /* Initialize mpu6050 */ mpu6050_init(); hc05_transmit("mpu6050 init\r\n",14); /* Now we are ready for application code to run. Enable interrupts */ _BIS_SR(GIE); while(1) { if(data_received != 0) { switch(data_received) { case 'T': data_received = 0; mpu6050_temp(); break; case 'A': data_received = 0; mpu6050_accel(); break; case 'G': data_received = 0; mpu6050_gyro(); break; case 'g': data_received = 0; mpu6050_calibrate_gyros(); break; case 'M': data_received = 0; mpu6050_getAddress(); break; case 'W': data_received = 0; mpu6050_wakeup(); break; case 'S': data_received = 0; mpu6050_sleep(); break; case 'R': data_received = 0; dmp_mode = 0; motion_detect_mode = 0; mpu6050_reset(); break; case 'd': data_received = 0; mpu6050_dmpinit(); break; case 'E': data_received = 0; motion_detect_mode = 0; dmp_mode = 1; mpu6050_setDMPEnabled(true); P2DIR &= ~MPU6050_INT; // Input P2SEL &= ~MPU6050_INT; // Digital IO Psel and psel2 are 0 P2SEL2 &= ~MPU6050_INT; P2IES &= ~MPU6050_INT; // Edge select 0 = low to high P2IFG &= ~MPU6050_INT; // Clear the interrupt flag before enabling interrupt P2IE |= MPU6050_INT; // Interrupt enable //mpu6050_resetFIFO(); break; case 'e': dmp_mode = 0; data_received = 0; mpu6050_setDMPEnabled(false); break; case 'm': /* Itseems that I can have motion detect interrupts if I first call the dmpinit() function, then this code is run. I can probably narrow it down to a certain function call in the dmpinit() it will just take time */ sendAck(); motion_detect_mode = 1; dmp_mode = 0; mpu6050_setDMPEnabled(false); i2c_write_reg(MPU6050_RA_INT_PIN_CFG,0x10);//interrupt status cleared on any read //i2c_write_reg(MPU6050_RA_MOT_DETECT_CTRL,0x30); // add the 3 ms delay to accel put mpu6050_setMotionDetectionThreshold(threshold);//not sure... but I'm told it's 2mg per LSB so 0xFF would only be about 0.512g mpu6050_setMotionDetectionDuration(threshold_duration); // This duration will really change the snappiness and responsiveness of the motion detect (duh) so // it should be set to as low as possible, then set detection threshold to the appropriate value for punches or whatever mpu6050_setIntEnabled(0x40);//motion detect... based on the product specification document, I don't think motion detect can generate an interrupt on INT pin, // so we also set the data ready interrupt. mpu6050_configAccel(MPU6050_ACCEL_FS_16<<(MPU6050_ACONFIG_AFS_SEL_BIT-1)); data_received = 0; P2DIR &= ~MPU6050_INT; // Input P2SEL &= ~MPU6050_INT; // Digital IO Psel and psel2 are 0 P2SEL2 &= ~MPU6050_INT; P2IES &= ~MPU6050_INT; // Edge select 0 = low to high P2IFG &= ~MPU6050_INT; // Clear the interrupt flag before enabling interrupt P2IE |= MPU6050_INT; // Interrupt enable /*while(1) { if(mpu6050_getIntStatus() & 0x40) { //motion interrupt hc05_transmit("Motion\r\n",8); } }*/ break; case 'l': threshold = threshold - 1; mpu6050_setMotionDetectionThreshold(threshold); break; case 'p': threshold = threshold + 1; mpu6050_setMotionDetectionThreshold(threshold); break; case 'L': threshold_duration = threshold_duration - 5; mpu6050_setMotionDetectionDuration(threshold_duration); break; case 'P': threshold_duration = threshold_duration + 5; mpu6050_setMotionDetectionDuration(threshold_duration); break; // case 'h': // hc05_setspeed(115200); // data_received = 0; // break; //case 'k': // hc05_key(); // data_received = 0; // break; default: sendAck(); data_received = 0; break; } } if(mpu6050_interrupt) { if(dmp_mode) { mpuIntStatus = mpu6050_getIntStatus(); fifoCount = mpu6050_getFIFOCount(); if(fifoCount > 16) { fifoCount =16; } mpu6050_getFIFOBytes(mpu6050_buffer,fifoCount); /* This seems to keep the fifo operating. I probably need to read the fifo faster so it doesn't 'die' on me */ mpu6050_resetFIFO(); /* From J.Rowberg's library, the dmp packet output is: bytes 0-15 quaternion (32 bits) (w,x,y,z) but just use the first two bytes as 16 bit number bytes 16-27 gyro (32 bits) (gx,gy,gz) but just use the first two bytes as 16 bit number bytes 28-39 acceleration (32 bits) (ax,ay,az) but just use the first two bytes as 16 bit number */ teapotPacket[2] = mpu6050_buffer[0]; teapotPacket[3] = mpu6050_buffer[1]; teapotPacket[4] = mpu6050_buffer[4]; teapotPacket[5] = mpu6050_buffer[5]; teapotPacket[6] = mpu6050_buffer[8]; teapotPacket[7] = mpu6050_buffer[9]; teapotPacket[8] = mpu6050_buffer[12]; teapotPacket[9] = mpu6050_buffer[13]; teapotPacket[10] = mpuIntStatus; // I modified the packet to sent the interrupt status in this byte hc05_transmit(teapotPacket,14); teapotPacket[11]++; mpu6050_interrupt = 0; } else if (motion_detect_mode) { if(mpu6050_getIntStatus() & 0x40) { // Disable motion interrupts, get accel and gyro values until they aren't interesting // anymore, then quit and enable interrupt. mpu6050_setIntEnabled(0x00); //motion interrupt // hc05_transmit("Motion\r\n",8); for(uint8_t j = 0; j<100; j++) { mpu6050_accel(); delay_ms(2); // i2c_tx_buffer[0] = 0x3B; // i2c_tx_buffer_counter = 1; // i2c_transmit_to_receive(); // i2c_transmit(); // i2c_multireceive(6); // for(j = 0; j< 6; j++) { // TXData = i2c_rx_buffer[j]; /* These are div 16384 if +/-2g, 8192 if +/-4g, 4096 if +/-8g and 2048 if +/-16g*/ //accelX[j] = (i2c_rx_buffer[0]<<8 | i2c_rx_buffer[1]); //accelY[j] = (i2c_rx_buffer[2]<<8 | i2c_rx_buffer[3]); //accelZ[j] = (i2c_rx_buffer[4]<<8 | i2c_rx_buffer[5]); //sprintf(tempbuf,"%d %d %d\r\n",ax,ay,az); //sprintf(tempbuf,"E%d,%d,%d\r\n",ax,ay,az); // hc05_transmit(tempbuf,strlen(tempbuf)); // hc05_transmit((char*)ax,1); //hc05_transmit((char*)ay,1); //hc05_transmit((char*)az,1); //} //__delay_us(1000); //delay_ms(2); } // hc05_transmit((char*)accelX,2*ACCELBUFSIZE); // hc05_transmit((char*)accelY,2*ACCELBUFSIZE); // hc05_transmit((char*)accelZ,2*ACCELBUFSIZE); // mpu6050_getIntStatus(); mpu6050_setIntEnabled(0x40); } else { // sprintf(tempbuf,"Unknown interrupt\r\n"); // hc05_transmit(tempbuf,strlen(tempbuf)); } } else { // mpu6050_getIntStatus(); // Clear any other interrupts // sprintf(tempbuf,"Unknown mode & interrupt\r\n"); // hc05_transmit(tempbuf,strlen(tempbuf)); } } __bis_SR_register(LPM0_bits + GIE); } }
void main() { OLED_Init(); //初始化oled qd=1; ftm_pwm_init(FTM0,FTM_CH3,10000,0); ftm_pwm_init(FTM0,FTM_CH4,10000,0); ftm_pwm_init(FTM2,FTM_CH0,10000,0); ftm_pwm_init(FTM2,FTM_CH1,10000,0); adc_init (ADC1_SE10); adc_init (ADC1_SE11); adc_init (ADC1_SE12); adc_init (ADC1_SE13); //按键初始化 gpio_init (PTA13, GPI,HIGH);//拨码开关初始化 gpio_init (PTA19, GPI,HIGH); gpio_init (PTA24, GPI,HIGH); gpio_init (PTA25, GPI,HIGH); gpio_init (PTA26, GPI,HIGH); gpio_init (PTA27, GPI,HIGH); gpio_init (PTA28, GPI,HIGH); gpio_init (PTA29, GPI,HIGH); led_init (LED0); mpu6050_init(); lptmr_delay_ms(1000); gyro_zero=ad_ave(100); gyro_zero1=ad_ave1(100); mpu6050_read(); accel_accel=(accel_x-accel_zero)/16384.0; if(accel_accel>1) accel_accel=1; if(accel_accel<-1) accel_accel=-1; angle_fuse=180/pi*(asin(accel_accel)); accel_accel1=(accel_y-accel_zero1)/16384.0; if(accel_accel1>1) accel_accel1=1; if(accel_accel1<-1) accel_accel1=-1; angle_fuse1=180/3.1415926*(asin(accel_accel1)); pit_init_ms(PIT0, 5); //初始化PIT0,定时时间为: 5ms set_vector_handler(PIT0_VECTORn ,PIT0_IRQHandler); //设置PIT0的中断服务函数为 PIT0_IRQHandler enable_irq (PIT0_IRQn); //使能PIT0中断 uart_init(UART3, 115200); while(aa<200); //初始化 1秒 DIP_switch(); while(1) { //display[0]=angle_fuse; //display[1]=angle_fuse1; display[0]=angle_fuse3; oledplay(); if(flag==1) mode1(); else if (flag==2) mode2(); else if (flag==3) mode3(); else if (flag==4) mode4(); vcan_sendware((unsigned char *)display, 20); } }
//main here int main(void) { uint8_t i = 0; char *wheel = "|/-\\"; //sending buffer addresses uint8_t sendpipe = 0; uint8_t addrtx0[NRF24L01_ADDRSIZE] = NRF24L01_ADDRP0; #if defined(TX) long *ptr = 0; double qw = 1.0f; double qx = 0.0f; double qy = 0.0f; double qz = 0.0f; double roll = 0.0f; double pitch = 0.0f; double yaw = 0.0f; //nrf24l01 variables uint8_t bufferout[NRF24L01_PAYLOAD]; #elif defined(RX) uint8_t bufferin[NRF24L01_PAYLOAD]; #endif //init uart uart_init(UART_BAUD_SELECT(UART_BAUD_RATE, F_CPU)); mpu6050_init(); nrf24l01_init(); sei(); uart_puts("\r\nUART initialized... \r\n"); _delay_ms(50); uart_puts("nrf24l01+ initialized... \r\n"); //init nrf24l01 _delay_ms(50); #if defined(TX) mpu6050_dmpInitialize(); mpu6050_dmpEnable(); _delay_ms(10); uart_puts("mpu6050 initialized... \r\n"); #endif //init interrupt uart_puts("interrupts enabled... \r\n"); for(i=0; i<100; i++) { uart_putc(wheel[i%4]); _delay_ms(10); uart_putc('\b'); } uart_puts("\r\n"); #if defined(TX) uart_puts("starting as tx...\r\n"); #elif defined(RX) uart_puts("starting as rx...\r\n"); #endif //setup buffer #if defined(TX) for(i=0; i<sizeof(bufferout); i++) bufferout[i] = ' '; #elif defined(RX) for(i=0; i<sizeof(bufferin); i++) bufferin[i] = 0; #endif #if NRF24L01_PRINTENABLE == 1 nrf24l01_printinfo(uart_puts, uart_putc); #endif //main loop nrf24l01_settxaddr(addrtx0); #if defined(TX) for(;;) { //tx char pipebuffer[5]; itoa(sendpipe, pipebuffer, 10); if(mpu6050_getQuaternionWait(&qw, &qx, &qy, &qz)) mpu6050_getRollPitchYaw(qw, qx, qy, qz, &roll, &pitch, &yaw); _delay_ms(10); //quaternion ptr = (long *)(&qw); for (i=0; i<4; i++) *(bufferout+i) = *ptr>>(i*8); ptr = (long *)(&qx); for (i=0; i<4; i++) *(bufferout+i+4) = *ptr>>(i*8); ptr = (long *)(&qy); for (i=0; i<4; i++) *(bufferout+i+8) = *ptr>>(i*8); ptr = (long *)(&qz); for (i=0; i<4; i++) *(bufferout+i+12) = *ptr>>(i*8); // roll pitch yaw ptr = (long *)(&roll); for (i=0; i<4; i++) *(bufferout+i+16) = *ptr>>(i*8); ptr = (long *)(&pitch); for (i=0; i<4; i++) *(bufferout+i+20) = *ptr>>(i*8); ptr = (long *)(&yaw); for (i=0; i<4; i++) *(bufferout+i+24) = *ptr>>(i*8); nrf24l01_write(bufferout); } #elif defined(RX) for(;;) { //rx uint8_t pipe = 0; if(nrf24l01_readready(&pipe)) { //if data is ready //read buffer nrf24l01_read(bufferin); for (i=0; i < 4*7; i++) uart_putc(*(bufferin+i)); uart_putc('\n'); } _delay_ms(10); } #endif }
int main(void) { #if MPU6050_GETATTITUDE == 0 int16_t ax = 0; int16_t ay = 0; int16_t az = 0; int16_t gx = 0; int16_t gy = 0; int16_t gz = 0; double axg = 0; double ayg = 0; double azg = 0; double gxds = 0; double gyds = 0; double gzds = 0; #endif #if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2 long *ptr = 0; double qw = 1.0f; double qx = 0.0f; double qy = 0.0f; double qz = 0.0f; double roll = 0.0f; double pitch = 0.0f; double yaw = 0.0f; #endif //init uart uart_init(UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU)); //init interrupt sei(); //init mpu6050 mpu6050_init(); _delay_ms(50); //init mpu6050 dmp processor #if MPU6050_GETATTITUDE == 2 mpu6050_dmpInitialize(); mpu6050_dmpEnable(); _delay_ms(10); #endif for(;;) { #if MPU6050_GETATTITUDE == 0 mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz); mpu6050_getConvData(&axg, &ayg, &azg, &gxds, &gyds, &gzds); #endif #if MPU6050_GETATTITUDE == 1 mpu6050_getQuaternion(&qw, &qx, &qy, &qz); mpu6050_getRollPitchYaw(&roll, &pitch, &yaw); _delay_ms(10); #endif #if MPU6050_GETATTITUDE == 2 if(mpu6050_getQuaternionWait(&qw, &qx, &qy, &qz)) { mpu6050_getRollPitchYaw(qw, qx, qy, qz, &roll, &pitch, &yaw); } _delay_ms(10); #endif #if MPU6050_GETATTITUDE == 0 char itmp[10]; ltoa(ax, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); ltoa(ay, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); ltoa(az, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); ltoa(gx, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); ltoa(gy, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); ltoa(gz, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' '); uart_puts("\r\n"); dtostrf(axg, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(ayg, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(azg, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gxds, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gyds, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); dtostrf(gzds, 3, 5, itmp); uart_puts(itmp); uart_putc(' '); uart_puts("\r\n"); uart_puts("\r\n"); _delay_ms(1000); #endif #if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2 //quaternion ptr = (long *)(&qw); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); ptr = (long *)(&qx); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); ptr = (long *)(&qy); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); ptr = (long *)(&qz); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); //roll pitch yaw ptr = (long *)(&roll); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); ptr = (long *)(&pitch); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); ptr = (long *)(&yaw); uart_putc(*ptr); uart_putc(*ptr>>8); uart_putc(*ptr>>16); uart_putc(*ptr>>24); uart_putc('\n'); #endif } }
int main(void) { bool init_status; int retValue; int i = defaultPerso.fall_detection_window; // initialize LEDs nrf_gpio_cfg_output(LED_RESETTING); nrf_gpio_cfg_output(LED_NO_FALL); nrf_gpio_cfg_output(LED_FALL_DETECTED); nrf_gpio_cfg_output(LED_OTHER); welcomeLEDs(); // Initialize timers_init(); ble_stack_init(); gap_params_init(); services_init(); advertising_init(); conn_params_init(); sec_params_init(); twi_master_init(); #ifdef TEST_ACTIVITY_LOG testActivityLog(); #endif initActivityLog(); initSnapshotBuffer(); initCoefficients(); // initialize values retValue = 0; init_status = mpu6050_init(MPU6050_DEVICE_ADDR); if ( false == init_status ) { init_status = mpu6050_init(MPU6050_DEVICE_ADDR+1); if ( false == init_status ) { retValue = -1; errorLEDs(); sd_nvic_SystemReset(); } } if ( 0 == retValue ) { init_status = false; i = 0; while ( false == init_status ) { if (i == 6) { errorLEDs(); sd_nvic_SystemReset(); } init_status = accel_setup(); i++; } } if (0 == retValue) { char outbuf[20]; // Start execution - write a log entry with zero steps to indicate startup // writeLogEntry(999); advertising_start(); setup_wdt(); timers_start(); // start sampliing while(1) { if (ReadSnapshots == 1) { FullSnapshotEntry* entry = (FullSnapshotEntry*)(SNAPSHOT_DATA_ADDRESS_START) ; SnapshotHeader *h; Sensor_Reading *r; sprintf (outbuf, "ID: %s ", getPerso()->uname); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); for(int i=0;i < getActivityLogSize(); i++) { h = &entry->hdr; sprintf (outbuf, ": SNAPSHOT # %d \n ",(i)); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 16); sprintf (outbuf, "Version: 2 \n");// ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 12); sprintf (outbuf, "t:%d n:%d last:%d",h->data.time, h->data.num_of_data_points, h->data.latest_data_point_index); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); // ptr++; r = entry->r; for (int j = 0; j < NUMBER_OF_ENTRIES; j++) { #define FLASH_READ_DELAY 40 nrf_gpio_pin_set(LED_RESETTING); sprintf (outbuf, "i: %d T: %04x ",j,r[j].val.temp); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); nrf_delay_ms(FLASH_READ_DELAY); sprintf (outbuf, "A: x%04xy%04xz%04x",r[j].val.x_ac,r[j].val.y_ac,r[j].val.z_ac); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); nrf_delay_ms(FLASH_READ_DELAY); sprintf (outbuf, "G: x%04xy%04xz%04x",r[j].val.x_gy,r[j].val.y_gy,r[j].val.z_gy); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); nrf_delay_ms(FLASH_READ_DELAY); nrf_gpio_pin_clear(LED_RESETTING); nrf_delay_ms(FLASH_READ_DELAY); //ptr += sizeof(Sensor_Reading); } entry++; } ReadSnapshots = 0; } if (ReadSnapshotsPartial == 1) { PartialSnapTotal = ((PartialSnap1-('0'))*10)+(PartialSnap2-('0')); FullSnapshotEntry* entry = (FullSnapshotEntry*)(SNAPSHOT_DATA_ADDRESS_START+(((PartialSnapTotal*sizeof(FullSnapshotEntry)*10))/4)) ; SnapshotHeader *h; Sensor_Reading *r; sprintf (outbuf, "ID: %s ", getPerso()->uname); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); for(int i=0;i < 10; i++) { h = &entry->hdr; sprintf (outbuf, ": SNAPSHOT # %d \n ",(i+(PartialSnapTotal*10))); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 16); sprintf (outbuf, "Version: 2 \n");// ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 12); sprintf (outbuf, "t:%d n:%d last:%d",h->data.time, h->data.num_of_data_points, h->data.latest_data_point_index); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); // ptr++; r = entry->r; for (int j = 0; j < NUMBER_OF_ENTRIES; j++) { #define FLASH_READ_DELAY 40 nrf_gpio_pin_set(LED_RESETTING); sprintf (outbuf, "i: %d T: %04x ",j,r[j].val.temp); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); nrf_delay_ms(FLASH_READ_DELAY); sprintf (outbuf, "A: x%04xy%04xz%04x",r[j].val.x_ac,r[j].val.y_ac,r[j].val.z_ac); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); nrf_delay_ms(FLASH_READ_DELAY); sprintf (outbuf, "G: x%04xy%04xz%04x",r[j].val.x_gy,r[j].val.y_gy,r[j].val.z_gy); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); nrf_delay_ms(FLASH_READ_DELAY); nrf_gpio_pin_clear(LED_RESETTING); nrf_delay_ms(FLASH_READ_DELAY); //ptr += sizeof(Sensor_Reading); } entry++; } ReadSnapshotsPartial = 0; } if (ReadDataBuffer == 1) { LogEntry *addr; sprintf (outbuf, "ID: %s ", getPerso()->uname); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); addr = (LogEntry *)(STEP_DATA_ADDRESS_START); for(int i=0;i < getActivityLogSize(); i++) { nrf_gpio_pin_set(LED_RESETTING); nrf_delay_ms(20); nrf_gpio_pin_clear(LED_RESETTING); nrf_delay_ms(20); // sprintf ( outbuf, ": h:%d s:%d a:%04f ",addr->item.hour, addr->item.sec, addr->item.activity_level); sprintf ( outbuf, "%d:%d - %03f ",addr->item.hour, (addr->item.sec)/60, addr->item.activity_level); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); //nrf_delay_ms(1); addr++; } ReadDataBuffer = 0; } if (ReadDataBufferPartial == 1) { LogEntry *addr; sprintf (outbuf, "ID: %s ", getPerso()->uname); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); PartialCountTotal = ((PartialCount1-('0'))*10)+(PartialCount2-('0')); addr = (LogEntry *)(STEP_DATA_ADDRESS_START+(2*(PartialCountTotal*1000))); for(int i=0;i < 1000; i++) { nrf_gpio_pin_set(LED_RESETTING); nrf_delay_ms(20); nrf_gpio_pin_clear(LED_RESETTING); nrf_delay_ms(20); // sprintf ( outbuf, ": h:%d s:%d a:%04f ",addr->item.hour, addr->item.sec, addr->item.activity_level); sprintf ( outbuf, "%d:%d - %03f ",addr->item.hour, (addr->item.sec)/60, addr->item.activity_level); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); //nrf_delay_ms(1); addr++; } ReadDataBufferPartial = 0; } if (Dumper == 1) { sprintf (outbuf, "Log: %d ",getActivityLogSize()); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); Dumper = 0; } if (ReadBigSnapshot == 1) { SnapshotHeader *h = getSnapshotHeader(); sprintf (outbuf, "t:%d n:%d last:%d",h->data.time, h->data.num_of_data_points, h->data.latest_data_point_index); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); for (i = 0; i < NUMBER_OF_ENTRIES; i++) //i <= h->data.latest_data_point_index; i++) { #define RAM_READ_DELAY 15 sprintf (outbuf, "i: %d T: %04x ",i,raw_data[i].val.temp); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); nrf_delay_ms(RAM_READ_DELAY); sprintf (outbuf, "A: x%04xy%04xz%04x",raw_data[i].val.x_ac,raw_data[i].val.y_ac,raw_data[i].val.z_ac); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); nrf_delay_ms(RAM_READ_DELAY); sprintf (outbuf, "G: x%04xy%04xz%04x",raw_data[i].val.x_gy,raw_data[i].val.y_gy,raw_data[i].val.z_gy); ble_nus_send_string(&m_nus, (unsigned char*)outbuf, 20); nrf_delay_ms(RAM_READ_DELAY); } ReadBigSnapshot = 0; } if (do_post_processing == true) { do_post_processing = false; if (emergencyCall == 1) { makeTheCall(); emergencyCall = 0; } post_comm_processing(); } // Power Method 1 // nrf_gpio_pin_write(LED_OTHER,1); // power_manage(); // nrf_gpio_pin_write(LED_OTHER,0); } } return retValue; }
int main(void) { double axg = 0, ayg = 0, azg = 0; double gxds = 0, gyds = 0, gzds = 0; //init mpu6050 mpu6050_init(); _delay_ms(50); LCD_init(); LCD_clear(); put_food(); // INtro while (1) { // Read current parameters mpu6050_getConvData(&axg, &ayg, &azg, &gxds, &gyds, &gzds); LCD_write_string(0,0," Little Snake "); LCD_write_string(0,3," >> Move << "); if (axg > LIMIT || axg < -LIMIT ||ayg < -LIMIT || ayg > LIMIT) break; } // Init snake, both on screen and array screen[3][5] = SNAKE; screen[3][6] = SNAKE; screen[3][7] = SNAKE; snake[0].x = 3; snake[0].y = 5; snake[1].x = 3; snake[1].y = 6; snake[2].x = 3; snake[2].y = 7; snake_tail = 0; snake_len = 3; for (int z=snake_len; z<MAX_LEN; z++) snake[z].x = -1; while(game) { // Read current parameters mpu6050_getConvData(&axg, &ayg, &azg, &gxds, &gyds, &gzds); // Take a decision if(axg > LIMIT) {if (direction != UP) direction = DOWN;} else if(axg < -LIMIT) {if (direction != DOWN) direction = UP;} if(ayg > LIMIT) {if (direction != LEFT) direction = RIGHT;} else if(ayg < -LIMIT) {if (direction != RIGHT) direction = LEFT;} // Make it happen change_dir(); // Check end game if (!game) break; LCD_init(); LCD_clear(); // Print entire screen for (int x=0; x<HEIGHT; x++) { for (int y=0; y<WIDTH; y++) { if (x == snake[snake_len-1].x && y == snake[snake_len-1].y) { LCD_write_char(HEAD); } else if (screen[x][y] == SNAKE) { LCD_write_char(SNAKE); } else if (screen[x][y] == FOOD) { LCD_write_char(FOOD); } else { LCD_write_char(SPACE); } } } _delay_ms(250); } }
/*--------------------------------------------------------------------------*/ int main(int argc, char **argv) { /* * Initalize hardware. */ msp430_cpu_init(); clock_init(); uart_init(9600); /* Must come before first printf */ /* xmem_init(); */ PRINTF("iWatch 0.10 build at " __TIME__ " " __DATE__ "\n"); UCSCTL8 &= ~BIT2; /* * Hardware initialization done! */ /* * Initialize Contiki and our processes. */ process_init(); process_start(&etimer_process, NULL); rtimer_init(); ctimer_init(); energest_init(); ENERGEST_ON(ENERGEST_TYPE_CPU); backlight_init(); battery_init(); SPI_FLASH_Init(); if (system_testing()) { clock_time_t t; backlight_on(200, 0); t = clock_seconds(); // sleep 1 while(clock_seconds() - t <= 3); printf("$$OK BACKLIGHT\n"); t = clock_seconds(); while(clock_seconds() - t <= 3); backlight_on(0, 0); motor_on(200, 0); // sleep 1s t = clock_seconds(); while(clock_seconds() - t <= 3); printf("$$OK MOTOR\n"); t = clock_seconds(); while(clock_seconds() - t <= 3); motor_on(0, 0); #if PRODUCT_W001 I2C_Init(); codec_init(); codec_bypass(1); // sleep 1s t = clock_seconds(); while(clock_seconds() - t <= 3); printf("$$OK MIC\n"); // sleep 1s t = clock_seconds(); while(clock_seconds() - t <= 3); codec_bypass(0); codec_shutdown(); #endif } int reason = CheckUpgrade(); window_init(reason); button_init(); rtc_init(); CFSFontWrapperLoad(); system_init(); // check system status and do factor reset if needed I2C_Init(); //codec_init(); //ant_init(); bluetooth_init(); #ifdef PRODUCT_W004 //bmx_init(); #else mpu6050_init(); #endif // check the button status if (button_snapshot() & (1 << BUTTON_UP)) { clock_time_t t; // delay 1 second // button up is pressed, we will set emerging flag motor_on(200, CLOCK_SECOND * 2); t = clock_seconds(); while(clock_seconds() - t <= 1); if (button_snapshot() & (1 << BUTTON_UP)) system_setemerging(); motor_on(0, 0); } if (!system_retail()) { bluetooth_discoverable(1); } #if PRODUCT_W001 if (system_testing()) ant_init(MODE_HRM); #endif system_restore(); // protocol_init(); // protocol_start(1); process_start(&system_process, NULL); /* * This is the scheduler loop. */ msp430_dco_required = 0; /* check firmware update */ if (reason == 0xff) { printf("Start Upgrade\n"); Upgrade(); // never return if sucessfully upgrade } watchdog_start(); while(1) { int r; do { /* Reset watchdog. */ watchdog_periodic(); r = process_run(); } while(r > 0); /* * Idle processing. */ int s = splhigh(); /* Disable interrupts. */ /* uart1_active is for avoiding LPM3 when still sending or receiving */ if(process_nevents() != 0) { splx(s); /* Re-enable interrupts. */ } else { static unsigned long irq_energest = 0; /* Re-enable interrupts and go to sleep atomically. */ ENERGEST_OFF(ENERGEST_TYPE_CPU); ENERGEST_ON(ENERGEST_TYPE_LPM); /* We only want to measure the processing done in IRQs when we are asleep, so we discard the processing time done when we were awake. */ energest_type_set(ENERGEST_TYPE_IRQ, irq_energest); watchdog_stop(); if (shutdown_mode) { system_shutdown(1); // never return LPM4; } if (msp430_dco_required) { __low_power_mode_0(); } else { __low_power_mode_3(); } /* We get the current processing time for interrupts that was done during the LPM and store it for next time around. */ __disable_interrupt(); irq_energest = energest_type_time(ENERGEST_TYPE_IRQ); __enable_interrupt(); watchdog_start(); ENERGEST_OFF(ENERGEST_TYPE_LPM); ENERGEST_ON(ENERGEST_TYPE_CPU); } } }