/******************************************************************************* * FUNCTION NAME: Process_Data_From_Master_uP * PURPOSE: Executes every 26.2ms when it gets new data from the master * microprocessor. * CALLED FROM: main.c * ARGUMENTS: none * RETURNS: void *******************************************************************************/ void Process_Data_From_Master_uP(void) { static unsigned int j = 0; Getdata(&rxdata); Camera_Handler(); Servo_Track(); //PAN_SERVO = 127; //TILT_SERVO = 127; Default_Routine(); j++; j++; if(j == 1) { printf("\rCalculating Gyro Bias..."); } if(j == 6) { Start_Gyro_Bias_Calc(); } if(j == 200) { Stop_Gyro_Bias_Calc(); Reset_Gyro_Angle(); printf("Done\r"); } Putdata(&txdata); }
void Calculate_Gyro_Bias(void) { static unsigned int i = 0; static unsigned int j = 0; int temp_gyro_rate; long temp_gyro_angle; int temp_gyro_bias; static char done=0; if(done==0) { i++; j++; // this will rollover every ~1000 seconds if(j == 10) { printf("\rCalculating Gyro Bias..."); } if(j == 60) { // start a gyro bias calculation Start_Gyro_Bias_Calc(); } if(j == 300) { // terminate the gyro bias calculation Stop_Gyro_Bias_Calc(); // reset the gyro heading angle Reset_Gyro_Angle(); printf("Done\r"); } if(i >= 30 && j >= 300) { temp_gyro_bias = Get_Gyro_Bias(); temp_gyro_rate = Get_Gyro_Rate(); temp_gyro_angle = Get_Gyro_Angle(); printf(" Gyro Bias=%d\r\n", temp_gyro_bias); printf(" Gyro Rate=%d\r\n", temp_gyro_rate); printf("Gyro Angle=%d\r\n\r\n", (int)temp_gyro_angle); i = 0; done = 1; } } }