private void assertWaypointsParsed(final String note, final int expectedWaypoints) {
recordMapStoreFlags();
try {
setMapStoreFlags(false, false);
final Geocache cache = new Geocache();
final String geocode = "Test" + System.nanoTime();
cache.setGeocode(geocode);
cache.setWaypoints(new ArrayList<Waypoint>(), false);
for (int i = 0; i < 2; i++) {
cache.setPersonalNote(note);
cache.parseWaypointsFromNote();
final List<Waypoint> waypoints = cache.getWaypoints();
assertThat(waypoints).isNotNull();
assertThat(waypoints).hasSize(expectedWaypoints);
final Waypoint waypoint = waypoints.get(0);
assertThat(waypoint.getCoords()).isEqualTo(new Geopoint("N51 13.888 E007 03.444"));
// assertThat(waypoint.getNote()).isEqualTo("Test");
assertThat(waypoint.getName())
.isEqualTo(
CgeoApplication.getInstance().getString(R.string.cache_personal_note) + " 1");
cache.store(StoredList.TEMPORARY_LIST.id, null);
}
removeCacheCompletely(geocode);
} finally {
restoreMapStoreFlags();
}
}
public Element getKMLElement(Document doc) {
Element root = doc.createElement("kml");
root.appendChild(doc.createElement("start").appendChild(doc.createTextNode("" + startTime)));
Element trailPoints = doc.createElement("trail-points");
StringBuilder ptsSB = new StringBuilder("");
for (TTLocation loc : locations) {
appendLocationString(ptsSB, loc);
}
trailPoints.appendChild(doc.createTextNode(ptsSB.toString()));
root.appendChild(trailPoints);
Element stops = doc.createElement("stops");
StringBuilder stopsSB = new StringBuilder("");
for (Stop stop : getStops()) {
appendLocationString(stopsSB, stop.getStartLocation());
}
stops.appendChild(doc.createTextNode(stopsSB.toString()));
root.appendChild(stops);
Element waypointsEle = doc.createElement("waypoints");
for (Waypoint wp : waypoints) {
Element wpEle = doc.createElement("waypoint");
wpEle.appendChild(doc.createElement("name").appendChild(doc.createTextNode(wp.getName())));
StringBuilder wpLocSB = new StringBuilder("");
appendLocationString(wpLocSB, wp.getLocation());
wpEle.appendChild(
doc.createElement("location").appendChild(doc.createTextNode(wpLocSB.toString())));
ByteArrayOutputStream outputStream = new ByteArrayOutputStream();
wp.getImage().compress(Bitmap.CompressFormat.JPEG, 80, outputStream);
Log.d("Map getKMLELement()", "jpg output:" + outputStream.toString());
wpEle.appendChild(
doc.createElement("image").appendChild(doc.createTextNode(outputStream.toString())));
waypointsEle.appendChild(wpEle);
}
root.appendChild(waypointsEle);
// TODO checkpoints
root.appendChild(doc.createElement("map-title").appendChild(doc.createTextNode(name)));
root.appendChild(
doc.createElement("path-distance")
.appendChild(doc.createTextNode(Float.toString(totalDistance))));
root.appendChild(
doc.createElement("linear-distance")
.appendChild(doc.createTextNode(Float.toString(linearDistance))));
root.appendChild(
doc.createElement("average-speed")
.appendChild(doc.createTextNode(Float.toString(averageSpeed))));
root.appendChild(
doc.createElement("maximum-speed")
.appendChild(doc.createTextNode(Float.toString(maximumSpeed))));
root.appendChild(
doc.createElement("start-elevation")
.appendChild(doc.createTextNode(Float.toString(startAltitude))));
root.appendChild(
doc.createElement("end-elevation")
.appendChild(doc.createTextNode(Float.toString(endAltitude))));
root.appendChild(
doc.createElement("trip-time")
.appendChild(doc.createTextNode(Integer.toString((endTime - startTime) / 1000))));
root.appendChild(doc.createElement("notes").appendChild(doc.createTextNode(notes)));
return root;
}
public void removeWaypoint(BlockVector vector) {
Waypoint waypoint = null;
for (Waypoint wp : waypoints) {
if (wp.getVector().equals(vector)) {
waypoint = wp;
break;
}
}
if (waypoint != null) {
this.removeWaypoint(waypoint);
}
}
public static Shape<Point> project(
Shape<? extends Coordinates> shape, Calibration calibration, int width, int height) {
Shape<Point> s;
Point[] points = projectPoints(shape, calibration, width, height);
if (shape instanceof Waypoint) {
Waypoint<Point> wp = new Waypoint<Point>(points[0]);
wp.label = ((Waypoint<?>) shape).label;
s = wp;
} else {
s = new Path<Point>(points);
}
s.style = shape.style;
return s;
}
public void testPackMissionItem() {
Mission mission = new Mission(null);
Waypoint item = new Waypoint(mission, new Coord3D(0, 1, new Altitude(2)));
List<msg_mission_item> listOfMsg = item.packMissionItem();
assertEquals(1, listOfMsg.size());
msg_mission_item msg = listOfMsg.get(0);
assertEquals(MAV_CMD.MAV_CMD_NAV_WAYPOINT, msg.command);
assertEquals(0.0f, msg.param1);
assertEquals(0.0f, msg.param2);
assertEquals(0.0f, msg.param3);
assertEquals(0.0f, msg.param3);
}
public static final Map instanceOf(TTSQLiteOpenHelper sqLiteOpenHelper, int mapId) {
Map map = new Map();
SQLiteDatabase database = sqLiteOpenHelper.getReadableDatabase();
Cursor cursor =
database.query(
TABLE_MAPS, ALL_MAP_COLUMNS, COLUMN_ID + " = " + mapId, null, null, null, null);
cursor.moveToFirst();
map.id = mapId;
map.name = cursor.getString(cursor.getColumnIndex(COLUMN_NAME));
map.startTime = cursor.getInt(cursor.getColumnIndex(COLUMN_START_TIME));
map.endTime = cursor.getInt(cursor.getColumnIndex(COLUMN_END_TIME));
map.averageSpeed = cursor.getFloat(cursor.getColumnIndex(COLUMN_AVERAGE_SPEED));
map.totalDistance = cursor.getFloat(cursor.getColumnIndex(COLUMN_TOTAL_DISTANCE));
map.linearDistance = cursor.getFloat(cursor.getColumnIndex(COLUMN_LINEAR_DISTANCE));
map.maximumSpeed = cursor.getFloat(cursor.getColumnIndex(COLUMN_MAXIMUM_SPEED));
map.maximumAltitude = cursor.getFloat(cursor.getColumnIndex(COLUMN_MAX_ALTITUDE));
map.minimumAltitude = cursor.getFloat(cursor.getColumnIndex(COLUMN_MIN_ALTITUDE));
map.startAltitude = cursor.getFloat(cursor.getColumnIndex(COLUMN_START_ALTITUDE));
map.endAltitude = cursor.getFloat(cursor.getColumnIndex(COLUMN_END_ALTITUDE));
map.notes = cursor.getString(cursor.getColumnIndex(COLUMN_NOTES));
cursor.close();
map.locations = TTLocation.getAll(sqLiteOpenHelper, mapId);
map.waypoints = Waypoint.getAll(sqLiteOpenHelper, mapId);
map.stops = Stop.getAll(sqLiteOpenHelper, mapId);
return map;
}
public boolean hasWaypoint(Waypoint waypoint) {
for (int i = 0; i < this.waypoints.size(); i++) {
if (waypoint.equals(this.waypoints.get(i))) {
return true;
}
}
return false;
}
public float getHeadingAtWP(int index) {
float heading = 0;
Waypoint nextWayPoint = getNextWayPoint(index);
// if next way point available, compute heading towards it
if (nextWayPoint != null) {
// compute driving direction by looking at next way point from current position
Vector3f targetPosition = nextWayPoint.getPosition().clone();
targetPosition.setY(0);
Vector3f currentPosition = waypointList.get(index).getPosition().clone();
currentPosition.setY(0);
Vector3f drivingDirection = targetPosition.subtract(currentPosition).normalize();
// compute heading (orientation) from driving direction vector for
// angle between driving direction and heading "0"
float angle0 = drivingDirection.angleBetween(new Vector3f(0, 0, -1));
// angle between driving direction and heading "90"
float angle90 = drivingDirection.angleBetween(new Vector3f(1, 0, 0));
// get all candidates for heading
// find the value from {heading1,heading2} which matches with one of {heading3,heading4}
float heading1 = (2.0f * FastMath.PI + angle0) % FastMath.TWO_PI;
float heading2 = (2.0f * FastMath.PI - angle0) % FastMath.TWO_PI;
float heading3 = (2.5f * FastMath.PI + angle90) % FastMath.TWO_PI;
float heading4 = (2.5f * FastMath.PI - angle90) % FastMath.TWO_PI;
float diff_1_3 = FastMath.abs(heading1 - heading3);
float diff_1_4 = FastMath.abs(heading1 - heading4);
float diff_2_3 = FastMath.abs(heading2 - heading3);
float diff_2_4 = FastMath.abs(heading2 - heading4);
if ((diff_1_3 < diff_1_4 && diff_1_3 < diff_2_3 && diff_1_3 < diff_2_4)
|| (diff_1_4 < diff_1_3 && diff_1_4 < diff_2_3 && diff_1_4 < diff_2_4)) {
// if diff_1_3 or diff_1_4 are smallest --> the correct heading is heading1
heading = heading1;
} else {
// if diff_2_3 or diff_2_4 are smallest --> the correct heading is heading2
heading = heading2;
}
}
return heading;
}
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);
}
@Override
public boolean updateWaypoint(Waypoint waypoint) {
int rows =
contentResolver.update(
WaypointsColumns.CONTENT_URI,
createContentValues(waypoint),
WaypointsColumns._ID + "=?",
new String[] {Long.toString(waypoint.getId())});
return rows == 1;
}
/**
* Suffix the waypoint type with a running number to get a default name.
*
* @param type type to create a new default name for
* @return
*/
private String getDefaultWaypointName(final WaypointType type) {
final ArrayList<String> wpNames = new ArrayList<>();
for (final Waypoint waypoint : cache.getWaypoints()) {
wpNames.add(waypoint.getName());
}
// try final and trailhead without index
if (type == WaypointType.FINAL || type == WaypointType.TRAILHEAD) {
if (!wpNames.contains(type.getL10n())) {
return type.getL10n();
}
}
// for other types add an index by default, which is highest found index + 1
int max = 0;
for (int i = 0; i < 30; i++) {
if (wpNames.contains(type.getL10n() + " " + i)) {
max = i;
}
}
return type.getL10n() + " " + (max + 1);
}
public long add(Waypoint w) {
if (w.time == 0) w.time = System.currentTimeMillis();
long out = db.add(w);
SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(context);
if (this.nextWaypointName().equals(w.name)) {
int num = prefs.getInt(KEY_WAYPOINT_NUMBER, 1);
prefs.edit().putInt(KEY_WAYPOINT_NUMBER, num + 1).commit();
}
if (map != null && out > 0) refreshMarkers();
Log.i(TAG, "adding waypoint " + out);
return out;
}
private Waypoint getNextStatisticsWaypointAfter(Waypoint waypoint) {
Cursor cursor = null;
try {
String selection =
WaypointsColumns._ID
+ ">? AND "
+ WaypointsColumns.TRACKID
+ "=? AND "
+ WaypointsColumns.TYPE
+ "="
+ Waypoint.TYPE_STATISTICS;
String[] selectionArgs =
new String[] {Long.toString(waypoint.getId()), Long.toString(waypoint.getTrackId())};
cursor = getWaypointCursor(null, selection, selectionArgs, WaypointsColumns._ID, 1);
if (cursor != null && cursor.moveToFirst()) {
return createWaypoint(cursor);
}
} finally {
if (cursor != null) {
cursor.close();
}
}
return null;
}
public void removeWaypoint(Waypoint waypoint) {
boolean found = false;
int deleteIndex = -1;
for (int index = 0; index < this.waypoints.size(); index++) {
if (found) {
this.waypoints.get(index).setPlaceInLine(this.waypoints.get(index).getPlaceInLine() - 1);
} else if (waypoint.equals(this.waypoints.get(index))) {
deleteIndex = index;
found = true;
}
}
if (found) {
this.waypoints.remove(deleteIndex);
}
}
private int getDefaultWaypointType() {
// potentially restore saved instance state
if (waypointTypeSelectorPosition >= 0) {
return waypointTypeSelectorPosition;
}
// when editing, use the existing type
if (waypoint != null) {
return POSSIBLE_WAYPOINT_TYPES.indexOf(waypoint.getWaypointType());
}
// make default for new waypoint depend on cache type
switch (cache.getType()) {
case MYSTERY:
return POSSIBLE_WAYPOINT_TYPES.indexOf(WaypointType.FINAL);
case MULTI:
return POSSIBLE_WAYPOINT_TYPES.indexOf(WaypointType.STAGE);
default:
return POSSIBLE_WAYPOINT_TYPES.indexOf(WaypointType.WAYPOINT);
}
}
@Override
public void deleteWaypoint(long waypointId, DescriptionGenerator descriptionGenerator) {
final Waypoint waypoint = getWaypoint(waypointId);
if (waypoint != null && waypoint.getType() == Waypoint.TYPE_STATISTICS) {
final Waypoint nextWaypoint = getNextStatisticsWaypointAfter(waypoint);
if (nextWaypoint == null) {
Log.d(TAG, "Unable to find the next statistics marker after deleting one.");
} else {
nextWaypoint.getTripStatistics().merge(waypoint.getTripStatistics());
nextWaypoint.setDescription(
descriptionGenerator.generateWaypointDescription(nextWaypoint.getTripStatistics()));
if (!updateWaypoint(nextWaypoint)) {
Log.e(TAG, "Unable to update the next statistics marker after deleting one.");
}
}
}
contentResolver.delete(
WaypointsColumns.CONTENT_URI,
WaypointsColumns._ID + "=?",
new String[] {Long.toString(waypointId)});
}
@Override
public void handleMessage(final Message msg) {
try {
if (waypoint == null) {
Log.d("No waypoint loaded to edit. id= " + waypointId);
waypointId = -1;
} else {
geocode = waypoint.getGeocode();
prefix = waypoint.getPrefix();
lookup = waypoint.getLookup();
own = waypoint.isUserDefined();
if (initViews) {
visitedCheckBox.setChecked(waypoint.isVisited());
if (waypoint.getCoords() != null) {
buttonLat.setText(
waypoint.getCoords().format(GeopointFormatter.Format.LAT_DECMINUTE));
buttonLon.setText(
waypoint.getCoords().format(GeopointFormatter.Format.LON_DECMINUTE));
}
waypointName.setText(
Html.fromHtml(StringUtils.trimToEmpty(waypoint.getName())).toString());
Dialogs.moveCursorToEnd(waypointName);
if (TextUtils.containsHtml(waypoint.getNote())) {
note.setText(
Html.fromHtml(StringUtils.trimToEmpty(waypoint.getNote())).toString());
} else {
note.setText(StringUtils.trimToEmpty(waypoint.getNote()));
}
Dialogs.moveCursorToEnd(note);
}
new AsyncTask<Void, Void, Geocache>() {
@Override
protected Geocache doInBackground(final Void... params) {
return DataStore.loadCache(geocode, LoadFlags.LOAD_CACHE_ONLY);
}
@Override
protected void onPostExecute(final Geocache cache) {
setCoordsModificationVisibility(ConnectorFactory.getConnector(geocode), cache);
}
}.execute();
}
if (own) {
initializeWaypointTypeSelector();
}
} catch (final RuntimeException e) {
Log.e("EditWaypointActivity.loadWaypointHandler", e);
} finally {
if (waitDialog != null) {
waitDialog.dismiss();
waitDialog = null;
}
}
}
ContentValues createContentValues(Waypoint waypoint) {
ContentValues values = new ContentValues();
// Value < 0 indicates no id is available
if (waypoint.getId() >= 0) {
values.put(WaypointsColumns._ID, waypoint.getId());
}
values.put(WaypointsColumns.NAME, waypoint.getName());
values.put(WaypointsColumns.DESCRIPTION, waypoint.getDescription());
values.put(WaypointsColumns.CATEGORY, waypoint.getCategory());
values.put(WaypointsColumns.ICON, waypoint.getIcon());
values.put(WaypointsColumns.TRACKID, waypoint.getTrackId());
values.put(WaypointsColumns.TYPE, waypoint.getType());
values.put(WaypointsColumns.LENGTH, waypoint.getLength());
values.put(WaypointsColumns.DURATION, waypoint.getDuration());
values.put(WaypointsColumns.STARTID, waypoint.getStartId());
values.put(WaypointsColumns.STOPID, waypoint.getStopId());
Location location = waypoint.getLocation();
if (location != null) {
values.put(WaypointsColumns.LONGITUDE, (int) (location.getLongitude() * 1E6));
values.put(WaypointsColumns.LATITUDE, (int) (location.getLatitude() * 1E6));
values.put(WaypointsColumns.TIME, location.getTime());
if (location.hasAltitude()) {
values.put(WaypointsColumns.ALTITUDE, location.getAltitude());
}
if (location.hasAccuracy()) {
values.put(WaypointsColumns.ACCURACY, location.getAccuracy());
}
if (location.hasSpeed()) {
values.put(WaypointsColumns.SPEED, location.getSpeed());
}
if (location.hasBearing()) {
values.put(WaypointsColumns.BEARING, location.getBearing());
}
}
TripStatistics tripStatistics = waypoint.getTripStatistics();
if (tripStatistics != null) {
values.put(WaypointsColumns.STARTTIME, tripStatistics.getStartTime());
values.put(WaypointsColumns.TOTALDISTANCE, tripStatistics.getTotalDistance());
values.put(WaypointsColumns.TOTALTIME, tripStatistics.getTotalTime());
values.put(WaypointsColumns.MOVINGTIME, tripStatistics.getMovingTime());
values.put(WaypointsColumns.AVGSPEED, tripStatistics.getAverageSpeed());
values.put(WaypointsColumns.AVGMOVINGSPEED, tripStatistics.getAverageMovingSpeed());
values.put(WaypointsColumns.MAXSPEED, tripStatistics.getMaxSpeed());
values.put(WaypointsColumns.MINELEVATION, tripStatistics.getMinElevation());
values.put(WaypointsColumns.MAXELEVATION, tripStatistics.getMaxElevation());
values.put(WaypointsColumns.ELEVATIONGAIN, tripStatistics.getTotalElevationGain());
values.put(WaypointsColumns.MINGRADE, tripStatistics.getMinGrade());
values.put(WaypointsColumns.MAXGRADE, tripStatistics.getMaxGrade());
}
return values;
}
@Override
public Uri insertWaypoint(Waypoint waypoint) {
waypoint.setId(-1L);
return contentResolver.insert(WaypointsColumns.CONTENT_URI, createContentValues(waypoint));
}
@Override
public Waypoint createWaypoint(Cursor cursor) {
int idIndex = cursor.getColumnIndexOrThrow(WaypointsColumns._ID);
int nameIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.NAME);
int descriptionIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.DESCRIPTION);
int categoryIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.CATEGORY);
int iconIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.ICON);
int trackIdIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.TRACKID);
int typeIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.TYPE);
int lengthIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.LENGTH);
int durationIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.DURATION);
int startTimeIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.STARTTIME);
int startIdIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.STARTID);
int stopIdIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.STOPID);
int longitudeIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.LONGITUDE);
int latitudeIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.LATITUDE);
int timeIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.TIME);
int altitudeIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.ALTITUDE);
int accuracyIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.ACCURACY);
int speedIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.SPEED);
int bearingIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.BEARING);
int totalDistanceIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.TOTALDISTANCE);
int totalTimeIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.TOTALTIME);
int movingTimeIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.MOVINGTIME);
int maxSpeedIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.MAXSPEED);
int minElevationIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.MINELEVATION);
int maxElevationIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.MAXELEVATION);
int elevationGainIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.ELEVATIONGAIN);
int minGradeIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.MINGRADE);
int maxGradeIndex = cursor.getColumnIndexOrThrow(WaypointsColumns.MAXGRADE);
Waypoint waypoint = new Waypoint();
if (!cursor.isNull(idIndex)) {
waypoint.setId(cursor.getLong(idIndex));
}
if (!cursor.isNull(nameIndex)) {
waypoint.setName(cursor.getString(nameIndex));
}
if (!cursor.isNull(descriptionIndex)) {
waypoint.setDescription(cursor.getString(descriptionIndex));
}
if (!cursor.isNull(categoryIndex)) {
waypoint.setCategory(cursor.getString(categoryIndex));
}
if (!cursor.isNull(iconIndex)) {
waypoint.setIcon(cursor.getString(iconIndex));
}
if (!cursor.isNull(trackIdIndex)) {
waypoint.setTrackId(cursor.getLong(trackIdIndex));
}
if (!cursor.isNull(typeIndex)) {
waypoint.setType(cursor.getInt(typeIndex));
}
if (!cursor.isNull(lengthIndex)) {
waypoint.setLength(cursor.getFloat(lengthIndex));
}
if (!cursor.isNull(durationIndex)) {
waypoint.setDuration(cursor.getLong(durationIndex));
}
if (!cursor.isNull(startIdIndex)) {
waypoint.setStartId(cursor.getLong(startIdIndex));
}
if (!cursor.isNull(stopIdIndex)) {
waypoint.setStopId(cursor.getLong(stopIdIndex));
}
Location location = new Location("");
if (!cursor.isNull(longitudeIndex) && !cursor.isNull(latitudeIndex)) {
location.setLongitude(((double) cursor.getInt(longitudeIndex)) / 1E6);
location.setLatitude(((double) cursor.getInt(latitudeIndex)) / 1E6);
}
if (!cursor.isNull(timeIndex)) {
location.setTime(cursor.getLong(timeIndex));
}
if (!cursor.isNull(altitudeIndex)) {
location.setAltitude(cursor.getFloat(altitudeIndex));
}
if (!cursor.isNull(accuracyIndex)) {
location.setAccuracy(cursor.getFloat(accuracyIndex));
}
if (!cursor.isNull(speedIndex)) {
location.setSpeed(cursor.getFloat(speedIndex));
}
if (!cursor.isNull(bearingIndex)) {
location.setBearing(cursor.getFloat(bearingIndex));
}
waypoint.setLocation(location);
TripStatistics tripStatistics = new TripStatistics();
boolean hasTripStatistics = false;
if (!cursor.isNull(startTimeIndex)) {
tripStatistics.setStartTime(cursor.getLong(startTimeIndex));
hasTripStatistics = true;
}
if (!cursor.isNull(totalDistanceIndex)) {
tripStatistics.setTotalDistance(cursor.getFloat(totalDistanceIndex));
hasTripStatistics = true;
}
if (!cursor.isNull(totalTimeIndex)) {
tripStatistics.setTotalTime(cursor.getLong(totalTimeIndex));
hasTripStatistics = true;
}
if (!cursor.isNull(movingTimeIndex)) {
tripStatistics.setMovingTime(cursor.getLong(movingTimeIndex));
hasTripStatistics = true;
}
if (!cursor.isNull(maxSpeedIndex)) {
tripStatistics.setMaxSpeed(cursor.getFloat(maxSpeedIndex));
hasTripStatistics = true;
}
if (!cursor.isNull(minElevationIndex)) {
tripStatistics.setMinElevation(cursor.getFloat(minElevationIndex));
hasTripStatistics = true;
}
if (!cursor.isNull(maxElevationIndex)) {
tripStatistics.setMaxElevation(cursor.getFloat(maxElevationIndex));
hasTripStatistics = true;
}
if (!cursor.isNull(elevationGainIndex)) {
tripStatistics.setTotalElevationGain(cursor.getFloat(elevationGainIndex));
hasTripStatistics = true;
}
if (!cursor.isNull(minGradeIndex)) {
tripStatistics.setMinGrade(cursor.getFloat(minGradeIndex));
hasTripStatistics = true;
}
if (!cursor.isNull(maxGradeIndex)) {
tripStatistics.setMaxGrade(cursor.getFloat(maxGradeIndex));
hasTripStatistics = true;
}
if (hasTripStatistics) {
waypoint.setTripStatistics(tripStatistics);
}
return waypoint;
}
public DroneMessage getAutonomousUpdate(double lat, double lon, double heading, Double altitude) {
// If we do not have a route, return nil
if (smoothRoute.size() == 0) {
return null;
}
// If we do not know which waypoint is the current target, find the next waypoint and set its
// index
if (currentTargetIndex == -1) {
// TODO: Resume route?
// TODO: Crash/reboot survival?
currentTargetIndex = 0;
}
/*Check if the next waypoint exists and we should switch to the next waypoint
Rules for switching to next WP
- Next WP must exist (otherwise orbit until directive changes)
- If we are orbiting an "orbit until target altitude" *orbit* waypoint, we must be at target altitude (within 10m)
- If we are orbiting, we must be withtin 25m of the *next* waypoint and witch to the next *plus one*
- If we are not orbiting, we switch to the orbit waypoint if we are within 25m od the *next one*
*/
if (currentTargetIndex < (smoothRoute.size() - 1)) {
Waypoint currentTarget = smoothRoute.get(currentTargetIndex);
Waypoint ownPosition = new Waypoint(lat, lon, altitude);
if (currentTarget.hasOrbit()) {
Waypoint endOfOrbit = smoothRoute.get(currentTargetIndex + 1);
double distanceToEnd = ownPosition.distance(endOfOrbit);
if (distanceToEnd < switchToNextWaypointRange) {
if (currentTarget.orbitUntilTargetAltitude == true) {
if (Math.abs(altitude - currentTarget.altitude) < epsilonAltitude) {
currentTargetIndex = currentTargetIndex + 2;
}
} else {
currentTargetIndex = currentTargetIndex + 2;
}
}
} else {
// If we are approaching anything beyond the second WP, Check if we have overshot the
// target,
// e.g the difference in bearing start - end and pos - end is greater than 90°
double difference;
if (currentTargetIndex > 0) {
Waypoint start = smoothRoute.get(currentTargetIndex - 1);
double startEnd = start.bearingTo(currentTarget);
double posEnd = ownPosition.bearingTo(currentTarget);
// Wonky angle subtraction
difference = startEnd - posEnd;
if (Math.abs(difference) > 180) {
if (startEnd - posEnd < 0) {
startEnd += 360;
} else {
posEnd += 360;
}
}
difference = startEnd - posEnd;
} else {
// Difference is irrelevant otherwise, set it to 0
difference = 0;
}
if (Math.abs(difference) > 90) {
System.out.println("Warning: overshot!");
currentTargetIndex = currentTargetIndex + 1;
} else {
Waypoint nextOrbitStart = smoothRoute.get(currentTargetIndex);
double distanceToNext = ownPosition.distance(nextOrbitStart);
if (distanceToNext < switchToNextWaypointRange) {
currentTargetIndex = currentTargetIndex + 1;
}
}
}
}
// This is the current Target *after* updating
Waypoint currentTarget = smoothRoute.get(currentTargetIndex);
Waypoint ownPosition = new Waypoint(lat, lon, altitude);
double desiredHeading;
if (currentTarget.hasOrbit()) {
// Calculate how far we are from the intended track
// >0 menas we are outside, <0 menas we are inside
double error = ownPosition.distance(currentTarget) - currentTarget.orbitRadius;
double courseCorrection = bearingCorrectionForError(error);
if (currentTarget.orbitClockwise) {
// If we are orbiting clockwise, the tangential heading is the bearing to the middle *-* 90
double tangentialHeading = ownPosition.bearingTo(currentTarget) - 90;
// If we are orbiting clockwise, we need to apply the coursecorrection by addition
desiredHeading = tangentialHeading + courseCorrection;
} else {
// Counterclockwise, add 90 to bearing towards middle
double tangentialHeading = ownPosition.bearingTo(currentTarget) + 90;
// And subtract the correnction
desiredHeading = tangentialHeading - courseCorrection;
}
} else {
if (currentTargetIndex > 0) {
Waypoint lineStart = smoothRoute.get(currentTargetIndex - 1);
double error = Waypoint.crossTrackError(lineStart, currentTarget, ownPosition);
double courseCorrection = bearingCorrectionForError(error);
double targetHeading = lineStart.bearingTo(currentTarget);
desiredHeading = targetHeading + courseCorrection;
} else {
// We are still approaching the start waypoint
desiredHeading = ownPosition.bearingTo(currentTarget);
}
}
System.out.format(
"Lat: %f, Lon: %f, Alt: %.1f, new Course %.0f, new Alt %.0f %n",
lat, lon, altitude, desiredHeading, currentTarget.altitude);
// Create the update message
DroneMessage update =
DroneMessage.newBuilder()
.setAutonomousUpdate(
DroneMessage.AutonomousUpdate.newBuilder()
.setAltitude((int) Math.round(currentTarget.altitude * 100))
.setHeading((int) Math.round(desiredHeading * 64))
.build())
.build();
return update;
}
private static Waypoint waypointFrom(Cursor c) {
Waypoint w = new Waypoint();
w.id = c.getLong(0);
w.lat = c.getDouble(1);
w.lon = c.getDouble(2);
w.altitude = c.getDouble(3);
w.name = c.getString(4);
w.description = c.getString(5);
w.icon = c.getString(6);
w.iconType = c.getString(7);
w.temporary = c.getInt(8) == 1;
w.time = c.getLong(9);
w.category = c.getString(10);
return w;
}
public float getReducedSpeed() {
// return a temporarily reduced speed for the traffic car
// in order to reach next (lower) speed limit in time
float reducedSpeedInKmh = Float.POSITIVE_INFINITY;
// if next way point with lower speed comes closer --> reduce speed
int currentIndex = motionControl.getCurrentWayPoint();
Waypoint nextWP = getNextWayPoint(currentIndex);
if (nextWP != null) {
// current way point (already passed)
Waypoint curentWP = waypointList.get(currentIndex);
// speed at current way point
float currentSpeedInKmh = curentWP.getSpeed();
float currentSpeed = currentSpeedInKmh / 3.6f;
// speed at next way point
float targetSpeedInKmh = nextWP.getSpeed();
float targetSpeed = targetSpeedInKmh / 3.6f;
// if speed at the next WP is lower than at the current WP --> brake vehicle
if (targetSpeed < currentSpeed) {
// % of traveled distance between current and next way point
float wayPercentage = motionControl.getCurrentValue();
// distance between current and next way point
Vector3f currentPos = curentWP.getPosition().clone();
currentPos.setY(0);
Vector3f nextPos = nextWP.getPosition().clone();
nextPos.setY(0);
float distance = currentPos.distance(nextPos);
// distance (in meters) between follow box and next way point
float distanceToNextWP = (1 - wayPercentage) * distance;
// speed difference in m/s between current WP's speed and next WP's speed
float speedDifference = currentSpeed - targetSpeed;
// compute the distance in front of the next WP at what the vehicle has to start
// braking with 50% brake force in order to reach the next WP's (lower) speed in time.
float deceleration50Percent = 50f * vehicle.getMaxBrakeForce() / vehicle.getMass();
// time in seconds needed for braking process
float time = speedDifference / deceleration50Percent;
// distance covered during braking process
float coveredDistance = 0.5f * -deceleration50Percent * time * time + currentSpeed * time;
// start braking in x meters
float distanceToBrakingPoint = distanceToNextWP - coveredDistance;
if (distanceToBrakingPoint < 0) {
// reduce speed linearly beginning from braking point
// % of traveled distance between braking point and next way point
float speedPercentage = -distanceToBrakingPoint / coveredDistance;
// 0% traveled: reduced speed = currentSpeed
// 50% traveled: reduced speed = (currentSpeed+targetSpeed)/2
// 100% traveled: reduced speed = targetSpeed
float reducedSpeed = currentSpeed - (speedPercentage * speedDifference);
reducedSpeedInKmh = reducedSpeed * 3.6f;
/*
if(vehicle.getName().equals("car1"))
{
float vehicleSpeedInKmh = vehicle.getLinearSpeedInKmh();
System.out.println(curentWP.getName() + " : " + speedPercentage + " : " +
reducedSpeedInKmh + " : " + vehicleSpeedInKmh + " : " + targetSpeedInKmh);
}
*/
}
}
}
return reducedSpeedInKmh;
}
@Override
protected Void doInBackground(final Void... params) {
final Waypoint waypoint = new Waypoint(name, type, own);
waypoint.setGeocode(geocode);
waypoint.setPrefix(prefix);
waypoint.setLookup(lookup);
waypoint.setCoords(coordsToSave);
waypoint.setNote(noteText);
waypoint.setVisited(visited);
waypoint.setId(waypointId);
final Geocache cache = DataStore.loadCache(geocode, LoadFlags.LOAD_WAYPOINTS);
if (cache == null) {
finishHandler.sendEmptyMessage(SAVE_ERROR);
return null;
}
final Waypoint oldWaypoint = cache.getWaypointById(waypointId);
if (cache.addOrChangeWaypoint(waypoint, true)) {
DataStore.saveCache(cache, EnumSet.of(SaveFlag.DB));
if (!StaticMapsProvider.hasAllStaticMapsForWaypoint(geocode, waypoint)) {
StaticMapsProvider.removeWpStaticMaps(oldWaypoint, geocode);
if (Settings.isStoreOfflineWpMaps()) {
StaticMapsProvider.storeWaypointStaticMap(cache, waypoint).subscribe();
}
}
if (modifyLocal.isChecked() || modifyBoth.isChecked()) {
if (!cache.hasUserModifiedCoords()) {
final Waypoint origWaypoint =
new Waypoint(
CgeoApplication.getInstance()
.getString(R.string.cache_coordinates_original),
WaypointType.ORIGINAL,
false);
origWaypoint.setCoords(cache.getCoords());
cache.addOrChangeWaypoint(origWaypoint, false);
cache.setUserModifiedCoords(true);
}
cache.setCoords(waypoint.getCoords());
DataStore.saveChangedCache(cache);
}
if (modifyBoth.isChecked() && waypoint.getCoords() != null) {
finishHandler.sendEmptyMessage(UPLOAD_START);
if (cache.supportsOwnCoordinates()) {
final boolean result = uploadModifiedCoords(cache, waypoint.getCoords());
finishHandler.sendEmptyMessage(result ? SUCCESS : UPLOAD_ERROR);
} else {
showToast(getString(R.string.waypoint_coordinates_couldnt_be_modified_on_website));
finishHandler.sendEmptyMessage(UPLOAD_NOT_POSSIBLE);
}
} else {
finishHandler.sendEmptyMessage(SUCCESS);
}
} else {
finishHandler.sendEmptyMessage(SAVE_ERROR);
}
return null;
}
private WaypointType getSelectedWaypointType() {
final int selectedTypeIndex = waypointTypeSelector.getSelectedItemPosition();
return selectedTypeIndex >= 0
? POSSIBLE_WAYPOINT_TYPES.get(selectedTypeIndex)
: waypoint.getWaypointType();
}
public FollowBox(Simulator sim, final TrafficCar vehicle, FollowBoxSettings settings) {
this.sim = sim;
this.vehicle = vehicle;
this.settings = settings;
waypointList = settings.getWayPoints();
maxDistance = settings.getMaxDistance();
motionPath = new MotionPath();
motionPath.setCycle(settings.isPathCyclic());
for (Waypoint wayPoint : waypointList) motionPath.addWayPoint(wayPoint.getPosition());
motionPath.setPathSplineType(SplineType.CatmullRom); // --> default: CatmullRom
motionPath.setCurveTension(settings.getCurveTension());
if (settings.isPathVisible())
motionPath.enableDebugShape(sim.getAssetManager(), sim.getSceneNode());
motionPath.addListener(
new MotionPathListener() {
public void onWayPointReach(MotionTrack control, int wayPointIndex) {
// set speed limit for next way point
int index = wayPointIndex % waypointList.size();
float speed = waypointList.get(index).getSpeed();
setSpeed(speed);
// if last way point reached
if (motionPath.getNbWayPoints() == wayPointIndex + 1) {
// reset vehicle to first way point if not cyclic
if (!motionPath.isCycle()) setToWayPoint(0);
}
}
});
followBox = createFollowBox();
motionControl = new MotionTrack(followBox, motionPath);
// get start way point
int startWayPointIndex = settings.getStartWayPointIndex();
// set start speed
float initialSpeed = waypointList.get(startWayPointIndex).getSpeed();
setSpeed(initialSpeed);
// set start position
setToWayPoint(startWayPointIndex);
// move object along path considering rotation
motionControl.setDirectionType(MotionTrack.Direction.PathAndRotation);
// loop movement of object
motionControl.setLoopMode(LoopMode.Loop);
// rotate moving object
// motionControl.setRotation(new Quaternion().fromAngleNormalAxis(-FastMath.HALF_PI,
// Vector3f.UNIT_Y));
// set moving object to position "20 seconds"
// motionPath.interpolatePath(20, motionControl);
// start movement
// motionControl.play(); // already contained in update method
}
@Override
public void onCreate(Bundle icicle) {
super.onCreate(icicle);
setContentView(R.layout.main);
compassView = (CompassView) this.findViewById(R.id.compassView1);
log = new ArrayList<String>();
locMan = (LocationManager) getSystemService(Context.LOCATION_SERVICE);
try {
String root = Environment.getExternalStorageDirectory().toString();
File myDir = new File(root + "/HSI/inputs");
myDir.mkdirs();
File input = new File(myDir, filename);
if (input.exists()) {
DataInputStream dataIO = new DataInputStream(new FileInputStream(input));
String line;
if ((line = dataIO.readLine()) != null) {
System.out.println("got header: " + line);
String[] gdata = line.split(",");
offset = Double.valueOf(gdata[0]);
int vForward = Integer.valueOf(gdata[1]);
ErrorVector.setVelocityForward(vForward);
dataIO.close();
course = Waypoint.getWaypoints(this, filename);
} else {
System.out.println("file empty, idiot!");
}
} else {
System.out.println("file doesn't exist, idiot!");
AssetManager am = getAssets();
InputStream is = am.open(filename);
DataInputStream dataIO = new DataInputStream(is);
String strLine = null;
if ((strLine = dataIO.readLine()) == null) {
System.out.println("file is empty, idiot!");
} else {
String[] gdata = strLine.split(",");
offset = Double.valueOf(gdata[0]);
int vForward = Integer.valueOf(gdata[1]);
ErrorVector.setVelocityForward(vForward);
dataIO.close();
course = Waypoint.getWaypoints(this, filename);
}
}
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
course = Waypoint.getWaypoints(this, filename);
timeBuffer = (course.get(0).time);
System.out.println("nearest buffer" + timeBuffer + " ," + course.size());
// course.remove(0);
startTime = System.currentTimeMillis() / 1000.0;
criteria = new Criteria();
criteria.setSpeedRequired(true);
criteria.setBearingRequired(true);
criteria.setAltitudeRequired(true);
compassView.invalidate();
// try to sleep this each iteration
while ((bestProvider = locMan.getBestProvider(criteria, false)) == null) {
System.out.println("Searching for provider.");
continue;
}
loclis =
new LocationListener() {
@Override
public void onLocationChanged(Location location) {
System.out.println("about to update position");
updatePosition(location);
}
@Override
public void onProviderDisabled(String provider) {}
@Override
public void onProviderEnabled(String provider) {}
@Override
public void onStatusChanged(String provider, int status, Bundle extras) {}
};
locMan.requestLocationUpdates(bestProvider, minTime, minDistance, loclis);
}
