int _tmain(int argc, _TCHAR* argv[])
{
NavWPHandler navHandler;
strncpy(navHandler.fileManager.cfgFileName, "sometext", strlen("sometext"));
somefile = NAV_fopen(filename, "wb");
size_t totalSize = 0;
NavFileHeader fileHeader;
NavFileWPListHeader WPListHeader;
fileHeader.fileType = WAYPOINT_LIST_FILE;
fileHeader.fileVersion = (NavVersion)NAV_VERSION_1;
fileHeader.nextHeaderSize = sizeof(WPListHeader);
std::cout << "sizeof(WPListHeader): " << sizeof(WPListHeader) << std::endl;
Coordinate coordArray[1];
for (size_t i = 0; i < (sizeof(coordArray) / sizeof(coordArray[0])); i++)
{
initCoordinate(&coordArray[i]);
// coordArray[i].dLatitude = 0xFFFF;
coordArray[i].dLatitude = 1;
coordArray[i].dLongitude = 2 * i;
coordArray[i].mLatitude = 0;
coordArray[i].mLongitude = 0;
coordArray[i].priority = 20 * i;
}
WPListHeader.startCoordinate = coordArray[0];
WPListHeader.endCoordinate = coordArray[0];
WPListHeader.numberOfEntries = (sizeof(coordArray) / sizeof(coordArray[0]));
WPListHeader.nextHeaderSize = sizeof(coordArray[0]);
somefile = fopen(filename, "wb");
fwrite(&fileHeader, sizeof(fileHeader), 1, somefile);
totalSize += sizeof(fileHeader);
std::cout << "totalSize after fileHeader: " << totalSize << std::endl;
fwrite(&WPListHeader, sizeof(WPListHeader), 1, somefile);
totalSize += sizeof(WPListHeader);
std::cout << "totalSize after NavFileWPListHeader: " << totalSize
<< std::endl;
for (size_t i = 0; i < (sizeof(coordArray) / sizeof(coordArray[0])); i++)
{
fwrite(&coordArray[i], sizeof(Coordinate), 1, somefile);
std::cout << "Wrote:" << std::endl;
printCoordData(&coordArray[i]);
totalSize += sizeof(Coordinate);
}
std::cout << "Expected file size: " << totalSize << std::endl;
fclose(somefile);
std::cout << "sizeof(fileheader.fileVersion): "
<< sizeof(fileHeader.fileVersion) << std::endl;
return 0;
}
static int gConfigure(int gFile)
{
CHECK(ITG3200_Init(gFile, ITG3200_ID, true));
struct ITG3200_Acquisition confAcquisition = {
.lowPassFilter = ITG3200_LOWPASSFILTER_42,
.sampleRateDivider = 0,
};
CHECK(ITG3200_ConfigureAcquisition(gFile, &confAcquisition));
struct ITG3200_Power confPower = {
.clockSource = ITG3200_CLOCKSOURCE_PLL_X,
};
CHECK(ITG3200_ConfigurePower(gFile, &confPower));
return 0;
}
int main(void)
{
int adapter;
char filename[20];
for (adapter = 0; adapter < MAX_ADAPTERS; ++adapter)
{
snprintf(filename, 20, "/dev/i2c-%d", adapter);
if (!access(filename, R_OK | W_OK))
break;
}
if (adapter == MAX_ADAPTERS)
{
fprintf(stderr, "No I2C adapter found\n");
return -1;
}
aFile = open(filename, O_RDWR);
if (aConfigure(aFile))
{
fprintf(stderr, "Failed to initialize accelerometer\n");
return closeAndExit(-1);
}
cFile = open(filename, O_RDWR);
if (cConfigure(cFile))
{
fprintf(stderr, "Failed to initialize compass\n");
return closeAndExit(-1);
}
gFile = open(filename, O_RDWR);
if (gConfigure(gFile))
{
fprintf(stderr, "Failed to initialize gyroscope\n");
return closeAndExit(-1);
}
ADXL345_ReadData(aFile, &aX.data, &aY.data, &aZ.data);
HMC5883L_ReadData(cFile, &cX.data, &cY.data, &cZ.data);
ITG3200_ReadData(gFile, &gX.data, &gY.data, &gZ.data);
ITG3200_ReadTemperature(gFile, &gT.data);
initCoordinate(&aX);
initCoordinate(&aY);
initCoordinate(&aZ);
initCoordinate(&cX);
initCoordinate(&cY);
initCoordinate(&cZ);
initCoordinate(&gX);
initCoordinate(&gY);
initCoordinate(&gZ);
initCoordinate(&gT);
setupHandlers();
initscr();
noecho();
curs_set(false);
running = 1;
while (running)
{
ADXL345_ReadData(aFile, &aX.data, &aY.data, &aZ.data);
HMC5883L_ReadData(cFile, &cX.data, &cY.data, &cZ.data);
ITG3200_ReadData(gFile, &gX.data, &gY.data, &gZ.data);
ITG3200_ReadTemperature(gFile, &gT.data);
updateCoordinate(&aX);
updateCoordinate(&aY);
updateCoordinate(&aZ);
updateCoordinate(&cX);
updateCoordinate(&cY);
updateCoordinate(&cZ);
updateCoordinate(&gX);
updateCoordinate(&gY);
updateCoordinate(&gZ);
updateCoordinate(&gT);
clear();
mvprintw(0, 0, "%16s: %8d X %8d Y %8d Z\n", "Accelerometer", aX.data, aY.data, aZ.data);
mvprintw(1, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", aX.smoothed, aY.smoothed, aZ.smoothed);
mvprintw(2, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", aX.min, aY.min, aZ.min);
mvprintw(3, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", aX.max, aY.max, aZ.max);
mvprintw(4, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", aX.normalized, aY.normalized, aZ.normalized);
mvprintw(6, 0, "%16s: %8d X %8d Y %8d Z\n", "Compass", cX.data, cY.data, cZ.data);
mvprintw(7, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", cX.smoothed, cY.smoothed, cZ.smoothed);
mvprintw(8, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", cX.min, cY.min, cZ.min);
mvprintw(9, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", cX.max, cY.max, cZ.max);
mvprintw(10, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", cX.normalized, cY.normalized, cZ.normalized);
mvprintw(12, 0, "%16s: %8d X %8d Y %8d Z\n", "Gyroscope", gX.data, gY.data, gZ.data);
mvprintw(13, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", gX.smoothed, gY.smoothed, gZ.smoothed);
mvprintw(14, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", gX.min, gY.min, gZ.min);
mvprintw(15, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", gX.max, gY.max, gZ.max);
mvprintw(16, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", gX.normalized, gY.normalized, gZ.normalized);
mvprintw(18, 0, "%16s: %8d %8.1f °C\n", "Temperature", gT.data, ITG3200_ConvertTemperature(gT.data));
mvprintw(19, 0, "%16s: %8.1f %8.1f °C\n", "Smoothed", gT.smoothed, ITG3200_ConvertTemperature(gT.smoothed));
mvprintw(20, 0, "%16s: %8.1f %8.1f °C\n", "Min", gT.min, ITG3200_ConvertTemperature(gT.min));
mvprintw(21, 0, "%16s: %8.1f %8.1f °C\n", "Max", gT.max, ITG3200_ConvertTemperature(gT.max));
mvprintw(22, 0, "%16s: %8.1f\n", "Normalized", gT.normalized);
refresh();
usleep(DELAY);
}
endwin();
close(aFile);
close(cFile);
close(gFile);
return 0;
}
