/*******************************************************************************
* 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;
}
}
}
