Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Estimation/Mapping/Mapping.cpp@25:bb5356402687, 2018-11-30 (annotated)

Committer:
shimniok
Date:
Fri Nov 30 16:11:53 2018 +0000
Revision:
25:bb5356402687
Parent:
19:ce7fdade3534 
Initial publish of revised version.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:a6a169de725f 1 #include "mbed.h" // debugging
shimniok 0:a6a169de725f 2 #include "Mapping.h"
shimniok 0:a6a169de725f 3
shimniok 0:a6a169de725f 4 /*
shimniok 0:a6a169de725f 5 * Provide a series of lat/lon coordinates/waypoints, and the object
shimniok 0:a6a169de725f 6 * automatically generates a bounding rectangle, and determines how to
shimniok 0:a6a169de725f 7 * translate between lat/lon and cartesian x/y
shimniok 0:a6a169de725f 8 */
shimniok 0:a6a169de725f 9 void Mapping::init(int count, GeoPosition *p)
shimniok 0:a6a169de725f 10 {
shimniok 0:a6a169de725f 11 double latMin = 360;
shimniok 0:a6a169de725f 12 double latMax = -360;
shimniok 0:a6a169de725f 13 double lonMin = 360;
shimniok 0:a6a169de725f 14 double lonMax = -360;
shimniok 0:a6a169de725f 15
shimniok 0:a6a169de725f 16 // Find minimum and maximum lat/lon
shimniok 0:a6a169de725f 17 for (int i=0; i < count; i++, p++) {
shimniok 0:a6a169de725f 18 //fprintf(stdout, "%d (%.6f, %.6f)\n", i, p->latitude(), p->longitude());
shimniok 0:a6a169de725f 19 if (p->latitude() > latMax)
shimniok 0:a6a169de725f 20 latMax = p->latitude();
shimniok 0:a6a169de725f 21 if (p->latitude() < latMin)
shimniok 0:a6a169de725f 22 latMin = p->latitude();
shimniok 0:a6a169de725f 23
shimniok 0:a6a169de725f 24 if (p->longitude() > lonMax)
shimniok 0:a6a169de725f 25 lonMax = p->longitude();
shimniok 0:a6a169de725f 26 if (p->longitude() < lonMin)
shimniok 0:a6a169de725f 27 lonMin = p->longitude();
shimniok 0:a6a169de725f 28 }
shimniok 0:a6a169de725f 29
shimniok 0:a6a169de725f 30 // Set the arbitrary cartesian origin to southwest corner
shimniok 0:a6a169de725f 31 lonZero = lonMin;
shimniok 0:a6a169de725f 32 latZero = latMin;
shimniok 0:a6a169de725f 33
shimniok 0:a6a169de725f 34 //fprintf(stdout, "min: (%.6f, %.6f)\n", latMin, lonMin);
shimniok 0:a6a169de725f 35 //fprintf(stdout, "max: (%.6f, %.6f)\n", latMax, lonMax);
shimniok 0:a6a169de725f 36
shimniok 0:a6a169de725f 37 // Three positions required to scale
shimniok 0:a6a169de725f 38 GeoPosition sw(latMin, lonMin);
shimniok 0:a6a169de725f 39 GeoPosition nw(latMax, lonMin);
shimniok 0:a6a169de725f 40 GeoPosition se(latMin, lonMax);
shimniok 0:a6a169de725f 41
shimniok 0:a6a169de725f 42 // Find difference between lat and lon min/max
shimniok 0:a6a169de725f 43 float dlat = (latMax - latMin);
shimniok 0:a6a169de725f 44 float dlon = (lonMax - lonMin);
shimniok 0:a6a169de725f 45
shimniok 0:a6a169de725f 46 //fprintf(stdout, "dlat=%.6f dlon=%.6f\n", dlat, dlon);
shimniok 0:a6a169de725f 47
shimniok 0:a6a169de725f 48 // Compute scale based on distances between edges of rectangle
shimniok 0:a6a169de725f 49 // and difference between min/max lat and min/max lon
shimniok 0:a6a169de725f 50 lonToX = sw.distanceTo(se) / dlon;
shimniok 0:a6a169de725f 51 latToY = sw.distanceTo(nw) / dlat;
shimniok 0:a6a169de725f 52
shimniok 19:ce7fdade3534 53 //fprintf(stdout, "lonToX=%.10f\nlatToY=%.10f\n", lonToX, latToY);
shimniok 0:a6a169de725f 54
shimniok 0:a6a169de725f 55 return;
shimniok 0:a6a169de725f 56 }
shimniok 0:a6a169de725f 57
shimniok 0:a6a169de725f 58 void Mapping::geoToCart(GeoPosition pos, CartPosition *cart)
shimniok 0:a6a169de725f 59 {
shimniok 0:a6a169de725f 60 cart->set( lonToX * (pos.longitude() - lonZero),
shimniok 0:a6a169de725f 61 latToY * (pos.latitude() - latZero) );
shimniok 0:a6a169de725f 62
shimniok 0:a6a169de725f 63 return;
shimniok 0:a6a169de725f 64 }
shimniok 0:a6a169de725f 65
shimniok 0:a6a169de725f 66 void Mapping::cartToGeo(float x, float y, GeoPosition *pos)
shimniok 0:a6a169de725f 67 {
shimniok 0:a6a169de725f 68 pos->set( (y / latToY) + latZero,
shimniok 0:a6a169de725f 69 (x / lonToX) + lonZero);
shimniok 0:a6a169de725f 70
shimniok 0:a6a169de725f 71 return;
shimniok 0:a6a169de725f 72 }
shimniok 0:a6a169de725f 73
shimniok 0:a6a169de725f 74 void Mapping::cartToGeo(CartPosition cart, GeoPosition *pos)
shimniok 0:a6a169de725f 75 {
shimniok 19:ce7fdade3534 76 cartToGeo(cart.x, cart.y, pos);
shimniok 0:a6a169de725f 77
shimniok 0:a6a169de725f 78 return;
shimniok 0:a6a169de725f 79 }
shimniok 0:a6a169de725f 80