/* * The self test data transmission (MAKE_TWI_TRAFFIC_FOR_SELF_TEST). * Communicate normally on the TWI interface. * The necessary wires from TWI to the sniffer porst must be connected to self-test. */ void noiseTask() { static uint32_t makeNoise = 0; static uint8_t noise = 0; if (makeNoise++ > 500000UL) { makeNoise = 0; MPU6050_init(noise++); if (noise == 1) noise = 0; } }
int main (void){ float acc_x, acc_z, gyro_x; float acc_angle,kal_angle; /* Init Systick to 1ms */ SysTick_Config( SystemCoreClock / 1000); /* Initialize GPIO (sets up clock) */ LPC_SYSCON->SYSAHBCLKCTRL |= (1<<6); SERVO_init(); if(I2CInit(I2CMASTER) == FALSE){ while(1); /* fatal error */ } if(MPU6050_init()){ return 0; } MPU6050_setZero(); kalman_init(); for(;;){ /* 100Hz loop */ if(gSysTick_10 >= 9){ gSysTick_10 = 0; /* get sensor values */ gyro_x = MPU6050_getGyroRoll_degree(); acc_x = MPU6050_getAccel_x(); acc_z = -MPU6050_getAccel_z(); /* acc angle */ acc_angle = atan2(acc_x , -acc_z) * 180/3.14159 ; // calculate accel angle kal_angle = kalman_update(acc_angle,gyro_x, 0.01); SERVO_set_slew((-kal_angle) - MECH_OFFSET); } } }