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
- Committer:
- shimniok
- Date:
- 2018-11-30
- Revision:
- 25:bb5356402687
- Parent:
- 19:ce7fdade3534
File content as of revision 25:bb5356402687:
#include "mbed.h" // debugging #include "Mapping.h" /* * Provide a series of lat/lon coordinates/waypoints, and the object * automatically generates a bounding rectangle, and determines how to * translate between lat/lon and cartesian x/y */ void Mapping::init(int count, GeoPosition *p) { double latMin = 360; double latMax = -360; double lonMin = 360; double lonMax = -360; // Find minimum and maximum lat/lon for (int i=0; i < count; i++, p++) { //fprintf(stdout, "%d (%.6f, %.6f)\n", i, p->latitude(), p->longitude()); if (p->latitude() > latMax) latMax = p->latitude(); if (p->latitude() < latMin) latMin = p->latitude(); if (p->longitude() > lonMax) lonMax = p->longitude(); if (p->longitude() < lonMin) lonMin = p->longitude(); } // Set the arbitrary cartesian origin to southwest corner lonZero = lonMin; latZero = latMin; //fprintf(stdout, "min: (%.6f, %.6f)\n", latMin, lonMin); //fprintf(stdout, "max: (%.6f, %.6f)\n", latMax, lonMax); // Three positions required to scale GeoPosition sw(latMin, lonMin); GeoPosition nw(latMax, lonMin); GeoPosition se(latMin, lonMax); // Find difference between lat and lon min/max float dlat = (latMax - latMin); float dlon = (lonMax - lonMin); //fprintf(stdout, "dlat=%.6f dlon=%.6f\n", dlat, dlon); // Compute scale based on distances between edges of rectangle // and difference between min/max lat and min/max lon lonToX = sw.distanceTo(se) / dlon; latToY = sw.distanceTo(nw) / dlat; //fprintf(stdout, "lonToX=%.10f\nlatToY=%.10f\n", lonToX, latToY); return; } void Mapping::geoToCart(GeoPosition pos, CartPosition *cart) { cart->set( lonToX * (pos.longitude() - lonZero), latToY * (pos.latitude() - latZero) ); return; } void Mapping::cartToGeo(float x, float y, GeoPosition *pos) { pos->set( (y / latToY) + latZero, (x / lonToX) + lonZero); return; } void Mapping::cartToGeo(CartPosition cart, GeoPosition *pos) { cartToGeo(cart.x, cart.y, pos); return; }