Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Estimation/Mapping/Mapping.cpp@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

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