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