Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 0:bec310bde899, committed 2011-09-27
- Comitter:
- pd0wm
- Date:
- Tue Sep 27 19:46:30 2011 +0000
- Commit message:
Changed in this revision
diff -r 000000000000 -r bec310bde899 MODGPS/ChangeLog.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS/ChangeLog.c Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,92 @@ +/* + +1.0 - 11/11/2010 - Initial release + +1.1 - 11/11/2010 - + + * Made some documentation corrections. + +1.2 - 11/11/2010 + + * Added user API callback options for teh following signals:- + 1PPS + NMEA RMC message processed + NMEA GGA message processed + + * Added example1.cpp and example2.cpp to demonstrate how to + setup and use various features/functions. + +1.3 - 11/11/2010 + + * Increased the frequency at which process_request flag to checked. + + * Included the baud() and format() methods which are just passed onto + the parent object, Serial. + + * Updated the example1.cpp and example2.cpp to include more details. + +1.4 - 13/10/2010 + + * Changed the name to MODGPS as "GPS" was too generic. + + * Improved the example1.cpp program with a lot more details + on the usage of the module. + +1.5 - 14/11/2010 + + * Added more cookbook documentation. + +1.6 - 14/11/2010 + + * Added more doxyden API comments. + +1.7 - 14/10/2010 + + * Added a lot more doxygen comments for teh API group. + +1.8 - 14/10/2010 + + * Added a lot more doxygen comments for the API group. + +1.9 - 15/11/2010 + + * Added @ingroup API to getGPSquality() + +1.10 - 17/11/2010 + + * Improved doxygen documentation + +1.11 - 17/11/2010 + + * Mbed's systems don't like @mainpage, rolling back + +1.12 - 15/04/2011 + + * Added VTG sentence (vectors speed and direction) + See example3.cpp + +1.13 - 16/04/2011 + + * Found that concat commas in NMEA sentences (indicating an empty field) + caused strtok() to return pointers in incorrect positions. + * Found copying geodetic and vtg data structs to temp buffers actually + didn't work properly. + +1.14 - 16/04/2011 + + * Doh, left in some debugging code which causes compile fail outside + of the test harness. + +1.15 - 20/04/2011 + + * Added the new feature that allows the library to set the Mbed RTC + to the GPS/UTC time. This ensures the RTC is kept up to date with + the latest GPS aquired time (syncs once each minute). + +1.16 - 21/04/2011 + + * Added the ability for an application to request a copy of a NMEA sentence + before it gets processed (mangled). + See setRmc(), setGga(), setVtg() and setUkn(). + +*/
diff -r 000000000000 -r bec310bde899 MODGPS/GPS.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS/GPS.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,236 @@ +/* + Copyright (c) 2010 Andy Kirkham + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "GPS.h" + +GPS::GPS(PinName tx, PinName rx, const char *name) : Serial(tx, rx, name) +{ + _nmeaOnUart0 = false; + + _lastByte = 0; + + _gga = (char *)NULL; + + _rmc = (char *)NULL; + + _vtg = (char *)NULL; + + switch(_uidx) { + case 1: _base = LPC_UART1; break; + case 2: _base = LPC_UART2; break; + case 3: _base = LPC_UART3; break; + default : _base = NULL; break; + } + + _pps = NULL; + _ppsInUse = false; + + if (_base != NULL) attach(this, &GPS::rx_irq); + + _second100 = new Ticker; + _second100->attach_us(this, &GPS::ticktock, GPS_TICKTOCK); +} + +void +GPS::ppsAttach(PinName irq, ppsEdgeType type) +{ + if (_pps != NULL) delete(_pps); + _pps = new InterruptIn(irq); + if (type == ppsRise) _pps->rise(this, &GPS::pps_irq); + else _pps->fall(this, &GPS::pps_irq); + _ppsInUse = true; +} + +void +GPS::ppsUnattach(void) +{ + if (_pps != NULL) delete(_pps); + _ppsInUse = false; +} + +double +GPS::latitude(void) +{ + double a, b; + do { a = thePlace.lat; b = thePlace.lat; } while (a != b); + return a; +} + +double +GPS::longitude(void) +{ + double a, b; + do { a = thePlace.lon; b = thePlace.lon; } while (a != b); + return a; +} + +double +GPS::altitude(void) +{ + double a, b; + do { a = thePlace.alt; b = thePlace.alt; } while (a != b); + return a; +} + +GPS_Geodetic * +GPS::geodetic(GPS_Geodetic *q) +{ + GPS_Geodetic a; + + if (q == NULL) q = new GPS_Geodetic; + + do { + memcpy(&a, &thePlace, sizeof(GPS_Geodetic)); + memcpy(q, &thePlace, sizeof(GPS_Geodetic)); + } + while (memcmp(&a, q, sizeof(GPS_Geodetic)) != 0); + + return q; +} + +GPS_VTG * +GPS::vtg(GPS_VTG *q) +{ + GPS_VTG a; + + if (q == NULL) q = new GPS_VTG; + + do { + memcpy(&a, &theVTG, sizeof(GPS_VTG)); + memcpy(q, &theVTG, sizeof(GPS_VTG)); + } + while (memcmp(&a, q, sizeof(GPS_VTG)) != 0); + + return q; +} + +void +GPS::ticktock(void) +{ + int i; + + // Increment the time structure by 1/100th of a second. + ++theTime; + + // Test the serial queue. + if (process_required) { + char *s = buffer[active_buffer == 0 ? 1 : 0]; + if (!strncmp(s, "$GPRMC", 6)) { + if (_rmc) { + for(i = 0; s[i] != '\n'; i++) { + _rmc[i] = s[i]; + } + _rmc[i++] = '\n'; _rmc[i] = '\0'; + } + theTime.nmea_rmc(s); + cb_rmc.call(); + if (!_ppsInUse) theTime.fractionalReset(); + } + else if (!strncmp(s, "$GPGGA", 6)) { + if (_gga) { + for(i = 0; s[i] != '\n'; i++) { + _gga[i] = s[i]; + } + _gga[i++] = '\n'; _gga[i] = '\0'; + } + thePlace.nmea_gga(s); + cb_gga.call(); + } + else if (!strncmp(s, "$GPVTG", 6)) { + if (_vtg) { + for(i = 0; s[i] != '\n'; i++) { + _vtg[i] = s[i]; + } + _vtg[i++] = '\n'; _vtg[i] = '\0'; + } + theVTG.nmea_vtg(s); + cb_vtg.call(); + } + else { + if (_ukn) { + for(i = 0; s[i] != '\n'; i++) { + _ukn[i] = s[i]; + } + _ukn[i++] = '\n'; _ukn[i] = '\0'; + cb_ukn.call(); + } + } + process_required = false; + } + + // If we have a valid GPS time then, once per minute, set the RTC. + if (theTime.status == 'A' && theTime.second == 0 && theTime.tenths == 0 && theTime.hundreths == 0) { + // set_time() is defined in rtc_time.h + // http://mbed.org/projects/libraries/svn/mbed/trunk/rtc_time.h + set_time(theTime.to_C_tm()); + } + +} + +void +GPS::pps_irq(void) +{ + theTime.fractionalReset(); + theTime++; // Increment the time/date by one second. + cb_pps.call(); +} + +void +GPS::rx_irq(void) +{ + uint32_t iir __attribute__((unused)); + char c; + + if (_base) { + iir = (uint32_t)*((char *)_base + GPS_IIR); + while((int)(*((char *)_base + GPS_LSR) & 0x1)) { + c = (char)(*((char *)_base + GPS_RBR) & 0xFF); + + // strtok workaround. + // Found that ,, together (which some NMEA sentences + // contain for a null/empty field) confuses strtok() + // function. Solution:- Push a "zero" into the string + // for missing/empty fields. + if (c == ',' && _lastByte == ',') { + buffer[active_buffer][rx_buffer_in] = '0'; + if (++rx_buffer_in >= GPS_BUFFER_LEN) rx_buffer_in = 0; + } + + // Debugging/dumping data. + if (_nmeaOnUart0) LPC_UART0->RBR = c; + + // Put the byte into the string. + buffer[active_buffer][rx_buffer_in] = c; + if (++rx_buffer_in >= GPS_BUFFER_LEN) rx_buffer_in = 0; + + // Save for next time an irq occurs. See strtok() above. + _lastByte = c; + + // If end of NMEA sentence flag for processing. + if (c == '\n') { + active_buffer = active_buffer == 0 ? 1 : 0; + process_required = true; + rx_buffer_in = 0; + } + } + } +}
diff -r 000000000000 -r bec310bde899 MODGPS/GPS.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS/GPS.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,849 @@ +/* + Copyright (c) 2010 Andy Kirkham + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef GPS_H +#define GPS_H + +#include "mbed.h" +#include "GPS_VTG.h" +#include "GPS_Time.h" +#include "GPS_Geodetic.h" + +#define GPS_RBR 0x00 +#define GPS_THR 0x00 +#define GPS_DLL 0x00 +#define GPS_IER 0x04 +#define GPS_DML 0x04 +#define GPS_IIR 0x08 +#define GPS_FCR 0x08 +#define GPS_LCR 0x0C +#define GPS_LSR 0x14 +#define GPS_SCR 0x1C +#define GPS_ACR 0x20 +#define GPS_ICR 0x24 +#define GPS_FDR 0x28 +#define GPS_TER 0x30 + +#define GPS_BUFFER_LEN 128 +#define GPS_TICKTOCK 10000 + +/** @defgroup API The MODGPS API */ + +/** GPS module + * @author Andy Kirkham + * @see http://mbed.org/cookbook/MODGPS + * @see example1.cpp + * @see example2.cpp + * @see API + * + * @image html /media/uploads/AjK/gps_interfaces.png "Wiring up the GPS module" + * + * Example: + * @code + * #include "mbed.h" + * #include "GPS.h" + * + * DigitalOut led1(LED1); + * Serial pc(USBTX, USBRX); + * GPS gps(NC, p10); + * + * int main() { + * GPS_Time t; + * + * // Wait for the GPS NMEA data to become valid. + * while (!gps.isTimeValid()) { + * led1 = !led1; + * wait(1); + * } + * + * gps.timeNow(&t); + * + * pc.printf("The time/date is %02d:%02d:%02d %02d/%02d/%04d\r\n", + * t.hour, t.minute, t.second, t.day, t.month, t.year); + * + * // Wait until at least four satellites produce a position fix and a valid quality. + * while (gps.numOfSats() < 4 && gps.getGPSquality != 0) { + * led1 = !led1; + * wait(1); + * } + * + * pc.printf("Lat = %.4f Lon = %.4f Alt = %.1fkm\r\n", + * gps.latitude(), gps.longitude, gps.altitude()); + * + * // Make the LED go steady to indicate we have finished. + * led1 = 1; + * + * while(1) {} + * } + * @endcode + */ + +class GPS : Serial { +public: + + //! The PPS edge type to interrupt on. + enum ppsEdgeType { + ppsRise = 0, /*!< Use the rising edge (default). */ + ppsFall /*!< Use the falling edge. */ + }; + + //! A copy of the Serial parity enum + enum Parity { + None = 0 + , Odd + , Even + , Forced1 + , Forced0 + }; + + //! GPS constructor. + /** + * The GPS constructor is used to initialise the GPS object. + * + * @param tx Usually unused and set to NC + * @param rx The RX pin the GPS is connected to, p10, p14( OR p25), p27. + * @param name An option name for RPC usage. + */ + GPS(PinName tx, PinName rx, const char *name = NULL); + + //! Is the time reported by the GPS valid. + /** + * Method used to check the validity of the time the GPS module is reporting. + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * if (gps.isTimeValid()) { + * // Time is valid :) + * } + * else { + * // Doh, time is not valid :( + * ) + * + * @endcode + * + * @ingroup API + * @return bool true if valid, false otherwise + */ + bool isTimeValid(void) { return theTime.status == 'V' ? false : true; } + + //! Is the positional fix reported by the GPS valid. + /** + * Method used to check the validity of the positional data. This method + * returns the GGA field, 0 is "bad, 1 is "ok", etc. See the NMEA GGA + * description for more details. + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * if (gps.getGPSquality() == 0) { + * // The location fix is no good/not accurate :( + * } + * else { + * // All good, can use last fix data. + * ) + * + * @endcode + * + * @ingroup API + * @return int 0 on no fix, 1... (see NMEA GGA for more details). + */ + int getGPSquality(void) { return thePlace.getGPSquality(); } + + //! How many satellites were used in the last fix. + /** + * Method returns the number of GPS satellites used on the last fix. + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * int sats = gps.numOfSats(); + * + * @endcode + * + * @ingroup API + * @return int The number of satellites. + */ + int numOfSats(void) { return thePlace.numOfSats(); } + + //! What was the last reported latitude (in degrees) + /** + * Method returns a double in degrees, positive being North, negative being South. + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * double latitude = gps.latitude(); + * + * @endcode + * + * @ingroup API + * @return double Degrees + */ + double latitude(void); + + //! What was the last reported longitude (in degrees) + /** + * Method returns a double in degrees, positive being East, negative being West. + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * double logitude = gps.logitude(); + * + * @endcode + * + * @ingroup API + * @return double Degrees + */ + double longitude(void); + + //! What was the last reported altitude (in kilometers) + /** + * Method returns a double in kilometers. + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * double altitude = gps.altitude(); + * + * @endcode + * + * @ingroup API + * @return double Kilometers + */ + double altitude(void); + + //! What was the last reported altitude/height (in kilometers) + /** + * @see altitude() + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * double height = gps.height(); + * + * @endcode + * + * Note, this is identical to altitude() + * @see altitude() + * + * @ingroup API + * @return double Kilometers + */ + double height(void) { return altitude(); } + + //! Get all vector parameters together. + /** + * Pass a pointer to a GPS_VTG object and the current + * GPS data will be copied into it. + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * // Then get the data... + * GPS_VTG p; + * gps.vtg(&p); + * printf("Speed (knots) = %.4f", p.velocity_knots); + * printf("Speed (kps) = %.4f", p.velocity_kps); + * printf("Track (true) = %.4f", p.track_true); + * printf("Track (mag) = %.4f", p.track_mag); + * + * @endcode + * + * @ingroup API + * @param g A GSP_VTG pointer to an existing GPS_VTG object. + * @return GPS_VTG * The pointer passed in. + */ + GPS_VTG *vtg(GPS_VTG *g); + + //! Get all vector parameters together. + /** + * Get all the vector data at once. For example:- + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * // Then get the data... + * GPS_VTG *p = gps.vtg(); + * printf("Speed (knots) = %.4f", p->velocity_knots); + * printf("Speed (kps) = %.4f", p->velocity_kps); + * printf("Track (true) = %.4f", p->track_true); + * printf("Track (mag) = %.4f", p->track_mag); + * delete(p); // then remember to delete the object to prevent memory leaks. + * + * @endcode + * + * @ingroup API + * @return GPS_Geodetic * A pointer to the data. + */ + GPS_VTG *vtg(void) { return vtg(NULL); } + + //! Get all three geodetic parameters together. + /** + * Pass a pointer to a GPS_Geodetic object and the current + * GPS data will be copied into it. + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * // Then get the data... + * GPS_Geodetic p; + * gps.geodetic(&p); + * printf("Latitude = %.4f", p.lat); + * printf("Longitude = %.4f", p.lon); + * printf("Altitude = %.4f", p.alt); + * + * @endcode + * + * @ingroup API + * @param g A GSP_Geodetic pointer to an existing GPS_Geodetic object. + * @return GPS_Geodetic * The pointer passed in. + */ + GPS_Geodetic *geodetic(GPS_Geodetic *g); + + //! Get all three geodetic parameters together. + /** + * Get all the geodetic data at once. For example:- + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * // Then get the data... + * GPS_Geodetic *p = gps.geodetic(); + * printf("Latitude = %.4f", p->lat); + * delete(p); // then remember to delete the object to prevent memory leaks. + * + * @endcode + * + * @ingroup API + * @return GPS_Geodetic * A pointer to the data. + */ + GPS_Geodetic *geodetic(void) { return geodetic(NULL); } + + //! Take a snap shot of the current time. + /** + * Pass a pointer to a GPS_Time object to get a copy of the current + * time and date as reported by the GPS. + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * // Then get the data... + * GPS_Time t; + * gps.timeNow(&t); + * printf("Year = %d", t.year); + * + * @endcode + * + * @ingroup API + * @param n A GPS_Time * pointer to an existing GPS_Time object. + * @return GPS_Time * The pointer passed in. + */ + GPS_Time * timeNow(GPS_Time *n) { return theTime.timeNow(n); } + + //! Take a snap shot of the current time. + /** + * Pass a pointer to a GPS_Time object to get a copy of the current + * time and date as reported by the GPS. + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * // Then get the data... + * GPS_Time *t = gps.timeNow(); + * printf("Year = %d", t->year); + * delete(t); // Avoid memory leaks. + * + * @endcode + * + * @ingroup API + * @return GPS_Time * The pointer passed in. + */ + GPS_Time * timeNow(void) { GPS_Time *n = new GPS_Time; return theTime.timeNow(n); } + + //! Return the curent day. + /** + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * // Then get the Julain Day Number. + * double julianDayNumber = gps.julianDayNumber(); + * + * @endcode + * + * @ingroup API + * @return double The Julian Date as a double. + */ + double julianDayNumber(void) { return theTime.julian_day_number(); } + + //! Return the curent date/time as a Julian date + /** + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * // Then get the Julian Date. + * double julianDate = gps.julianDate(); + * + * @endcode + * + * @ingroup API + * @return double The Julian Date as a double. + */ + double julianDate(void) { return theTime.julian_date(); } + + //! Get the current sidereal degree angle. + /** + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * double sidereal = gps.siderealDegrees(); + * + * @endcode + * + * @ingroup API + * @return double Sidereal degree angle.. + */ + double siderealDegrees(void) { return theTime.siderealDegrees(&theTime, longitude()); } + + //! Get the current sidereal hour angle. + /** + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * double sidereal = gps.siderealHA(); + * + * @endcode + * + * @ingroup API + * @return double Sidereal degree angle.. + */ + double siderealHA(void) { return theTime.siderealHA(&theTime, longitude()); } + + //! Optionally, connect a 1PPS single to an Mbed pin. + /** + * Optional: If the GPS unit has a 1PPS output, use this to + * connect that to our internal ISR. Using the 1PPS increases + * the GPS_Time time accuracy from +/-0.25s to +/-0.001s + * + * @code + * // Assuming we have a GPS object previously created... + * GPS gps(NC, p9); + * + * gps.ppsAttach(p29); // default to GPS::ppsRise, rising edge. + * + * // Or... + * gps.ppsAttach(p29, GPS::ppsRise); // The default. + * + * // Or... + * gps.ppsAttach(p29, GPS::ppsFall); // If a falling edge. + * + * @endcode + * + * <b>Note</b>, before using this function you should attach an actual + * callback function using attach_pps() + * + * @see attach_pps() + * + * @ingroup API + * @param irq A PinName to attach + * @param type The type of edge, MAX7456::ppsRise OR MAX7456::ppsFall + */ + void ppsAttach(PinName irq, ppsEdgeType type = ppsRise); + + //! Remove any 1PPS signal previously attached. + void ppsUnattach(void); + + //! GPS serial receive interrupt handler. + void rx_irq(void); + + //! GPS pps interrupt handler. + void pps_irq(void); + + //! A pointer to the UART peripheral base address being used. + void *_base; + + //! The RX serial buffer. + char buffer[2][GPS_BUFFER_LEN]; + + //! The current "active" buffer, i.e. the buffer the ISR is writing to. + int active_buffer; + + //! The active buffer "in" pointer. + int rx_buffer_in; + + //! Boolean flag set when the "passive" buffer is full and needs processing. + bool process_required; + + //! 10ms Ticker callback. + void ticktock(void); + + //! Attach a user object/method callback function to the PPS signal + /** + * Attach a user callback object/method to call when the 1PPS signal activates. + * + * @code + * class FOO { + * public: + * void myCallback(void); + * }; + * + * GPS gps(NC, p9); + * Foo foo; + * + * gps.attach_pps(foo, &FOO::myCallback); + * + * @endcode + * + * @ingroup API + * @param tptr pointer to the object to call the member function on + * @param mptr pointer to the member function to be called + */ + template<typename T> + void attach_pps(T* tptr, void (T::*mptr)(void)) { cb_pps.attach(tptr, mptr); } + + //! Attach a user callback function to the PPS signal + /** + * Attach a user callback function pointer to call when the 1PPS signal activates. + * + * @code + * void myCallback(void) { ... } + * + * GPS gps(NC, p9); + * Foo foo; + * + * gps.attach_pps(&myCallback); + * + * @endcode + * + * @ingroup API + * @param fptr Callback function pointer + */ + void attach_pps(void (*fptr)(void)) { cb_pps.attach(fptr); } + + //! A callback object for the 1PPS user API. + FunctionPointer cb_pps; + + //! Attach a user callback function to the NMEA RMC message processed signal. + /** + * Attach a user callback object/method to call when an NMEA RMC packet has been processed. + * + * @code + * class FOO { + * public: + * void myCallback(void); + * }; + * + * GPS gps(NC, p9); + * Foo foo; + * + * gps.attach_rmc(foo, &FOO::myCallback); + * + * @endcode + * + * @ingroup API + * @param tptr pointer to the object to call the member function on + * @param mptr pointer to the member function to be called + */ + template<typename T> + void attach_rmc(T* tptr, void (T::*mptr)(void)) { cb_rmc.attach(tptr, mptr); } + + //! Attach a user callback function to the NMEA RMC message processed signal. + /** + * Attach a user callback function pointer to call when an NMEA RMC packet has been processed. + * + * @code + * void myCallback(void) { ... } + * + * GPS gps(NC, p9); + * Foo foo; + * + * gps.attach_rmc(&myCallback); + * + * @endcode + * + * @ingroup API + * @param fptr Callback function pointer. + */ + void attach_rmc(void (*fptr)(void)) { cb_rmc.attach(fptr); } + + //! A callback object for the NMEA RMS message processed signal user API. + FunctionPointer cb_rmc; + + //! Attach a user callback function to the NMEA GGA message processed signal. + /** + * Attach a user callback object/method to call when an NMEA GGA packet has been processed. + * + * @code + * class FOO { + * public: + * void myCallback(void); + * }; + * + * GPS gps(NC, p9); + * Foo foo; + * + * gps.attach_gga(foo, &FOO::myCallback); + * + * @endcode + * + * @ingroup API + * @param tptr pointer to the object to call the member function on + * @param mptr pointer to the member function to be called + */ + template<typename T> + void attach_gga(T* tptr, void (T::*mptr)(void)) { cb_gga.attach(tptr, mptr); } + + //! Attach a user callback function to the NMEA GGA message processed signal. + /** + * Attach a user callback function pointer to call when an NMEA GGA packet has been processed. + * + * @code + * void myCallback(void) { ... } + * + * GPS gps(NC, p9); + * Foo foo; + * + * gps.attach_gga(&myCallback); + * + * @endcode + * + * @ingroup API + * @param fptr Callback function pointer. + */ + void attach_gga(void (*fptr)(void)) { cb_gga.attach(fptr); } + + //! A callback object for the NMEA GGA message processed signal user API. + FunctionPointer cb_gga; + + + //! Attach a user callback function to the NMEA VTG message processed signal. + /** + * Attach a user callback object/method to call when an NMEA VTG packet has been processed. + * + * @code + * class FOO { + * public: + * void myCallback(void); + * }; + * + * GPS gps(NC, p9); + * Foo foo; + * + * gps.attach_vtg(foo, &FOO::myCallback); + * + * @endcode + * + * @ingroup API + * @param tptr pointer to the object to call the member function on + * @param mptr pointer to the member function to be called + */ + template<typename T> + void attach_vtg(T* tptr, void (T::*mptr)(void)) { cb_vtg.attach(tptr, mptr); } + + //! Attach a user callback function to the NMEA VTG message processed signal. + /** + * Attach a user callback function pointer to call when an NMEA VTG packet has been processed. + * + * @code + * void myCallback(void) { ... } + * + * GPS gps(NC, p9); + * Foo foo; + * + * gps.attach_vtg(&myCallback); + * + * @endcode + * + * @ingroup API + * @param fptr Callback function pointer. + */ + void attach_vtg(void (*fptr)(void)) { cb_vtg.attach(fptr); } + + //! A callback object for the NMEA RMS message processed signal user API. + FunctionPointer cb_vtg; + + //! Attach a user callback function to the unknown NMEA message. + /** + * Attach a user callback object/method to call when an unknown NMEA packet. + * + * @code + * class FOO { + * public: + * void myCallback(void); + * }; + * + * GPS gps(NC, p9); + * Foo foo; + * + * gps.attach_ukn(foo, &FOO::myCallback); + * + * @endcode + * + * @ingroup API + * @param tptr pointer to the object to call the member function on + * @param mptr pointer to the member function to be called + */ + template<typename T> + void attach_ukn(T* tptr, void (T::*mptr)(void)) { cb_ukn.attach(tptr, mptr); } + + //! Attach a user callback function to the unknown NMEA message. + /** + * Attach a user callback function pointer to call when an unknown NMEA. + * + * @code + * void myCallback(void) { ... } + * + * GPS gps(NC, p9); + * Foo foo; + * + * gps.attach_ukn(&myCallback); + * + * @endcode + * + * @ingroup API + * @param fptr Callback function pointer. + */ + void attach_ukn(void (*fptr)(void)) { cb_ukn.attach(fptr); } + + //! A callback object for the NMEA RMS message processed signal user API. + FunctionPointer cb_ukn; + + /** + * Set's the GGA string memory pointer. + * @param s char pointer ti string. + * @return char s passed in. + */ + char * setGga(char *s) { _gga = s; return s; } + + /** + * Set's the RMC string memory pointer. + * @param s char pointer ti string. + * @return char s passed in. + */ + char * setRmc(char *s) { _rmc = s; return s; }; + + /** + * Set's the VTG string memory pointer. + * @param s char pointer ti string. + * @return char s passed in. + */ + char * setVtg(char *s) { _vtg = s; return s; }; + + /** + * Set's the UKN string memory pointer. + * @param s char pointer ti string. + * @return char s passed in. + */ + char * setUkn(char *s) { _ukn = s; return s; }; + + //! Set the baud rate the GPS module is using. + /** + * Set the baud rate of the serial port + * + * @see http://mbed.org/projects/libraries/api/mbed/trunk/Serial#Serial.baud + * + * @ingroup API + * @param baudrate The baudrate to set. + */ + void baud(int baudrate) { Serial::baud(baudrate); } + + //! Set the serial port format the GPS module is using. + /** + * Set the transmission format used by the Serial port + * + * @see http://mbed.org/projects/libraries/api/mbed/trunk/Serial#Serial.format + * + * @ingroup API + * @param bits - The number of bits in a word (5-8; default = 8) + * @param parity - The parity used (GPS::None, GPS::Odd, GPS::Even, GPS::Forced1, GPS::Forced0; default = GPS::None) + * @param stop_bits - The number of stop bits (1 or 2; default = 1) + */ + void format(int bits, Parity parity, int stop_bits) { Serial::format(bits, (Serial::Parity)parity, stop_bits); } + + //! Send incoming GPS bytes to Uart0 + /** + * Send incoming GPS bytes to Uart0 + * + * This can be useful for printing out the bytes from the GPS onto + * a the common debug port Uart0. Note, Uart0 should have been setup + * and initialised before switching this on. Also, realistically, + * you should ensure Uart0 has a higher baud rate than that being + * used by the GPS. Sending of bytes to Uart0 is "raw" and should + * only be used to initially gather data and should NOT be used as + * part of the application design. If you need to forward on the + * data you should come up with a proper strategy. + * + * @ingroup API + * @param b - True to send to Uart0, false otherwise + */ + void NmeaOnUart0(bool b) { _nmeaOnUart0 = b; } + +protected: + + //! Flag set true when a GPS PPS has been attached to a pin. + bool _ppsInUse; + + //! An InterruptIn object to "trigger" on the PPS edge. + InterruptIn *_pps; + + //! A Ticker object called every 10ms. + Ticker *_second100; + + //! A GPS_Time object used to hold the last parsed time/date data. + GPS_Time theTime; + + //! A GPS_Geodetic object used to hold the last parsed positional data. + GPS_Geodetic thePlace; + + //! A GPS_VTG object used to hold vector data. + GPS_VTG theVTG; + + //! Used to record the previous byte received. + char _lastByte; + + char *_gga; + char *_rmc; + char *_vtg; + char *_ukn; + + //! Used for debugging. + bool _nmeaOnUart0; +}; + +#endif +
diff -r 000000000000 -r bec310bde899 MODGPS/GPS_Geodetic.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS/GPS_Geodetic.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,117 @@ +/* + Copyright (c) 2010 Andy Kirkham + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "GPS_Geodetic.h" + +void +GPS_Geodetic::nmea_gga(char *s) { + char *token; + int token_counter = 0; + char *latitude = (char *)NULL; + char *longitude = (char *)NULL; + char *lat_dir = (char *)NULL; + char *lon_dir = (char *)NULL; + char *qual = (char *)NULL; + char *altitude = (char *)NULL; + char *sats = (char *)NULL; + + int nw = 0; + int n = 0; + + while (s[n] != '\n' && n < 128){ + if (s[n] == 'N' || s[n] == 'S' || s[n] == 'E' || s[n] == 'W') + nw++; + n++; + } + + token = strtok(s, ","); + while (token) { + switch (token_counter) { + case 2: latitude = token; break; + case 4: longitude = token; break; + case 3: lat_dir = token; break; + case 5: lon_dir = token; break; + case 6: qual = token; break; + case 7: sats = token; break; + case 9: altitude = token; break; + } + token = strtok((char *)NULL, ","); + token_counter++; + } + + // If the fix quality is valid set our location information. + if (latitude && longitude && altitude && sats && nw == 2) { + lat = convert_lat_coord(latitude, lat_dir[0]); + lon = convert_lon_coord(longitude, lon_dir[0]); + alt = convert_height(altitude); + num_of_gps_sats = atoi(sats); + gps_satellite_quality = atoi(qual); + } + else { + gps_satellite_quality = 0; + //lat = 0; + //lon = 0; + } +} + +double +GPS_Geodetic::convert_lat_coord(char *s, char north_south) +{ + int deg, min, sec; + double fsec, val; + + deg = ( (s[0] - '0') * 10) + s[1] - '0'; + min = ( (s[2] - '0') * 10) + s[3] - '0'; + sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0')); + fsec = (double)((double)sec /10000.0); + val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0); + if (north_south == 'S') { val *= -1.0; } + lat = val; + return val; +} + +double +GPS_Geodetic::convert_lon_coord(char *s, char east_west) +{ + int deg, min, sec; + double fsec, val; + + deg = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0'); + min = ( (s[3] - '0') * 10) + s[4] - '0'; + sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0')); + fsec = (double)((double)sec /10000.0); + val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0); + if (east_west == 'W') { val *= -1.0; } + lon = val; + return val; + +} + +double +GPS_Geodetic::convert_height(char *s) +{ + double val = (double)(atof(s) / 1000.0); + alt = val; + return val; +} + +
diff -r 000000000000 -r bec310bde899 MODGPS/GPS_Geodetic.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS/GPS_Geodetic.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,55 @@ +/* + Copyright (c) 2010 Andy Kirkham + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef GPS_GEODETIC_H +#define GPS_GEODETIC_H + +#include "mbed.h" + +/** GPS_Geodetic definition. + */ +class GPS_Geodetic { +public: + + //! double The latitude + double lat; + + //! double The longitude + double lon; + + //! double The altitude + double alt; + + int num_of_gps_sats; + int gps_satellite_quality; + GPS_Geodetic() { lat = 0.0; lon = 0.0; alt = 0.0; } + + int numOfSats(void) { return num_of_gps_sats; } + int getGPSquality(void) { return gps_satellite_quality; } + void nmea_gga(char *s); + double convert_lat_coord(char *s, char north_south); + double convert_lon_coord(char *s, char east_west); + double convert_height(char *s); +}; + +#endif +
diff -r 000000000000 -r bec310bde899 MODGPS/GPS_Time.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS/GPS_Time.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,230 @@ +/* + Copyright (c) 2010 Andy Kirkham + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "GPS_Time.h" + +GPS_Time::GPS_Time() +{ + year = 2000; + month = 1; + day = 1; + hour = 0; + minute = 0; + second = 0; + tenths = 0; + hundreths = 0; + status = 'V'; + velocity = 0; + track = 0; + magvar_dir = 'W'; + magvar = 0; +} + +time_t +GPS_Time::to_C_tm(bool set) +{ + GPS_Time t; + tm ct; + time_t q; + + timeNow(&t); + ct.tm_sec = t.second; + ct.tm_min = t.minute; + ct.tm_hour = t.hour; + ct.tm_mday = t.day; + ct.tm_mon = t.month - 1; + ct.tm_year = t.year - 1900; + ct.tm_isdst = 0; // GPS has no understanding of DST. + + q = mktime(&ct); + if (set) { + set_time(q); + } + return q; +} + +GPS_Time * +GPS_Time::timeNow(GPS_Time *n) +{ + if (n == NULL) n = new GPS_Time; + + do { + memcpy(n, this, sizeof(GPS_Time)); + } + while (memcmp(n, this, sizeof(GPS_Time))); + return n; +} + +void +GPS_Time::operator++() +{ + hundreths++; + if (hundreths == 10) { + hundreths = 0; + tenths++; + if (tenths == 10) { + tenths = hundreths = 0; + } + } +} + +void +GPS_Time::operator++(int) +{ + const int days[12] = { 31,28,31,30,31,30,31,31,30,31,30,31 }; + bool dateInc = false; + + tenths = hundreths = 0; + second++; + + if (second == 60) { + second = 0; + minute++; + if (minute == 60) { + minute = 0; + hour++; + if (hour == 24) { + hour = 0; + dateInc = true; + } + } + } + + if (dateInc) { + /* Handle February leap year. */ + int leap_year = ((year % 4 == 0 && year % 100 != 0) || year % 400 == 0) ? 1 : 0; + int days_this_month = days[month - 1]; + if (month == 2 && leap_year) days_this_month++; + day++; + if (day > days_this_month) { + day = 1; + month++; + if (month == 13) { + year++; + } + } + } +} + +// $GPRMC,112709.735,A,5611.5340,N,00302.0306,W,000.0,307.0,150411,,,A*70 +void +GPS_Time::nmea_rmc(char *s) +{ + char *token; + int token_counter = 0; + char *time = (char *)NULL; + char *date = (char *)NULL; + char *stat = (char *)NULL; + char *vel = (char *)NULL; + char *trk = (char *)NULL; + char *magv = (char *)NULL; + char *magd = (char *)NULL; + + token = strtok(s, ","); + while (token) { + switch (token_counter) { + case 9: date = token; break; + case 1: time = token; break; + case 2: stat = token; break; + case 7: vel = token; break; + case 8: trk = token; break; + case 10: magv = token; break; + case 11: magd = token; break; + } + token = strtok((char *)NULL, ","); + token_counter++; + } + + if (stat && date && time) { + hour = (char)((time[0] - '0') * 10) + (time[1] - '0'); + minute = (char)((time[2] - '0') * 10) + (time[3] - '0'); + second = (char)((time[4] - '0') * 10) + (time[5] - '0'); + day = (char)((date[0] - '0') * 10) + (date[1] - '0'); + month = (char)((date[2] - '0') * 10) + (date[3] - '0'); + year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000; + status = stat[0]; + velocity = atof(vel); + track = atof(trk); + magvar = atof(magv); + magvar_dir = magd[0]; + } +} + +double +GPS_Time::julian_day_number(GPS_Time *t) { + double wikipedia_jdn = (double)(1461 * ((int)t->year + 4800 + ((int)t->month - 14) / 12)) / 4 + (367 * ((int)t->month - 2 - 12 * (((int)t->month - 14) / 12))) / 12 - (3 * (((int)t->year + 4900 + ((int)t->month - 14) / 12 ) / 100)) / 4 + (int)t->day - 32075; + return wikipedia_jdn; +} + +double +GPS_Time::julian_date(GPS_Time *t) { + double hour, minute, second, jd; + hour = (double)t->hour; + minute = (double)t->minute; + second = (double)t->second + ((double)t->tenths / 10.) + ((double)t->hundreths / 100.); + + jd = julian_day_number(t) - 0.5 + + ((hour - 12.) / 24.) + + (minute / 1440.) + + (second / 86400.); + + return jd; +} + +double +GPS_Time::siderealDegrees(double jd, double longitude) { + double sidereal, gmst, lmst; + double T = jd - 2451545.0; + double T1 = T / 36525.0; + double T2 = T1 * T1; + double T3 = T2 * T1; + + /* Calculate gmst angle. */ + sidereal = 280.46061837 + (360.98564736629 * T) + (0.000387933 * T2) - (T3 / 38710000.0); + + /* Convert to degrees. */ + sidereal = fmod(sidereal, 360.0); + if (sidereal < 0.0) sidereal += 360.0; + + gmst = sidereal; + lmst = gmst + longitude; + return lmst; +} + +double +GPS_Time::siderealDegrees(GPS_Time *t, double longitude) { + if (t == NULL) t = new GPS_Time; + return siderealDegrees(julian_date(t), longitude); +} + +double +GPS_Time::siderealHA(double jd, double longitude) { + double lmst = siderealDegrees(jd, longitude); + return lmst / 360.0 * 24.0; +} + +double +GPS_Time::siderealHA(GPS_Time *t, double longitude) { + double lmst = siderealDegrees(t, longitude); + return lmst / 360.0 * 24.0; +} +
diff -r 000000000000 -r bec310bde899 MODGPS/GPS_Time.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS/GPS_Time.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,85 @@ +/* + Copyright (c) 2010 Andy Kirkham + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef GPS_TIME_H +#define GPS_TIME_H + +#include "mbed.h" + +/** GPS_Time definition. + */ +class GPS_Time { +public: + + //! The year + int year; + //! The month + int month; + //! The day + int day; + //! The hour + int hour; + //! The minute + int minute; + //! The second + int second; + //! Tenths of a second + int tenths; + //! Hundredths of a second + int hundreths; + //! Time status. + char status; + //! The velocity (in knots) + double velocity; + //! The track (in decimal degrees true) + double track; + //! The magnetic variation direction + char magvar_dir; + //! The magnetic variation value + double magvar; + + GPS_Time(); + void fractionalReset(void) { tenths = hundreths = 0; } + void operator++(); + void operator++(int); + GPS_Time * timeNow(GPS_Time *n); + GPS_Time * timeNow(void) { return timeNow(NULL); } + void nmea_rmc(char *s); + double velocity_knots(void) { return velocity; } + double velocity_kph(void) { return (velocity * 1.852); } + double velocity_mps(void) { return velocity_kph() / 3600.0; } + double velocity_mph(void) { return velocity_kph() / 0.621371192; } + double track_over_ground(void) { return track; } + double magnetic_variation(void) { return magvar_dir == 'W' ? (magvar * -1.0) : (magvar); } + double julian_day_number(GPS_Time *t); + double julian_date(GPS_Time *t); + double julian_day_number(void) { return julian_day_number(this); } + double julian_date(void) { return julian_date(this); } + double siderealDegrees(double jd, double longitude); + double siderealDegrees(GPS_Time *t, double longitude); + double siderealHA(double jd, double longitude); + double siderealHA(GPS_Time *t, double longitude); + time_t to_C_tm(bool set = false); +}; + +#endif +
diff -r 000000000000 -r bec310bde899 MODGPS/GPS_VTG.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS/GPS_VTG.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,74 @@ +/* + Copyright (c) 2010 Andy Kirkham + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "GPS_VTG.h" +#include <math.h> + +GPS_VTG::GPS_VTG() +{ + _velocity_knots = 0; + _velocity_kph = 0; + _track_true = 0; + _track_mag = 0; +} + +GPS_VTG * +GPS_VTG::vtg(GPS_VTG *n) +{ + if (n == NULL) n = new GPS_VTG; + + n->_velocity_knots = _velocity_knots; + n->_velocity_kph = _velocity_kph; + n->_track_true = _track_true; + n->_track_mag = _track_mag; + + return n; +} + +void +GPS_VTG::nmea_vtg(char *s) +{ + char *token; + int token_counter = 0; + char *vel_knots = (char *)NULL; + char *vel_kph = (char *)NULL; + char *trk_t = (char *)NULL; + char *trk_m = (char *)NULL; + + token = strtok(s, ","); + while (token) { + switch (token_counter) { + case 5: vel_knots = token; break; + case 7: vel_kph = token; break; + case 1: trk_t = token; break; + case 3: trk_m = token; break; + } + token = strtok((char *)NULL, ","); + token_counter++; + } + + if (trk_t) { _track_true = atof(trk_t); } + if (trk_m) { _track_mag = atof(trk_m); } + if (vel_knots) { _velocity_knots = atof(vel_knots); } + if (vel_kph) { _velocity_kph = atof(vel_kph); } +} +
diff -r 000000000000 -r bec310bde899 MODGPS/GPS_VTG.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS/GPS_VTG.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,54 @@ +/* + Copyright (c) 2010 Andy Kirkham + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef GPS_VTG_H +#define GPS_VTG_H + +#include "mbed.h" + +/** GPS_Time definition. + */ +class GPS_VTG { +public: + + //! The velocity (in knots) + double _velocity_knots; + //! The velocity (in kph) + double _velocity_kph; + //! The track (in decimal degrees true) + double _track_true; + //! The track (in decimal degrees magnetic) + double _track_mag; + + GPS_VTG(); + GPS_VTG * vtg(GPS_VTG *n); + void nmea_vtg(char *s); + + double velocity_knots(void) { return _velocity_knots; } + double velocity_kph(void) { return _velocity_kph; } + double track_true(void) { return _track_true; } + double track_mag(void) { return _track_mag; } + +}; + +#endif +
diff -r 000000000000 -r bec310bde899 MODGPS/info.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS/info.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,13 @@ +/* + +Typical NMEA sentences from my GPS module. + +$GPGGA,075851.891,5611.5305,N,00302.0369,W,0,00,4.8,44.0,M,52.0,M,,0000*77 +$GPGSA,A,1,,,,,,,,,,,,,4.8,4.8,0.7*37 +$GPGSV,3,1,12,20,82,116,,01,79,246,,32,54,077,,17,48,254,*70 +$GPGSV,3,2,12,23,46,168,,24,40,128,,04,25,295,,11,24,143,*73 +$GPGSV,3,3,12,31,22,065,,13,15,190,,12,11,343,,25,00,019,*7D +$GPRMC,075851.891,V,5611.5305,N,00302.0369,W,002.2,252.9,160411,,,N*68 +$GPVTG,252.9,T,,M,002.2,N,004.1,K,N*0B + +*/ \ No newline at end of file
diff -r 000000000000 -r bec310bde899 invoer/gps_wrapper.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/invoer/gps_wrapper.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,49 @@ +#include "gps_wrapper.h" + +Gps::Gps(GPS * g){ + gps = g; + gps->baud(9600); + gps->format(8, GPS::None, 1); + gps->setGga(gga); + +} + +void Gps::set(pos* new_pos){ + positie.lat = new_pos->lat; + positie.lon = new_pos->lon; +} + +pos Gps::get(void){ + positie.lat = gps->latitude(); + positie.lon = gps->longitude(); + return positie; +} + +char* Gps::getgga(void){ + return gga; +} +void Gps::simuleer(double koers, double afstand){ + + koers *= deg2rad; + afstand /= 6371.0; + + double lat1 = positie.lat * deg2rad; + double lon1 = positie.lon * deg2rad; + + double dlat = afstand * cos(koers); + double lat2 = dlat + lat1; + + double df = log(tan(lat2/2.0 + PI/4.0)/tan(lat1/2.0 + PI/4.0)); + double q; + if (df == 0) + q = cos(lat1); + else + q = dlat / df; + + double dlon = afstand * sin(koers) / q; + + double lon2 = fmod((lon1 + dlon + PI),(2.0 * PI)) - PI; + + positie.lat = lat2 * rad2deg; + positie.lon = lon2 * rad2deg; +}
diff -r 000000000000 -r bec310bde899 invoer/gps_wrapper.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/invoer/gps_wrapper.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,26 @@ +#ifndef GPS_WRAPPER_H +#define GPS_WRAPPER_H + +#include "mbed.h" +#include "pws.h" +#include "math.h" +#include "GPS.h" +#include <string> + +class Gps{ + pos positie; + GPS * gps; + + + public: + char gga[128]; + Gps(GPS *); + void set(pos *); + pos get(void); + void simuleer(double,double); + char* getgga(void); + +}; + + +#endif
diff -r 000000000000 -r bec310bde899 invoer/kompas.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/invoer/kompas.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,119 @@ +#include "kompas.h" + +Kompas::Kompas():compass(p9, p10) { + koers = 0; + prev = 0; + cal = 0; + + wait(0.5); + + char compass_out[3]; + compass_out[0] = 0xF1; + compass_out[1] = 0x05; + compass_out[1] = 0x02; + compass.write(ADDR,compass_out,3); + wait_ms(1); + refresh.attach(this,&Kompas::update,0.1); + + + +} + +void Kompas::update(void) { + //wat variabeles om als zend en ontvang buffer te fungeren + char compass_out[1]; + char compass_in[6]; + int i; + + //als het kompas niet bezig met calibreren kan er gelezen worden + if (cal == 0) { + + //zet 0x50(="sample heading") in zend buffer + compass_out[0] = 0x50; + //stuur deze buffer over de bus + compass.write(ADDR,compass_out,1); + + //geef het kompas even de tijd om de opdracht uit te voeren + wait_ms(1); + + //Daarna kunnen er 6 8bits waardes uitgelezen worden + compass.read(ADDR,compass_in,6); + + + //Deze 6 8 bits waardes moeten omgerekend worden naar 3 16 bits waardes + int compass_heading = (int)compass_in[0] << 8 | (int)compass_in[1]; + int compass_pitch = (int)compass_in[2] << 8 | (int)compass_in[3]; + int compass_roll = (int)compass_in[4] << 8 | (int)compass_in[5]; + int n = compass_heading / 10; + + + int verschil = n - prev; + if (verschil > 180) + verschil -= 360; + if (verschil < -180) + verschil += 360; + + if (abs(verschil) > 60) { + if (verschil > 0) + prev += 20; + else + prev -= 20; + } else { + + + //n is ok, nu gemiddelde berekenen + + //buffer doorschuiven + for (i = 0;i != 9; i++) + opslag[i] = opslag[i+1]; + + opslag[9] = n; + + //gemiddelde berekenen + int som = 0; + for (i = 0;i != 10; i++) + som += opslag[i]; + + koers = (som / 20); + koers *= 2; + + + + + } + + + } else + koers = 0; +} +int Kompas::get(void) { + return koers; + +} + +void Kompas::simulate(int inc) { + koers += inc; + if (koers > 360) + koers -= 360; + if (koers < 0) + koers += 360; +} + +void Kompas::startcal(void) { + char compass_out[1]; + if (cal == 0) { + cal = 1; + compass_out[0] = 0x71; + compass.write(ADDR,compass_out,1); + } +} + +void Kompas::stopcal(void) { + char compass_out[1]; + if (cal == 1) { + compass_out[0] = 0x7E; + compass.write(ADDR,compass_out,1); + cal = 0; + } +} +
diff -r 000000000000 -r bec310bde899 invoer/kompas.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/invoer/kompas.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,26 @@ +#ifndef KOMPAS_H +#define KOMPAS_H + +#include "mbed.h" +#include <math.h> + +#define ADDR 0x32 + +class Kompas{ + + I2C compass; // sda, scl + int cal; + int koers,prev; + Ticker refresh; + int opslag[10]; + public: + Kompas(); + int get(void); + void simulate(int); + void update(void); + void startcal(void); + void stopcal(void); +}; + + +#endif
diff -r 000000000000 -r bec310bde899 invoer/vaantje.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/invoer/vaantje.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,14 @@ +#include "vaantje.h" + +Vaantje::Vaantje(){ + hoek = 180; +} + +int Vaantje::get(void){ + return hoek; +} + +void Vaantje::set(int h){ + hoek = h; +} +
diff -r 000000000000 -r bec310bde899 invoer/vaantje.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/invoer/vaantje.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,11 @@ +#ifndef VAANTJE_H +#define VAANTJE_H +class Vaantje{ + int hoek; + public: + Vaantje(); + int get(void); + void set(int); +}; + +#endif
diff -r 000000000000 -r bec310bde899 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,36 @@ +#include "mbed.h" + +#include "pws.h" +#include "roer.h" +#include "zeil.h" +#include "kompas.h" +#include "status.h" +#include "gps_wrapper.h" +#include "route.h" +#include "vaantje.h" +#include "pid.h" + + +DigitalOut myled(LED1); +GPS g(NC, p27); +Gps gps(&g); +Kompas kompas; +Roer roer; + +Route route(&gps); +Vaantje vaantje; +Zeil zeil; +Goto got(&gps,&route,&kompas,&vaantje); +Pid pid(&got,&kompas,&roer); +Status status(&roer,&zeil, &kompas, &gps, &vaantje, &route, &got, &pid); + + +int main() { + while (1) { + myled = 1; + wait(0.5); + myled = 0; + wait(0.5); + + } +}
diff -r 000000000000 -r bec310bde899 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912
diff -r 000000000000 -r bec310bde899 other/pws.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/other/pws.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,12 @@ +#include "pws.h" + +double verschil(double van, double naar){ + double error = naar - van; + + if (error > 180) + error -= 360; + if (error < -180) + error += 360; + + return error; +}
diff -r 000000000000 -r bec310bde899 other/pws.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/other/pws.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,16 @@ +#ifndef PWS_H +#define PWS_H + + +#define PI 3.14159265 +#define deg2rad PI / 180.0 +#define rad2deg 180.0 / PI + +struct pos{ + double lat; + double lon; +}; + +double verschil(double,double); + +#endif
diff -r 000000000000 -r bec310bde899 status/status.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/status/status.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,329 @@ +#include "status.h" + + +Status::Status(Roer * r, Zeil * z, Kompas * k, Gps * g, Vaantje * v, Route * rout, Goto * gt, Pid * p) : zender(p13,p14) { + roer = r; + zeil = z; + kompas = k; + gps = g; + route = rout; + vaantje = v; + got = gt; + pid = p; + + manual = 1; + zeilmanual = 1; + + zender.baud(115200); + zender.attach(this, &Status::newData); +} + +void Status::autoZeil(void) { + double hoek = fabs(verschil((double)kompas->get(), (double)vaantje->get())); + int s; + if (hoek <= 40) + s = 35; + else if (hoek > 40 && hoek <= 100) + s = (hoek - 40) * (55.0/60.0) + 35; + else + s = 90; + zeil->set(s); +} + +void Status::newData(void) { + + //buffer variables + char lezen; + + //if data is ready in the buffer + while (zender.readable()) { + //read 1 character + lezen = zender.getc(); + + //if character is $ than it is the start of a sentence + if (lezen == '$') { + //so the pointer should be set to the first position + b.clear(); + + } + //write buffer character to big buffer string + b += lezen; + + //if the character is # than the end of the sentence is reached and some stuff has to be done + if (lezen == '#') { + + char * cstr, *p; + string c1,c2,c3,c4; + int set = 1; + int n; + float f; + + //start en stop karakters afhakken + b.erase(0,1); + b.erase(b.length()-1,1); + + cstr = new char [b.size()+1]; + strcpy (cstr, b.c_str()); + + p = strtok(cstr," "); + if (p != NULL) + c1 = p; + p = strtok(NULL," "); + if (p != NULL) + c2 = p; + p = strtok(NULL," "); + if (p != NULL) + c3 = p; + else + set = 0; + + p = strtok(NULL," "); + if (p != NULL) + c4 = p; + + delete[] cstr; + + if (c1 == "s") { + zender.printf("%d %d %d %f %f %d %f %f %d\n\r",roer->get(),zeil->get(),kompas->get(),gps->get().lat,gps->get().lon,route->getActive(),route->afstand_tot_doel(), route->koers_tot_doel(),zeilmanual); + + } + + else if (c1 == "roer") { + if (c2 == "set") { + if (set) { + sscanf (c3.c_str(),"%d",&n); + roer->set(n); + } + } else if (c2 == "get") { + if (!set) { + zender.printf("%d\n\r",roer->get()); + } + } + } else if (c1 == "zeil") { + if (c2 == "set") { + if (set) { + sscanf (c3.c_str(),"%d",&n); + zeil->set(n); + } + } else if (c2 == "get") { + if (!set) { + zender.printf("%d\n\r",zeil->get()); + } + } + + else if (c2 == "auto") { + if (set) { + sscanf (c3.c_str(),"%d",&n); + if (n == 1) { + zeiltimer.attach(this,&Status::autoZeil,0.1); + zeilmanual = 0; + } + if (n == 0) { + zeiltimer.detach(); + zeilmanual = 1; + + } + } + if (!set) { + zender.printf("%d\n\r",zeilmanual); + + } + } + } else if (c1 == "pid") { + if (c2 == "enable") { + if (set) { + sscanf (c3.c_str(),"%d",&n); + if (n == 1) { + timer.attach(pid,&Pid::update,0.1); + manual = 0; + } + if (n == 0) { + timer.detach(); + manual = 1; + + } + } + if (!set) { + zender.printf("%d\n\r",manual); + + } + + } else if (c2 == "info") { + if (!set) { + zender.printf("%d %f %f %f %f %f %f %f %f %f %d %d %d\n\r",manual,pid->getKP(),pid->getKI(),pid->getKD(),pid->getP(),pid->getI(),pid->getD(),pid->getE(),pid->getSP(),pid->getPV(),pid->getO(),route->getLoopMode(),got->getminhoek()); + + } + + } else if (c2 == "setp") { + if (set) { + sscanf (c3.c_str(),"%f",&f); + pid->setKp(f); + + + } + } else if (c2 == "seti") { + if (set) { + sscanf (c3.c_str(),"%f",&f); + pid->setKi(f); + + + } + } else if (c2 == "setd") { + if (set) { + sscanf (c3.c_str(),"%f",&f); + pid->setKd(f); + + + } + + } else if (c2 == "manual") { + if (set) { + sscanf (c3.c_str(),"%d",&n); + int m = n; + sscanf (c4.c_str(),"%d",&n); + + got->manual(m,n); + + + } + } + + } else if (c1 == "kompas") { + if (c2 == "get") { + if (!set) { + zender.printf("%d\n\r",kompas->get()); + } + } + if (c2 == "cal") { + if (set) { + sscanf (c3.c_str(),"%d",&n); + if (n) { + kompas->startcal(); + } else { + kompas->stopcal(); + } + } + } + } else if (c1 == "vaantje") { + if (c2 == "set") { + if (set) { + sscanf (c3.c_str(),"%d",&n); + vaantje->set(n); + } + } + } else if (c1 == "gps") { + if (c2 == "gga") { + if (!set) { + zender.printf("%s",gps->getgga()); + } + } else if (c2 == "lat") { + if (!set) { + zender.printf("%f",gps->get().lat); + } + } else if (c2 == "lon") { + if (!set) { + zender.printf("%f",gps->get().lon); + } + } + } else if (c1 == "route") { + if (c2 == "list") { + if (!set) { + for (int x = 0; x!=route->getlen(); x++) { + zender.printf("%d) %f %f\r\n",x,route->read(x).lat,route->read(x).lon); + } + zender.printf("OK\r\n"); + + } + } else if (c2 == "add") { + if (set) { + float la; + float lo; + + + sscanf(c3.c_str(),"%f", &la); + + sscanf(c4.c_str(),"%f", &lo); + + + pos t; + t.lat = la; + t.lon = lo; + + route->add(t); + } + } else if (c2 == "del") { + if (set) { + sscanf(c3.c_str(),"%d", &n); + route->del(n); + + } + + } else if (c2 == "up") { + if (set) { + sscanf(c3.c_str(),"%d", &n); + route->up(n); + + } + + } else if (c2 == "down") { + if (set) { + sscanf(c3.c_str(),"%d", &n); + route->down(n); + + } + + } else if (c2 == "act") { + if (set) { + sscanf(c3.c_str(),"%d", &n); + route->setActive(n); + + } + if (!set) { + zender.printf("%d\r\n",route->getActive()); + + } + + } else if (c2 == "dist") { + if (!set) { + zender.printf("%f\r\n",route->afstand_tot_doel()); + + } + } else if (c2 == "koers") { + if (!set) { + zender.printf("%f\r\n",route->koers_tot_doel()); + + } + + } else if (c2 == "minhoek") { + if (set) { + sscanf(c3.c_str(),"%d", &n); + got->setminhoek(n); + + } + } else if (c2 == "hys") { + if (set) { + sscanf(c3.c_str(),"%d", &n); + got->sethys(n); + + } + } else if (c2 == "loop") { + if (set) { + sscanf(c3.c_str(),"%d", &n); + route->loopMode(n); + + } + + } + + + + + + } + + } + + } + + +}
diff -r 000000000000 -r bec310bde899 status/status.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/status/status.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,51 @@ +#ifndef STATUS_H +#define STATUS_H + +#include <string> + +#include "roer.h" +#include "zeil.h" +#include "kompas.h" +#include "gps_wrapper.h" +#include "route.h" +#include "pws.h" +#include "vaantje.h" +#include "goto.h" +#include "pid.h" + +#include "mbed.h" + +class Status{ + + Serial zender; + Ticker timer; + Ticker zeiltimer; + + Roer * roer; + Zeil * zeil; + Kompas * kompas; + Gps * gps; + Route * route; + Vaantje * vaantje; + Goto * got; + Pid * pid; + + string b; + int plaats; + int manual; + int zeilmanual; + + + + void newData(void); + + + + public: + Status(Roer *, Zeil *, Kompas *, Gps *, Vaantje *, Route *, Goto *, Pid *); + void autoZeil(void); + + +}; + +#endif \ No newline at end of file
diff -r 000000000000 -r bec310bde899 uitvoer/roer.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/uitvoer/roer.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,27 @@ +#include "roer.h" + +Roer::Roer():led(LED2),rudder(p21) { + stand = 50; + this->write(); +} + +void Roer::set(int a) { + stand = a; + if (stand > 76) + stand = 76; + if (stand < 5) + stand = 5; + + + this->write(); +} + +int Roer::get(void) { + return stand; +} + +void Roer::write(void){ + led = stand / 100.0; + rudder.pulsewidth(0.001 + stand / 100000.0); +} + \ No newline at end of file
diff -r 000000000000 -r bec310bde899 uitvoer/roer.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/uitvoer/roer.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,20 @@ +#ifndef ROER_H +#define ROER_H + +#include "mbed.h" + +class Roer{ + int stand; + PwmOut led; + PwmOut rudder; + + + public: + Roer(); + + void set(int); + int get(void); + void write(void); + +}; +#endif
diff -r 000000000000 -r bec310bde899 uitvoer/zeil.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/uitvoer/zeil.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,35 @@ +#include "zeil.h" + +Zeil::Zeil() : led(LED3),sail(p22) { + + stand = 50; + this->write(); +} + +void Zeil::set(int s) { + stand = s; + + if (stand > 100) + stand = 100; + if (stand < 0) + stand = 0; + + this->write(); + + +} + + + + +int Zeil::get(void) { + return stand; +} + +void Zeil::write(void){ + led = stand / 100.0; + sail.pulsewidth(0.001 + stand / 100000.0); +} + + +
diff -r 000000000000 -r bec310bde899 uitvoer/zeil.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/uitvoer/zeil.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,20 @@ +#ifndef ZEIL_H +#define ZEIL_H + +#include "vaantje.h" +#include "mbed.h" + +class Zeil{ + int stand; + + PwmOut led; + PwmOut sail; + + public: + Zeil(); + void set(int); + //void set(); + int get(void); + void write(void); +}; +#endif
diff -r 000000000000 -r bec310bde899 verwerking/goto.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/verwerking/goto.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,106 @@ +#include "goto.h" + + +Goto::Goto(Gps * g, Route * r, Kompas * k, Vaantje * v){ + huidig = g; + doel = r; + kompas = k; + vaantje = v; + + opkruisen = 0; + eerste = 1; + reset = 0; + rak = 0; + holdcourse = 0; + course = 0; + minhoek = 35; + hys = 5; + +} +void Goto::manual(int m, int k){ + holdcourse = m; + course = k; +} + + +double Goto::get(void){ + if (holdcourse) + return course; + + double k = koers(&huidig->get(),&doel->get()); //koers naar doel berekenen + double d = afstand(&huidig->get(),&doel->get()); //afstand nar doel berekenen + + + double ware_wind = vaantje->get(); //ware wind uit vaantje halen + + if (ware_wind > 360) + ware_wind -= 360; + + if (fabs(verschil(k,ware_wind)) < minhoek + hys){ //kijken of er moet worden opgekruist + if (!opkruisen){ //kijken of er niet al opgekruist wordt + begin_opkruisen = huidig->get(); //huidige positie opslaan als begin van het + // opkruisen + begin_rak = huidig->get(); //en als begin van het huidige rak + koers_begin = k; //de koers van de layline opslaan + eerste = 1; //installen dat het eerste rak is + } + else{ + reset = 0; + } + opkruisen = 1; + } + if (fabs(verschil(k,ware_wind)) > minhoek + hys){ //kijken of het al bezeild is + opkruisen = 0; + eerste = 0; + } + if (!opkruisen){ //als er niet opgekruist hoeft te worden + return k; //direct naar het doel varen + } + else //anders: + { + double boeg1 = ware_wind + (double)minhoek; //twee koersen uitrekenen + double boeg2 = ware_wind - (double)minhoek; + if (boeg1 > 360) + boeg1 -= 360; + if (boeg2 < 0) + boeg2 += 360; + + double lengte = afstand(&begin_opkruisen, &doel->get()) / 5.0; //lengte van de rakken + // uitrekenen + double a = fabs(verschil(ware_wind,koers_begin)); + double lengte1 = lengte / sin(((double)minhoek+a) * deg2rad); + double lengte2 = lengte / sin(((double)minhoek-a) * deg2rad); + + if (eerste){ + //eerste rak 2x zo kort + lengte1 /= 2.0; + lengte2 /= 2.0; + //bij eerste rak kiezen welke koers het dichste bij de huidige licht + if (fabs(verschil(boeg1,kompas->get())) < fabs(verschil(boeg2,kompas->get()))) + boeg = 1; + else + boeg = 2; + } + if (boeg == 1){ + if (afstand(&begin_rak,&huidig->get()) > lengte1){ //als het einde van het rak bereikt is + eerste = 0; + boeg = 2; //ga overstag + begin_rak = huidig->get(); + } + return boeg1; + } + if (boeg == 2){ + if (afstand(&begin_rak,&huidig->get()) > lengte2){ + eerste = 0; + boeg = 1; + begin_rak = huidig->get(); + } + return boeg2; + } + } +} + + + + + \ No newline at end of file
diff -r 000000000000 -r bec310bde899 verwerking/goto.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/verwerking/goto.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,48 @@ +#ifndef GOTO_H +#define GOTO_H + +#include "pws.h" +#include "gps_wrapper.h" +#include "route.h" +#include "vaantje.h" +#include "kompas.h" +#include "rhumb.h" +#include <math.h> + +class Goto{ + Gps * huidig; + Route * doel; + Vaantje * vaantje; + Kompas * kompas; + + int opkruisen; + int eerste; + int reset; + int rak; + int hys; + + pos begin_opkruisen; + pos begin_rak; + double koers_begin; + int boeg; + + int holdcourse; + int course; + + int minhoek; + + public: + Goto(Gps *, Route *, Kompas *, Vaantje *); + double get(void); + void manual(int,int); + void setminhoek(int m){minhoek = m;} + void sethys(int h){hys = h;} + int getminhoek(void){return minhoek;} + +}; + + + + + +#endif
diff -r 000000000000 -r bec310bde899 verwerking/pid.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/verwerking/pid.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,40 @@ +#include "pid.h" + +Pid::Pid(Goto * p, Kompas * k, Roer * r){ + sp = p; + pv = k; + output = r; + dt = 0.1; + + kp = 1.2; + ki = 1; + kd = 0.1; + + prev_error = 0; +} + +void Pid::update(void){ + double error = verschil(pv->get(),sp->get()); + + p = error * kp; + i = i + error * dt; + if (i > 25) + i = 25; + if (i < -25) + i = -25; + i *= ki; + + d = (error - prev_error)/dt * kd; + o = p+i+d + 50; + + if (o > 100) + o = 100; + if (0 < 0) + o = 0; + + + output->set(o); + prev_error = error; + + +}
diff -r 000000000000 -r bec310bde899 verwerking/pid.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/verwerking/pid.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,41 @@ +#ifndef PID_H +#define PID_H + +#include "pws.h" +#include "kompas.h" +#include "goto.h" +#include "roer.h" + +class Pid{ + double p,i,d; + double kp,ki,kd; + double dt; + double prev_error; + int o; + + Goto * sp; + Kompas * pv; + Roer * output; + + public: + Pid (Goto *, Kompas *, Roer *); + + void setKp(double k){kp = k;} + void setKi(double k){ki = k;} + void setKd(double k){kd = k;} + + double getP(void){return p;} + double getI(void){return i;} + double getD(void){return d;} + double getKP(void){return kp;} + double getKI(void){return ki;} + double getKD(void){return kd;} + double getE(void){return prev_error;} + double getSP(void){return sp->get();} + double getPV(void){return pv->get();} + int getO(void){return o;} + + void update(void); +}; + +#endif \ No newline at end of file
diff -r 000000000000 -r bec310bde899 verwerking/rhumb.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/verwerking/rhumb.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,54 @@ +#include "rhumb.h" +#include <math.h> + + +double afstand(pos *cur, pos *tar){ + double lat_cur_r = cur->lat * deg2rad; + double lon_cur_r = cur->lon * deg2rad; + double lat_tar_r = tar->lat * deg2rad; + double lon_tar_r = tar->lon * deg2rad; + + double df = log(tan(lat_tar_r/2.0 + PI/4.0)/tan(lat_cur_r/2.0 + PI/4.0)); + + + double dla = lat_tar_r - lat_cur_r; + double dlo = lon_tar_r - lon_cur_r; + + double q; + + if (df == 0) + q = cos(lat_cur_r); + else + q = dla / df; + + + double r = 6371.0; + + + double d = sqrt(pow(dla,2) + pow(q,2) * pow(dlo,2)) * r; + return d; + + +} + +double koers(pos *cur, pos *tar){ + double lat_cur_r = cur->lat * deg2rad; + double lon_cur_r = cur->lon * deg2rad; + double lat_tar_r = tar->lat * deg2rad; + double lon_tar_r = tar->lon * deg2rad; + + double df = log(tan(lat_tar_r/2.0 + PI/4.0)/tan(lat_cur_r/2.0 + PI/4.0)); + + + double dla = lat_tar_r - lat_cur_r; + double dlo = lon_tar_r - lon_cur_r; + + + double k = atan2(dlo,df) * rad2deg; + if (k < 0) + k += 360; + + return k; + + +}
diff -r 000000000000 -r bec310bde899 verwerking/rhumb.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/verwerking/rhumb.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,10 @@ +#ifndef RHUMB_H +#define RHUMB_H +//#include "route.h" +#include "pws.h" + +double afstand(pos *, pos *); +double koers(pos * , pos *); + + +#endif
diff -r 000000000000 -r bec310bde899 verwerking/route.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/verwerking/route.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,110 @@ +#include "route.h" + +Route::Route(Gps * g){ + loop = 0; + len = 0; + active = 0; + gps = g; + klaar = 0; +} + +void Route::loopMode(int l){ + loop = l; +} + +int Route::getLoopMode(void){ + return loop; +} + +void Route::setActive(int index){ + active = index; +} +int Route::getActive(void){ + return active; +} + + +void Route::add(pos nieuw){ + waypoints[len] = nieuw; + len++; +} + +void Route::del(int index){ + if (index < len && index >= 0){ + for (int x = index;x < len; x++){ + waypoints[x] = waypoints[x+1]; + } + len--; + } +} + +void Route::up(int index){ + index--; + if (index < len -1 && index >= 0){ + pos tmp = waypoints[index]; + waypoints[index] = waypoints[index+1]; + waypoints[index+1] = tmp; + } +} + +void Route::down(int index){ + if (index < len && index >= 1){ + pos tmp = waypoints[index]; + waypoints[index] = waypoints[index - 1]; + waypoints[index - 1] = tmp; + } +} + +const char* Route::list(void){ + string s; + char *t; + for (int x = 0;x!=len;x++){ + sprintf(t,"%d) %f %f\r\n",x,waypoints[x].lat,waypoints[x].lon); + s += t; + } + + + return s.c_str(); +} + +pos Route::get(void){ + if (afstand(&gps->get(),&waypoints[active]) < 0.01){ + active++; + if (active = len){ + if (loop) + active = 0; + else + active = len -1; + klaar = 1; + } + } + else{ + klaar = 0; + } + return waypoints[active]; +} + +int Route::isklaar(void){ + return klaar; +} + +double Route::afstand_tot_doel(void){ + return afstand(&gps->get(),&waypoints[active]); +} + +double Route::koers_tot_doel(void){ + return koers(&gps->get(),&waypoints[active]); +} + + + +pos Route::read(int i){ + return waypoints[i]; +} + +int Route::getlen(void){ + return len; +} + + + \ No newline at end of file
diff -r 000000000000 -r bec310bde899 verwerking/route.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/verwerking/route.h Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,43 @@ +#ifndef ROUTE_H +#define ROUTE_H + +#include "pws.h" +#include "rhumb.h" +#include "gps_wrapper.h" + +#include <string> + + +class Route{ + int active; + pos waypoints[10]; + int len; + int klaar; + + Gps * gps; + + int loop; + + public: + Route(Gps *); + void loopMode(int); + void setActive(int); + int getActive(void); + void add(pos); + void del(int); + void up(int); + void down(int); + const char * list(void); + pos get(void); + int isklaar(void); + double afstand_tot_doel(void); + double koers_tot_doel(void); + pos read(int); + int getlen(void); + int getLoopMode(void); + + +}; + + +#endif