private void updatePosition(Location location) {
System.out.println("Updating position");
currentTime = System.currentTimeMillis() / 1000.0 - startTime + timeBuffer - offset;
System.out.println("current time nearest" + currentTime);
Waypoint desiredLocation = Waypoint.getNearestWaypoint(course, currentTime);
System.out.println("nearest waypoint: " + desiredLocation.toString());
double lat = location.getLatitude();
double lon = location.getLongitude();
System.out.println("longitude: " + lon);
double alt = location.getAltitude();
System.out.println("altitude: " + alt);
Waypoint actual = new Waypoint(lat, lon, alt, currentTime, null, null);
log.add(actual.toString());
// write log to file each time, or get rid of log entirely
System.out.println("theta before" + desiredLocation.time);
ErrorVector currentError = ErrorVector.errorVector(actual, desiredLocation);
updateDials(actual, desiredLocation, currentError, location);
}
