void CloseIMU()
{
int errorCode = vn200_disconnect(&vn200);
if (errorCode != VNERR_NO_ERROR) {
printf("Error encountered when trying to disconnect from the sensor.\n");
}
}
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;
}
void* sensors_main(void* arg) {
Vn200 vn200;
while (!globQuitSig) {
while(initAsyncSensors(&vn200) == -1) // in case we failed sleep and try to reconnect
sleep(VN_CONNECTION_POLL_RATE);
printf("Sensornav connected succesfully\n");
while(vn200_verifyConnectivity(&vn200)) {
usleep(VN_GET_DATA_ASYNC_RATE);
vn200_getCurrentAsyncData(&vn200, &sensorData);
sprintf(gps_buf,
"GPS.Lat: %+#7.2f, "
"GPS.Lon: %+#7.2f, "
"GPS.Alt: %+#7.2f\n",
sensorData.latitudeLongitudeAltitude.c0,
sensorData.latitudeLongitudeAltitude.c1,
sensorData.latitudeLongitudeAltitude.c2);
sprintf(sensors_buf,
"YPR.Yaw: %+#7.2f, "
"YPR.Pitch: %+#7.2f, "
"YPR.Roll: %+#7.2f",
sensorData.ypr.yaw,
sensorData.ypr.pitch,
sensorData.ypr.roll);
sprintf(velocity_buf,
"VelocityX: %+#7.2f, "
"VelocityY: %+#7.2f, "
"VelocityZ: %+#7.2f",
sensorData.velocity.c0,
sensorData.velocity.c1,
sensorData.velocity.c2);
}
printf("Disconnecting from sensors\n");
if (vn200_disconnect(&vn200) != VNERR_NO_ERROR)
perror("Error encountered when trying to disconnect from the sensor\n");
}
return 0;
}
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;
}
