/**************************************************************************
* Public Function Prototypes
**************************************************************************/
void InitIMU(void)
{
VN_ERROR_CODE errorCode;
errorCode = vn200_connect(&vn200, COM_PORT, VN_BAUDRATE);
/* Make sure the user has permission to use the COM port. */
if (errorCode == VNERR_PERMISSION_DENIED) {
printf("Current user does not have permission to open the COM port.\n");
printf("Try running again using 'sudo'.\n");
} else if (errorCode != VNERR_NO_ERROR) {
printf("%d Error encountered when trying to connect to the sensor.\n",errorCode);
}
printf("%d\n",errorCode);
}
int main()
{
Vn200 vn200;
vn200_connect(&vn200, COM_PORT, BAUD_RATE);
vn200_registerAsyncDataReceivedListener(&vn200, &asyncDataListener);
sleep(10);
vn200_unregisterAsyncDataReceivedListener(&vn200, &asyncDataListener);
vn200_disconnect(&vn200);
return 0;
}
int initAsyncSensors(Vn200* vn200)
{
VN_ERROR_CODE errorCode;
errorCode = vn200_connect(vn200,COM_PORT,BAUD_RATE);
if (errorCode == VNERR_PERMISSION_DENIED) {
printf("Current user does not have permission to open the COM port\n");
printf("Try running again using 'sudo'.\n");
return -1;
}
else if (errorCode != VNERR_NO_ERROR) {
printf("Error encountered when trying to connect to the sensor\n");
return -1;
}
errorCode = vn200_setAsynchronousDataOutputType(vn200,VNASYNC_VNINS,true);
sleep(1);
return 1;
}
void InitIMU(void)
{ struct timespec start, end;double interval_sec = (double)1/20;
VN_ERROR_CODE errorCode;
clock_gettime(CLOCK_REALTIME, &start);
errorCode = vn200_connect(&vn200, COM_PORT, VN_BAUDRATE);
clock_gettime(CLOCK_REALTIME, &end);
uint64_t tmpdiff;
double remain_us;
tmpdiff = get_elapsed(&start, &end);
remain_us = (interval_sec * 1000000 - tmpdiff / 1000);
printf("\nVectornav initialization took %llu us\n", tmpdiff/1000);
/* Make sure the user has permission to use the COM port. */
if (errorCode == VNERR_PERMISSION_DENIED) {
printf("Current user does not have permission to open the COM port.\n");
printf("Try running again using 'sudo'.\n");
} else if (errorCode != VNERR_NO_ERROR) {
printf("%d Error encountered when trying to connect to the sensor.\n",errorCode);
}
printf("%d\n",errorCode);
}
int main()
{
// double gpsTime;
// unsigned short gpsWeek, status;
// VnVector3 ypr, latitudeLognitudeAltitude, nedVelocity;
// float attitudeUncertainty, positionUncertainty, velocityUncertainty;
VnVector3 magnetic, acceleration, angularRate;
float temperature, pressure;
float mx, my, mz, ax, ay, az, gx, gy, gz;
Vn200 vn200;
// Vn100 vn100;
int i = 0;
vn200_connect(&vn200, COM_PORT, BAUD_RATE);
while(1) {
vn200_getCalibratedSensorMeasurements(
&vn200,
&magnetic,
&acceleration,
&angularRate,
&temperature,
&pressure);
mx = (float) magnetic.c0;
my = (float) magnetic.c1;
mz = (float) magnetic.c2;
ax = (float) acceleration.c0;
ay = (float) acceleration.c1;
az = (float) acceleration.c2;
gx = (float) angularRate.c0;
gy = (float) angularRate.c1;
gz = (float) angularRate.c2;
MadgwickAHRSupdate(gx, gy, gz, ax, ay, az, mx, my, mz);
// printf("IMU Solution: \n"
// "i: %d\n"
// "magnetic.x: %+#7.2f\n"
// "magnetic.y: %+#7.2f\n"
// "magnetic.z: %+#7.2f\n"
// "acceleration.x: %+#7.2f\n"
// "acceleration.y: %+#7.2f\n"
// "acceleration.z: %+#7.2f\n"
// "angularRate.x: %+#7.2f\n"
// "angularRate.y: %+#7.2f\n"
// "angularRate.z: %+#7.2f\n",
// i,
// magnetic.c0,
// magnetic.c1,
// magnetic.c2,
// acceleration.c0,
// acceleration.c1,
// acceleration.c2,
// angularRate.c0,
// angularRate.c1,
// angularRate.c2);
// vn200_getInsSolution(
// &vn200,
// &gpsTime,
// &gpsWeek,
// &status,
// &ypr,
// &latitudeLognitudeAltitude,
// &nedVelocity,
// &attitudeUncertainty,
// &positionUncertainty,
// &velocityUncertainty);
// printf("INS Solution:\n"
// " i: %d\n"
// " GPS Time: %f\n"
// " GPS Week: %u\n"
// " INS Status: %.4X\n"
// " YPR.Yaw: %+#7.2f\n"
// " YPR.Pitch: %+#7.2f\n"
// " YPR.Roll: %+#7.2f\n"
// " LLA.Lattitude: %+#7.2f\n"
// " LLA.Longitude: %+#7.2f\n"
// " LLA.Altitude: %+#7.2f\n"
// " Velocity.North: %+#7.2f\n"
// " Velocity.East: %+#7.2f\n"
// " Velocity.Down: %+#7.2f\n"
// " Attitude Uncertainty: %+#7.2f\n"
// " Position Uncertainty: %+#7.2f\n"
// " Velocity Uncertainty: %+#7.2f\n",
// i,
// gpsTime,
// gpsWeek,
// status,
// ypr.c0,
// ypr.c1,
// ypr.c2,
// latitudeLognitudeAltitude.c0,
// latitudeLognitudeAltitude.c1,
// latitudeLognitudeAltitude.c2,
// nedVelocity.c0,
// nedVelocity.c1,
// nedVelocity.c2,
// attitudeUncertainty,
// positionUncertainty,
// velocityUncertainty);
printf("\n\n");
// sleep(0.05);
i++;
}
vn200_disconnect(&vn200);
return 0;
}
