Revision 0:bec310bde899, committed 2011-09-27
- Comitter:
- pd0wm
- Date:
- Tue Sep 27 19:46:30 2011 +0000
- Commit message:
Changed in this revision
--- /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().
+
+*/
--- /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;
+ }
+ }
+ }
+}
--- /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
+
--- /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;
+}
+
+
--- /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
+
--- /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;
+}
+
--- /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
+
--- /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); }
+}
+
--- /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
+
--- /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
--- /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;
+}
--- /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
--- /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;
+ }
+}
+
--- /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
--- /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;
+}
+
--- /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
--- /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);
+
+ }
+}
--- /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
--- /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;
+}
--- /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
--- /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);
+
+ }
+
+ }
+
+
+
+
+
+ }
+
+ }
+
+ }
+
+
+}
--- /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
--- /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
--- /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
--- /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);
+}
+
+
+
--- /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
--- /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
--- /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
--- /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;
+
+
+}
--- /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
--- /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;
+
+
+}
--- /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
--- /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
--- /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
Willem Melching