int getLocationInfoEx(int num, char *location, char *locationCode) {
    if (file_exists(locationPath) < 0) {

#ifdef DEBUG
        __android_log_print(ANDROID_LOG_INFO, TAG, "data file not exist!");
#endif
        return -1;
    }

    getLocationInfo(locationPath, num, location, locationCode);

#ifdef DEBUG
    __android_log_print(ANDROID_LOG_INFO, TAG, "return is %d, %s, %d, %s",
                        strlen(location), location, strlen(locationCode),
                        locationCode);
#endif

    if (location[0] == ' ' && location[1] == 0x00) return -1;
    strcat(locationCode, ",");
    strcat(locationCode, location);

#ifdef DEBUG
    __android_log_print(ANDROID_LOG_INFO, TAG, "[%d] == %s", num,
                        locationCode);
#endif

    return 0;
}
void setupSequence() {

	pinMode(6,INPUT_PULLUP);
	commandProcessor.setDefaultHandler(&commandDefault);

	delayMicroseconds(1); // make sure the microsecond delay isnt optimised away!

	Serial.begin(9600);
	Serial.println("Initialized.");

	Wire.begin();

	delay(10);  //

	Serial.println("Initializing imaging unit");
	activateImagingScanner(true);
	delay(3000);  //
	activateImagingScanner(false);
	getImagingInfo(imagingInfo);
	printImagingInfo();

	Serial.println("Initializing localization");
	getLocationInfo(navInfo);
	printLocalInfo();

	Serial.println("Arming Motors");

	motorPower(false);
	delay(1000);  //

	motorPower(true);

	Serial.println("Init complete");
}
void updateSensors()
{
	if (updateTimer.timeout(true))
	{
		getImagingInfo(imagingInfo);
		getLocationInfo(navInfo);

		sonarDistance.update(navInfo.sonar);
	}
}
void commandIMU() {
	Serial.println("TEST IMU");

	getLocationInfo(navInfo);
	printLocalInfo();
}