int main()
{
int rc;
struct nav_struct nav;
printf("Nav board test program\r\n");
//nav board
printf("Init Navboard ...\r\n");
rc = nav_Init(&nav);
if(rc) return rc;
printf("Init Navboard OK\r\n");
//calibrate
printf("Calibration ...\r\n");
rc=nav_FlatTrim();
if(rc) {printf("FlatTrim Failed: retcode=%d\r\n",rc); return rc;}
printf("Calibration OK\r\n");
//main loop
while(1) {
//get sample
rc = nav_GetSample(&nav);
if(rc) {
printf("ERROR: nav_GetSample return code=%d\n",rc);
}
nav_Print(&nav);
}
nav_Close();
printf("\nDone...\n");
return 0;
}
// Use navigation data to set motors
void checkNavdata (nav_struct * nav)
{
// Get navdata sample
int rc = nav_GetSample(nav);
if(rc) {
printf("ERROR: nav_GetSample return code=%d\n",rc);
}
// Print Navdata
nav_Print(nav);
// Shut off motors if z-axis accelerometer is outside thresholds
if (nav->az >= 0.3 && ignore_navdata==0) {
throttle1 = 0.01;
throttle2 = 0.01;
throttle3 = 0.01;
throttle4 = 0.01;
mot_Run(throttle1,throttle2,throttle3,throttle4);
}
}
