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/GeoPosition/GeoPosition.h@25:bb5356402687, 2018-11-30 (annotated)
- Committer:
- shimniok
- Date:
- Fri Nov 30 16:11:53 2018 +0000
- Revision:
- 25:bb5356402687
- Parent:
- 0:a6a169de725f
Initial publish of revised version.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:a6a169de725f | 1 | #ifndef __GEOPOSITION_H |
shimniok | 0:a6a169de725f | 2 | #define __GEOPOSITION_H |
shimniok | 0:a6a169de725f | 3 | |
shimniok | 0:a6a169de725f | 4 | #ifndef _PI |
shimniok | 0:a6a169de725f | 5 | #define _PI 3.141592653 |
shimniok | 0:a6a169de725f | 6 | #endif |
shimniok | 0:a6a169de725f | 7 | |
shimniok | 0:a6a169de725f | 8 | #define degrees(x) ((x)*180/_PI) |
shimniok | 0:a6a169de725f | 9 | #define radians(x) ((x)*_PI/180) |
shimniok | 0:a6a169de725f | 10 | |
shimniok | 0:a6a169de725f | 11 | /** Geographical position and calculation. Most of this comes from http://www.movable-type.co.uk/scripts/latlong.html |
shimniok | 0:a6a169de725f | 12 | * |
shimniok | 0:a6a169de725f | 13 | */ |
shimniok | 0:a6a169de725f | 14 | class GeoPosition { |
shimniok | 0:a6a169de725f | 15 | public: |
shimniok | 0:a6a169de725f | 16 | |
shimniok | 0:a6a169de725f | 17 | /** Create a new emtpy position object |
shimniok | 0:a6a169de725f | 18 | * |
shimniok | 0:a6a169de725f | 19 | */ |
shimniok | 0:a6a169de725f | 20 | GeoPosition(); |
shimniok | 0:a6a169de725f | 21 | |
shimniok | 0:a6a169de725f | 22 | /** Create a new position with the specified latitude and longitude. See set() |
shimniok | 0:a6a169de725f | 23 | * |
shimniok | 0:a6a169de725f | 24 | * @param latitude is the latitude to set |
shimniok | 0:a6a169de725f | 25 | * @param longitude is the longitude to set |
shimniok | 0:a6a169de725f | 26 | */ |
shimniok | 0:a6a169de725f | 27 | GeoPosition(double latitude, double longitude); |
shimniok | 0:a6a169de725f | 28 | |
shimniok | 0:a6a169de725f | 29 | /** Get the position's latitude |
shimniok | 0:a6a169de725f | 30 | * |
shimniok | 0:a6a169de725f | 31 | * @returns the position's latitude |
shimniok | 0:a6a169de725f | 32 | */ |
shimniok | 0:a6a169de725f | 33 | double latitude(); |
shimniok | 0:a6a169de725f | 34 | |
shimniok | 0:a6a169de725f | 35 | /** Get the position's longitude |
shimniok | 0:a6a169de725f | 36 | * |
shimniok | 0:a6a169de725f | 37 | * @returns the position's longitude |
shimniok | 0:a6a169de725f | 38 | */ |
shimniok | 0:a6a169de725f | 39 | double longitude(); |
shimniok | 0:a6a169de725f | 40 | |
shimniok | 0:a6a169de725f | 41 | /** Set the position's location to another position's coordinates |
shimniok | 0:a6a169de725f | 42 | * |
shimniok | 0:a6a169de725f | 43 | * @param pos is another position from which coordinates will be copied |
shimniok | 0:a6a169de725f | 44 | */ |
shimniok | 0:a6a169de725f | 45 | void set(GeoPosition pos); |
shimniok | 0:a6a169de725f | 46 | |
shimniok | 0:a6a169de725f | 47 | /** Set the position's location to the specified coordinates |
shimniok | 0:a6a169de725f | 48 | * |
shimniok | 0:a6a169de725f | 49 | * @param latitude is the new latitude to set |
shimniok | 0:a6a169de725f | 50 | * @param longitude is the new longitude to set |
shimniok | 0:a6a169de725f | 51 | */ |
shimniok | 0:a6a169de725f | 52 | void set(double latitude, double longitude); |
shimniok | 0:a6a169de725f | 53 | |
shimniok | 0:a6a169de725f | 54 | /** Move the location of the position by the specified distance and in |
shimniok | 0:a6a169de725f | 55 | * the specified direction |
shimniok | 0:a6a169de725f | 56 | * |
shimniok | 0:a6a169de725f | 57 | * @param course is the direction of movement in degrees, absolute not relative |
shimniok | 0:a6a169de725f | 58 | * @param distance is the distance of movement along the specified course in meters |
shimniok | 0:a6a169de725f | 59 | */ |
shimniok | 0:a6a169de725f | 60 | void move(float course, float distance); |
shimniok | 0:a6a169de725f | 61 | |
shimniok | 0:a6a169de725f | 62 | /** Get the bearing from the specified origin position to this position. To get |
shimniok | 0:a6a169de725f | 63 | * relative bearing, subtract the result from your heading. |
shimniok | 0:a6a169de725f | 64 | * |
shimniok | 0:a6a169de725f | 65 | * @param from is the position from which to calculate bearing |
shimniok | 0:a6a169de725f | 66 | * @returns the bearing in degrees |
shimniok | 0:a6a169de725f | 67 | */ |
shimniok | 0:a6a169de725f | 68 | float bearing(GeoPosition from); |
shimniok | 0:a6a169de725f | 69 | |
shimniok | 0:a6a169de725f | 70 | float bearingFrom(GeoPosition from); |
shimniok | 0:a6a169de725f | 71 | |
shimniok | 0:a6a169de725f | 72 | float bearingTo(GeoPosition to); |
shimniok | 0:a6a169de725f | 73 | |
shimniok | 0:a6a169de725f | 74 | /** Get the distance from the specified origin position to this position |
shimniok | 0:a6a169de725f | 75 | * |
shimniok | 0:a6a169de725f | 76 | * @param from is the position from which to calculate distance |
shimniok | 0:a6a169de725f | 77 | * @returns the distance in meters |
shimniok | 0:a6a169de725f | 78 | */ |
shimniok | 0:a6a169de725f | 79 | float distance(GeoPosition from); |
shimniok | 0:a6a169de725f | 80 | |
shimniok | 0:a6a169de725f | 81 | float distanceTo(GeoPosition to); |
shimniok | 0:a6a169de725f | 82 | |
shimniok | 0:a6a169de725f | 83 | float distanceFrom(GeoPosition from); |
shimniok | 0:a6a169de725f | 84 | |
shimniok | 0:a6a169de725f | 85 | /** Set an arbitrary timestamp on the position |
shimniok | 0:a6a169de725f | 86 | * |
shimniok | 0:a6a169de725f | 87 | * @param timestamp is an integer timestamp, eg., seconds, milliseconds, or whatever |
shimniok | 0:a6a169de725f | 88 | */ |
shimniok | 0:a6a169de725f | 89 | void setTimestamp(int time); |
shimniok | 0:a6a169de725f | 90 | |
shimniok | 0:a6a169de725f | 91 | /** Return a previously set timestamp on the position |
shimniok | 0:a6a169de725f | 92 | * |
shimniok | 0:a6a169de725f | 93 | * @returns the timestamp (e.g., seconds, milliseconds, etc.) |
shimniok | 0:a6a169de725f | 94 | */ |
shimniok | 0:a6a169de725f | 95 | int getTimestamp(void); |
shimniok | 0:a6a169de725f | 96 | |
shimniok | 0:a6a169de725f | 97 | private: |
shimniok | 0:a6a169de725f | 98 | double _R; /** Earth's mean radius */ |
shimniok | 0:a6a169de725f | 99 | double _latitude; /** The position's latitude */ |
shimniok | 0:a6a169de725f | 100 | double _longitude; /** The position's longitude */ |
shimniok | 0:a6a169de725f | 101 | double _northing; /** The position's UTM northing coordinate */ |
shimniok | 0:a6a169de725f | 102 | double _easting; /** The position's UTM easting coordinate */ |
shimniok | 0:a6a169de725f | 103 | int _time; /** Timestamp */ |
shimniok | 0:a6a169de725f | 104 | }; |
shimniok | 0:a6a169de725f | 105 | |
shimniok | 0:a6a169de725f | 106 | #endif |