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