int main(int argc, char **argv)
{
printf("%s------------------------------------------------------------%s\n", ANSI_COLOUR_MAGENTA_BOLD, ANSI_COLOUR_RESET);
printf("%s-----------------------eGlove Project-----------------------%s\n", ANSI_COLOUR_MAGENTA_BOLD, ANSI_COLOUR_RESET);
printf("%s------------------------------------------------------------%s\n", ANSI_COLOUR_MAGENTA_BOLD, ANSI_COLOUR_RESET);
sleep(1); //1s delay
mraa_init();
MPU9250_GPIO_Init();
MPU9250_i2c = mraa_i2c_init(1);
signal(SIGINT, &sig_handler);
usleep(1000); //1ms delay
MPU9250_I2C_Init();
sleep(1); //1s delay
MPU9250_I2C_Config(SEN_COUNT);
sleep(1); //1s delay
printf("%s------------------------------------------------------------%s\n", ANSI_COLOUR_WHITE_BOLD, ANSI_COLOUR_RESET);
#ifdef GESTURE_SCAN
//Initialize Database
MPU9250_Alpha_Struct();
#endif
//Store First Time Instance
Timer = clock();
while(isrunning)
{
for(i = 0; i < SEN_COUNT; i++)
{
MPU9250_Loop();
#ifdef RAW_ANGLES
//Print Raw Angles
printf("%s[ SEN%d ] Raw Angles: Raw_X:%6.2lf Raw_Y:%6.2lf %s\n", Ansi_Colour_Bold, i, r[i].Raw_X, r[i].Raw_Y, ANSI_COLOUR_RESET);
#endif
#ifdef COMPLEMENTARY_ANGLES
//Print Complementary Filter Angles
printf("%s[ SEN%d ] CF Angles: C_X:%6.2lf C_Y:%6.2lf %s\n", Ansi_Colour_Bold, i, c[i].C_X, c[i].C_Y, ANSI_COLOUR_RESET);
#endif
s[i].Roll = c[i].C_X;
s[i].Pitch = c[i].C_Y;
#ifdef EULER_ANGLES
//Print Euler Angles
printf("\r%s[ SEN%d ] Euler Angles: Roll:%6.2lf Pitch:%6.2lf %s", Ansi_Colour_Bold, i, s[i].Roll, s[i].Pitch, ANSI_COLOUR_RESET);
#endif
// printf("%s------------------------------------------------------------%s\n", ANSI_COLOUR_WHITE_BOLD, ANSI_COLOUR_RESET);
dt = (double)(clock() - Timer) / CLOCKS_PER_SEC;
#ifdef LOOP_TIME
//Print Loop-time
printf("\rTime elapsed in s: %f", dt);
#endif
Timer = clock();
// printf("%s------------------------------------------------------------%s\n", ANSI_COLOUR_WHITE_BOLD, ANSI_COLOUR_RESET);
}
#ifdef GESTURE_SCAN
MPU9250_Gesture_Scan();
#endif
}
return MRAA_SUCCESS;
}
void initIMU(void) {
float selfTest9250[6];
/* Initialize motors */
Motor_Init();
trace_printf("Motor initialized.\n");
/* Initialize related variables */
GyroMeasError = _PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
GyroMeasDrift = _PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
/* Initialize balancing */
bal_pitch = 0;
bal_roll = 0;
bal_yaw = 0;
/* Initialize PID */
pitchReg.SetMode(AUTOMATIC);
pitchReg.SetOutputLimits(-PID_PITCH_INFLUENCE, PID_PITCH_INFLUENCE);
rollReg.SetMode(AUTOMATIC);
rollReg.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);
yawReg.SetMode(AUTOMATIC);
yawReg.SetOutputLimits(-PID_YAW_INFLUENCE, PID_YAW_INFLUENCE);
trace_printf("PID initialized.\n");
/* Detect whether MPU9250 is online */
MPU9250_I2C_Init();
resetMPU9250();
if(MPU9250_I2C_ByteRead(MPU9250_ADDRESS, WHO_AM_I_MPU9250) != 0x71) {
trace_printf("Could not find MPU9250!\n");
}
trace_printf("MPU9250 is online.\n");
timer_sleep(100);
/* MPU9250 self test and calibrate */
MPU9250SelfTest(selfTest9250);
trace_printf("x-axis self test: acceleration trim within : %6f %% of factory value\n", selfTest9250[0]);
trace_printf("y-axis self test: acceleration trim within : %f %% of factory value\n", selfTest9250[1]);
trace_printf("z-axis self test: acceleration trim within : %f %% of factory value\n", selfTest9250[2]);
trace_printf("x-axis self test: gyration trim within : %f %% of factory value\n", selfTest9250[3]);
trace_printf("y-axis self test: gyration trim within : %f %% of factory value\n", selfTest9250[4]);
trace_printf("z-axis self test: gyration trim within : %f %% of factory value\n", selfTest9250[5]);
timer_sleep(100);
calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
trace_printf("x gyro bias = %f\n", gyroBias[0]);
trace_printf("y gyro bias = %f\n", gyroBias[1]);
trace_printf("z gyro bias = %f\n", gyroBias[2]);
trace_printf("x accel bias = %f\n", accelBias[0]);
trace_printf("y accel bias = %f\n", accelBias[1]);
trace_printf("z accel bias = %f\n", accelBias[2]);
timer_sleep(100);
/* Initialize MPU9250 and AK8963 */
initMPU9250();
trace_printf("MPU9250 initialized for active data mode....\n"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
timer_sleep(500);
initAK8963(magCalibration);
trace_printf("AK8963 initialized for active data mode....\n"); // Initialize device for active mode read of magnetometer
trace_printf("Accelerometer full-scale range = %f g\n", 2.0f*(float)(1<<Ascale));
trace_printf("Gyroscope full-scale range = %f deg/s\n", 250.0f*(float)(1<<Gscale));
if(Mscale == 0) trace_printf("Magnetometer resolution = 14 bits\n");
if(Mscale == 1) trace_printf("Magnetometer resolution = 16 bits\n");
if(Mmode == 2) trace_printf("Magnetometer ODR = 8 Hz\n");
if(Mmode == 6) trace_printf("Magnetometer ODR = 100 Hz\n");
timer_sleep(100);
getAres(); // Get accelerometer sensitivity
getGres(); // Get gyro sensitivity
getMres(); // Get magnetometer sensitivity
trace_printf("Accelerometer sensitivity is %f LSB/g \n", 1.0f/aRes);
trace_printf("Gyroscope sensitivity is %f LSB/deg/s \n", 1.0f/gRes);
trace_printf("Magnetometer sensitivity is %f LSB/G \n", 1.0f/mRes);
magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
magbias[1] = +120.; // User environmental x-axis correction in milliGauss
magbias[2] = +125.; // User environmental x-axis correction in milliGauss
}
