diff --git a/src/Adafruit_GPS.cpp b/src/Adafruit_GPS.cpp index baec1e7..9d9c3f0 100644 --- a/src/Adafruit_GPS.cpp +++ b/src/Adafruit_GPS.cpp @@ -434,7 +434,7 @@ char Adafruit_GPS::read(void) { uint32_t tStart = millis(); // as close as we can get to time char was sent char c = 0; - if (paused) + if (paused || noComms) return c; #if (defined(__AVR__) || defined(ESP8266)) && defined(USE_SW_SERIAL) @@ -578,6 +578,16 @@ Adafruit_GPS::Adafruit_GPS(SPIClass *theSPI, int8_t cspin) { gpsSPI_cs = cspin; } +/**************************************************************************/ +/*! + @brief Constructor when there are no communications attached +*/ +/**************************************************************************/ +Adafruit_GPS::Adafruit_GPS() { + common_init(); // Set everything to common state, then... + noComms = true; +} + /**************************************************************************/ /*! @brief Initialization code used by all constructor types @@ -605,6 +615,19 @@ void Adafruit_GPS::common_init(void) { HDOP = VDOP = PDOP = 0.0; // nmea_float_t } +/**************************************************************************/ +/*! + @brief Destroy the object. + @return none +*/ +/**************************************************************************/ +Adafruit_GPS::~Adafruit_GPS() { +#ifdef NMEA_EXTENSIONS + for (int i = 0; i < (int)NMEA_MAX_INDEX; i++) + removeHistory((nmea_index_t)i); // to free any history mallocs +#endif +} + /**************************************************************************/ /*! @brief Start the HW or SW serial port diff --git a/src/Adafruit_GPS.h b/src/Adafruit_GPS.h index 145fce5..8da85b2 100644 --- a/src/Adafruit_GPS.h +++ b/src/Adafruit_GPS.h @@ -83,7 +83,9 @@ public: Adafruit_GPS(HardwareSerial *ser); // Constructor when using HardwareSerial Adafruit_GPS(TwoWire *theWire); // Constructor when using I2C Adafruit_GPS(SPIClass *theSPI, int8_t cspin); // Constructor when using SPI - + Adafruit_GPS(); // Constructor for no communications, just data storage + virtual ~Adafruit_GPS(); + char *lastNMEA(void); bool newNMEAreceived(); void common_init(void); @@ -100,7 +102,6 @@ public: bool check(char *nmea); bool parse(char *); - void addChecksum(char *buff); nmea_float_t secondsSinceFix(); nmea_float_t secondsSinceTime(); nmea_float_t secondsSinceDate(); @@ -109,6 +110,32 @@ public: bool wakeup(void); bool standby(void); + // NMEA_build.cpp +#ifdef NMEA_EXTENSIONS + char *build(char *nmea, const char *thisSource, const char *thisSentence, + char ref = 'R'); +#endif + void addChecksum(char *buff); + + // NMEA_data.cpp + void newDataValue(nmea_index_t tag, nmea_float_t v); +#ifdef NMEA_EXTENSIONS + nmea_float_t get(nmea_index_t idx); + nmea_float_t getSmoothed(nmea_index_t idx); + void initDataValue(nmea_index_t idx, char *label = NULL, char *fmt = NULL, + char *unit = NULL, unsigned long response = 0, + nmea_value_type_t type = NMEA_SIMPLE_FLOAT); + nmea_history_t *initHistory(nmea_index_t idx, nmea_float_t scale = 10.0, + nmea_float_t offset = 0.0, + unsigned historyInterval = 20, + unsigned historyN = 192); + void removeHistory(nmea_index_t idx); + void showDataValue(nmea_index_t idx, int n = 7); + bool isCompoundAngle(nmea_index_t idx); +#endif + nmea_float_t boatAngle(nmea_float_t s, nmea_float_t c); + nmea_float_t compassAngle(nmea_float_t s, nmea_float_t c); + int thisCheck = 0; ///< the results of the check on the current sentence char thisSource[NMEA_MAX_SOURCE_ID] = { 0}; ///< the first two letters of the current sentence, e.g. WI, GP @@ -180,11 +207,20 @@ public: uint8_t LOCUS_percent; ///< Log life used percentage #ifdef NMEA_EXTENSIONS - // NMEA additional public functions - char *build(char *nmea, const char *thisSource, const char *thisSentence, - char ref = 'R'); - // NMEA additional public variables + nmea_datavalue_t + val[NMEA_MAX_INDEX]; ///< an array of data value structs, val[0] = most + ///< recent HDOP so that ockam indexing works + nmea_float_t depthToKeel = + 2.4; ///< depth from surface to bottom of keel in metres + nmea_float_t depthToTransducer = + 0.0; ///< depth of transducer below the surface in metres + + char toID[NMEA_MAX_WP_ID] = { + 0}; ///< id of waypoint going to on this segment of the route + char fromID[NMEA_MAX_WP_ID] = { + 0}; ///< id of waypoint coming from on this segment of the route + char txtTXT[63] = {0}; ///< text content from most recent TXT sentence int txtTot = 0; ///< total TXT sentences in group int txtID = 0; ///< id of the text message @@ -200,6 +236,8 @@ private: bool parseLatDir(char *); void parseLon(char *); bool parseLonDir(char *); + // NMEA_data.cpp + void data_init(); bool parseFix(char *); // used by check() for validity tests, room for future expansion const char *sources[5] = {"II", "WI", "GP", "GN", @@ -226,6 +264,7 @@ private: #if (defined(__AVR__) || defined(ESP8266)) && defined(USE_SW_SERIAL) SoftwareSerial *gpsSwSerial; #endif + bool noComms = false; HardwareSerial *gpsHwSerial; TwoWire *gpsI2C; SPIClass *gpsSPI; diff --git a/src/NMEA_data.cpp b/src/NMEA_data.cpp new file mode 100644 index 0000000..c5d0c15 --- /dev/null +++ b/src/NMEA_data.cpp @@ -0,0 +1,563 @@ +/**************************************************************************/ +/*! + @file NMEA_data.cpp + + @section intro Introduction + + Code for tracking values that change with time so that history can be + examined for recent trends in real time. This code will only generate the + stubs for newDataValue() and data_init(), adding essentially nothing to + the memory footprint unless NMEA_EXTENSIONS is defined. + + This is code intended to complement the Adafruit GPS library and process + data for many additional NMEA sentences, mostly of interest to sailors. + + The parse function can be a direct substitute for the Adafruit_GPS + function of the same name, updating the same variables within an NMEA + object. A simple use case would involve: + + Define an Adafruit_GPS object and use it to collect and parse sentences + from a serial port. The GPS object will be updated and can be used exactly + as usual. + + Define an NMEA object and use it to parse the same sentences. It will + succeed on more sentences than the GPS object and keep more detailed data + records. It updates all the same variables as the GPS object, so you could + skip the GPS parsing step. + + @section author Author + + Written by Rick Sellens. + + @section license License + + CCBY license +*/ +/**************************************************************************/ + +#include "Adafruit_GPS.h" + +/**************************************************************************/ +/*! + @brief Update the value and history information with a new value. Call + whenever a new data value is received. The function does nothing if the + NMEA extensions are not enabled. + @param idx The data index for which a new value has been received + @param v The new value received + @return none +*/ +/**************************************************************************/ +void Adafruit_GPS::newDataValue(nmea_index_t idx, nmea_float_t v) { +#ifdef NMEA_EXTENSIONS + // Serial.println();Serial.print(idx);Serial.print(", "); Serial.println(v); + val[idx].latest = v; // update the value + + // update the smoothed verion + if (isCompoundAngle(idx)) { // angle with sin/cos component recording + newDataValue((nmea_index_t)(idx + 1), sin(v / RAD_TO_DEG)); + newDataValue((nmea_index_t)(idx + 2), cos(v / RAD_TO_DEG)); + } + // weighting factor for smoothing depends on delta t / tau + nmea_float_t w = min((nmea_float_t)1.0, ((nmea_float_t)millis() - val[idx].lastUpdate) / + val[idx].response); + // default smoothing + val[idx].smoothed = (1.0 - w) * val[idx].smoothed + w * v; + // special smoothing for some angle types + if (val[idx].type == NMEA_COMPASS_ANGLE_SIN) + val[idx].smoothed = + compassAngle(val[idx + 1].smoothed, val[idx + 2].smoothed); + if (val[idx].type == NMEA_BOAT_ANGLE_SIN) + val[idx].smoothed = boatAngle(val[idx + 1].smoothed, val[idx + 2].smoothed); + // some types just don't make sense to smooth -- use latest + if (val[idx].type == NMEA_BOAT_ANGLE) + val[idx].smoothed = val[idx].latest; + if (val[idx].type == NMEA_COMPASS_ANGLE) + val[idx].smoothed = val[idx].latest; + if (val[idx].type == NMEA_DDMM) + val[idx].smoothed = val[idx].latest; + if (val[idx].type == NMEA_HHMMSS) + val[idx].smoothed = val[idx].latest; + + val[idx].lastUpdate = millis(); // take a time stamp + if (val[idx].hist) { // there's a history struct for this tag + unsigned long seconds = (millis() - val[idx].hist->lastHistory) / 1000; + // do an update if the time has come, or if this is the first time through + if (seconds >= val[idx].hist->historyInterval || + val[idx].hist->lastHistory == 0) { + + // move the old history back in time by one step + for (unsigned i = 0; i < (val[idx].hist->n - 1); i++) + val[idx].hist->data[i] = val[idx].hist->data[i + 1]; + + // Create the new entry, scaling and offsetting the value to fit into an + // integer, and based on the smoothed value. + val[idx].hist->data[val[idx].hist->n - 1] = + val[idx].hist->scale * (val[idx].smoothed - val[idx].hist->offset); + val[idx].hist->lastHistory = millis(); + } + } +#endif // NMEA_EXTENSIONS +} + +/**************************************************************************/ +/*! + @brief Initialize the object. Build a val[] matrix of data values for + all of the enumerated values, including the extra values for the compound + angle types. The initializer shold probably leave it up to the user + sketch to decide which data values should carry the extra memory burden + of history. + @return none +*/ +/**************************************************************************/ +void Adafruit_GPS::data_init() { +#ifdef NMEA_EXTENSIONS + // fill all the data values with nothing + static char c[] = "NUL"; + for (int i = 0; i < (int)NMEA_MAX_INDEX; i++) { + initDataValue((nmea_index_t)i, c, NULL, NULL, 0, (nmea_value_type_t)0); + } + + // fill selected data values with the relevant information and pointers + static char BoatSpeedfmt[] = "%6.2f"; + static char WindSpeedfmt[] = "%6.1f"; + static char Speedunit[] = "knots"; + static char Anglefmt[] = "%6.0f"; + static char BoatAngleunit[] = "Degrees"; + static char TrueAngleunit[] = "Deg True"; + static char MagAngleunit[] = "Deg Mag"; + + static char HDOPlabel[] = "HDOP"; + initDataValue(NMEA_HDOP, HDOPlabel); + + static char LATlabel[] = "Lat"; + static char LATfmt[] = "%9.4f"; + static char LATunit[] = "DDD.dddd"; + initDataValue( + NMEA_LAT, LATlabel, LATfmt, LATunit, 0, + NMEA_BOAT_ANGLE); // angle from -180 to 180, or actually -90 to 90 for lat + + static char LONlabel[] = "Lon"; + initDataValue(NMEA_LON, LONlabel, LATfmt, LATunit, 0, + NMEA_BOAT_ANGLE); // angle from -180 to 180 + + static char LATWPlabel[] = "WP Lat"; + initDataValue(NMEA_LATWP, LATWPlabel, LATfmt, LATunit, 0, NMEA_BOAT_ANGLE); + + static char LONWPlabel[] = "WP Lon"; + initDataValue(NMEA_LONWP, LONWPlabel, LATfmt, LATunit, 0, NMEA_BOAT_ANGLE); + + static char SOGlabel[] = "SOG"; + initDataValue(NMEA_SOG, SOGlabel, BoatSpeedfmt, Speedunit); + + static char COGlabel[] = "COG"; + // types with sin/cos need two extra spots in the values matrix! + initDataValue(NMEA_COG, COGlabel, Anglefmt, TrueAngleunit, 0, + NMEA_COMPASS_ANGLE_SIN); // type: 0-360 angle with sin/cos 11 + + static char COGWPlabel[] = "WP COG"; + initDataValue(NMEA_COGWP, COGWPlabel, Anglefmt, TrueAngleunit, 0, + NMEA_COMPASS_ANGLE); // type: angle 0-360 1 + + static char XTElabel[] = "XTE"; + static char XTEfmt[] = "%6.2f"; + static char XTEunit[] = "NM"; + initDataValue(NMEA_XTE, XTElabel, XTEfmt, XTEunit); + + static char DISTWPlabel[] = "WP Dist"; + initDataValue(NMEA_DISTWP, DISTWPlabel, XTEfmt, XTEunit); + + static char AWAlabel[] = "AWA"; + initDataValue(NMEA_AWA, AWAlabel, Anglefmt, BoatAngleunit, 0, + NMEA_BOAT_ANGLE_SIN); // type: +-180 angle with sin/cos 12 + + static char AWSlabel[] = "AWS"; + initDataValue(NMEA_AWS, AWSlabel, WindSpeedfmt, Speedunit); + + static char TWAlabel[] = "TWA"; + initDataValue(NMEA_TWA, TWAlabel, Anglefmt, BoatAngleunit, 0, + NMEA_BOAT_ANGLE_SIN); // type: +-180 angle with sin/cos 12 + + static char TWDlabel[] = "TWD"; + initDataValue(NMEA_TWD, TWDlabel, Anglefmt, TrueAngleunit, 0, + NMEA_COMPASS_ANGLE_SIN); // type: 0-360 angle with sin/cos 11 + + static char TWSlabel[] = "TWS"; + initDataValue(NMEA_TWS, TWSlabel, WindSpeedfmt, Speedunit); + + static char VMGlabel[] = "VMG"; + initDataValue(NMEA_VMG, VMGlabel, BoatSpeedfmt, Speedunit); + + static char VMGWPlabel[] = "WP VMG"; + initDataValue(NMEA_VMGWP, VMGWPlabel, BoatSpeedfmt, Speedunit); + + static char HEELlabel[] = "Heel"; + static char HEELunit[] = "Deg Stbd"; + initDataValue(NMEA_HEEL, HEELlabel, Anglefmt, HEELunit, 0, + NMEA_BOAT_ANGLE); // type: angle +/-180 2 + + static char PITCHlabel[] = "Pitch"; + static char PITCHunit[] = "Deg Bow Up"; + initDataValue(NMEA_PITCH, PITCHlabel, Anglefmt, PITCHunit, 0, + NMEA_BOAT_ANGLE); // type: angle +/-180 2 + static char HDGlabel[] = "HDG"; + initDataValue(NMEA_HDG, HDGlabel, Anglefmt, MagAngleunit, 0, + NMEA_COMPASS_ANGLE_SIN); // type: 0-360 angle with sin/cos 11 + + static char HDTlabel[] = "HDG"; + initDataValue(NMEA_HDT, HDTlabel, Anglefmt, TrueAngleunit, 0, + NMEA_COMPASS_ANGLE_SIN); // type: 0-360 angle with sin/cos 11 + + static char VTWlabel[] = "VTW"; + initDataValue(NMEA_VTW, VTWlabel, BoatSpeedfmt, Speedunit); + + static char LOGlabel[] = "Log"; + static char LOGfmt[] = "%6.0f"; + static char LOGunit[] = "NM"; + initDataValue(NMEA_LOG, LOGlabel, LOGfmt, LOGunit); + + static char LOGRlabel[] = "Trip"; + static char LOGRfmt[] = "%6.2f"; + initDataValue(NMEA_LOG, LOGRlabel, LOGRfmt, LOGunit); + + static char DEPTHlabel[] = "Depth"; + static char DEPTHfmt[] = "%6.1f"; + static char DEPTHunit[] = "m"; + initDataValue(NMEA_DEPTH, DEPTHlabel, DEPTHfmt, DEPTHunit); + + static char RPM_M1label[] = "Motor 1"; + static char RPM_M1fmt[] = "%6.0f"; + static char RPM_M1unit[] = "RPM"; + initDataValue(NMEA_RPM_M1, RPM_M1label, RPM_M1fmt, RPM_M1unit); + + static char TEMPERATURE_M1label[] = "Temp 1"; + static char TEMPERATURE_M1fmt[] = "%6.0f"; + static char TEMPERATURE_M1unit[] = "Deg C"; + initDataValue(NMEA_TEMPERATURE_M1, TEMPERATURE_M1label, TEMPERATURE_M1fmt, + TEMPERATURE_M1unit); + + static char PRESSURE_M1label[] = "Oil 1"; + static char PRESSURE_M1fmt[] = "%6.0f"; + static char PRESSURE_M1unit[] = "kPa"; + initDataValue(NMEA_PRESSURE_M1, PRESSURE_M1label, PRESSURE_M1fmt, + PRESSURE_M1unit); + + static char VOLTAGE_M1label[] = "Motor 1"; + static char VOLTAGE_M1fmt[] = "%6.2f"; + static char VOLTAGE_M1unit[] = "Volts"; + initDataValue(NMEA_VOLTAGE_M1, VOLTAGE_M1label, VOLTAGE_M1fmt, + VOLTAGE_M1unit); + + static char CURRENT_M1label[] = "Motor 1"; + static char CURRENT_M1fmt[] = "%6.1f"; + static char CURRENT_M1unit[] = "Amps"; + initDataValue(NMEA_CURRENT_M1, CURRENT_M1label, CURRENT_M1fmt, + CURRENT_M1unit); + + static char RPM_M2label[] = "Motor 2"; + initDataValue(NMEA_RPM_M2, RPM_M2label, RPM_M1fmt, RPM_M1unit); + + static char TEMPERATURE_M2label[] = "Temp 2"; + initDataValue(NMEA_TEMPERATURE_M2, TEMPERATURE_M2label, TEMPERATURE_M1fmt, + TEMPERATURE_M1unit); + + static char PRESSURE_M2label[] = "Oil 2"; + initDataValue(NMEA_PRESSURE_M2, PRESSURE_M2label, PRESSURE_M1fmt, + PRESSURE_M1unit); + + static char VOLTAGE_M2label[] = "Motor 2"; + initDataValue(NMEA_VOLTAGE_M2, VOLTAGE_M2label, VOLTAGE_M1fmt, + VOLTAGE_M1unit); + + static char CURRENT_M2label[] = "Motor 2"; + initDataValue(NMEA_CURRENT_M2, CURRENT_M2label, CURRENT_M1fmt, + CURRENT_M1unit); + + static char TEMPERATURE_AIRlabel[] = "Air"; + static char TEMPERATURE_AIRfmt[] = "%6.1f"; + static char TEMPERATURE_AIRunit[] = "Deg C"; + initDataValue(NMEA_TEMPERATURE_AIR, TEMPERATURE_AIRlabel, TEMPERATURE_AIRfmt, + TEMPERATURE_AIRunit); + + static char TEMPERATURE_WATERlabel[] = "Water"; + static char TEMPERATURE_WATERfmt[] = "%6.1f"; + static char TEMPERATURE_WATERunit[] = "Deg C"; + initDataValue(NMEA_TEMPERATURE_WATER, TEMPERATURE_WATERlabel, + TEMPERATURE_WATERfmt, TEMPERATURE_WATERunit); + + static char HUMIDITYlabel[] = "Humidity"; + static char HUMIDITYfmt[] = "%6.0f"; + static char HUMIDITYunit[] = "% RH"; + initDataValue(NMEA_HUMIDITY, HUMIDITYlabel, HUMIDITYfmt, HUMIDITYunit); + + static char BAROMETERlabel[] = "Barometer"; + static char BAROMETERfmt[] = "%6.0f"; + static char BAROMETERunit[] = "Pa"; + initDataValue(NMEA_BAROMETER, BAROMETERlabel, BAROMETERfmt, BAROMETERunit); +#endif // NMEA_EXTENSIONS +} + +#ifdef NMEA_EXTENSIONS +/**************************************************************************/ +/*! + @brief Clearer approach to retrieving NMEA values by allowing calls that + look like nmea.get(NMEA_TWA) instead of val[NMEA_TWA].latest. + Use newDataValue() to set the values. + @param idx the NMEA value's index + @return the latest NMEA value +*/ +/**************************************************************************/ +nmea_float_t Adafruit_GPS::get(nmea_index_t idx) { + if (idx >= NMEA_MAX_INDEX || idx < NMEA_HDOP) + return 0.0; + return val[idx].latest; +} + +/**************************************************************************/ +/*! + @brief Clearer approach to retrieving NMEA values + @param idx the NMEA value's index + @return the latest NMEA value, smoothed +*/ +/**************************************************************************/ +nmea_float_t Adafruit_GPS::getSmoothed(nmea_index_t idx) { + if (idx >= NMEA_MAX_INDEX || idx < NMEA_HDOP) + return 0.0; + return val[idx].smoothed; +} + +/**************************************************************************/ +/*! + @brief Initialize the contents of a data value table entry + @param idx The data index for the value to be initialized + @param label Pointer to a label string that describes the value + @param fmt Pointer to a sprintf format to use for the value, e.g. "%6.2f" + @param unit Pointer to a string for the units, e.g. "Deg Mag" + @param response Time constant for smoothing in ms. The longer the time + constant, the more slowly the smoothed value will move towards a new value. + @param type The type of data contained in the value. simple float 0, + angle 0-360 1, angle +/-180 2, angle with history centered +/- around + the latest angle 3, lat/lon DDMM.mm 10, time HHMMSS 20. + @return none +*/ +/**************************************************************************/ +void Adafruit_GPS::initDataValue(nmea_index_t idx, char *label, char *fmt, + char *unit, unsigned long response, + nmea_value_type_t type) { + if (idx < NMEA_MAX_INDEX) { + if (label) + val[idx].label = label; + if (fmt) + val[idx].fmt = fmt; + if (unit) + val[idx].unit = unit; + if (response) + val[idx].response = response; + val[idx].type = type; + if ((int)(val[idx].type / 10) == + 1) { // angle with sin/cos component recording + initDataValue((nmea_index_t)( + idx + 1)); // initialize the next two data values as well + initDataValue((nmea_index_t)(idx + 2)); + } + } +} + +/**************************************************************************/ +/*! + @brief Attempt to add history to a data value table entry. If it fails + to malloc the space, history will not be added. Test the pointer for a + check if needed. + @param idx The data index for the value to have history recorded + @param scale Value for scaling the integer history list + @param offset Value for scaling the integer history list + @param historyInterval Approximate Time in seconds between historical + values. + @return pointer to the history +*/ +/**************************************************************************/ +nmea_history_t *Adafruit_GPS::initHistory(nmea_index_t idx, nmea_float_t scale, + nmea_float_t offset, + unsigned historyInterval, + unsigned historyN) { + historyN = max((unsigned)10, historyN); + if (idx < NMEA_MAX_INDEX) { + // remove any existing history + if (val[idx].hist != NULL) + removeHistory(idx); + // space for the struct + val[idx].hist = (nmea_history_t *)malloc(sizeof(nmea_history_t)); + if (val[idx].hist != NULL) { + // space for the data array of the appropriate size + val[idx].hist->data = (int16_t *)malloc(sizeof(int16_t) * historyN); + if (val[idx].hist->data != NULL) { + // initialize the data array + for (unsigned i = 0; i < historyN; i++) + val[idx].hist->data[i] = 0; + } else + free(val[idx].hist); + } + if (val[idx].hist != NULL) { + val[idx].hist->n = historyN; + if (scale > 0.0) + val[idx].hist->scale = scale; + val[idx].hist->offset = offset; + if (historyInterval > 0) + val[idx].hist->historyInterval = historyInterval; + } + return val[idx].hist; + } + return NULL; +} + +/**************************************************************************/ +/*! + @brief Remove history from a data value table entry, if it has been added. + @param idx The data index for the value to have history removed + @return none +*/ +/**************************************************************************/ +void Adafruit_GPS::removeHistory(nmea_index_t idx) { + if (idx < NMEA_MAX_INDEX) { + if (val[idx].hist == NULL) + return; + free(val[idx].hist->data); + free(val[idx].hist); + val[idx].hist = NULL; + } +} + +/**************************************************************************/ +/*! + @brief Print out the current state of a data value. Primarily useful as + a debugging aid. + @param idx The index for the data value + @param n The number of history values to include + @return none +*/ +/**************************************************************************/ +void Adafruit_GPS::showDataValue(nmea_index_t idx, int n) { + Serial.print("idx: "); + if(idx < 10) Serial.print(" "); + Serial.print(idx); + Serial.print(", "); + Serial.print(val[idx].label); + Serial.print(", "); + Serial.print(val[idx].latest, 4); + Serial.print(", "); + Serial.print(val[idx].smoothed, 4); + Serial.print(", at "); + Serial.print(val[idx].lastUpdate); + Serial.print(" ms, tau = "); + Serial.print(val[idx].response); + Serial.print(" ms, type:"); + Serial.print(val[idx].type); + Serial.print(", ockam:"); + Serial.print(val[idx].ockam); + if (val[idx].hist) { + Serial.print("\n History at "); + Serial.print(val[idx].hist->historyInterval); + Serial.print(" second intervals: "); + Serial.print(val[idx].hist->data[val[idx].hist->n - 1]); + for (unsigned i = val[idx].hist->n - 2; i >= max(val[idx].hist->n - n, (unsigned)0); + i--) { // most recent first + Serial.print(", "); + Serial.print(val[idx].hist->data[i]); + } + } + Serial.print("\n"); + if (idx == NMEA_LAT) { + Serial.print(" latitude (DDMM.mmmm): "); + Serial.print(latitude, 4); + Serial.print(", lat: "); + Serial.print(lat); + Serial.print(", latitudeDegrees: "); + Serial.print(latitudeDegrees,8); + Serial.print(", latitude_fixed: "); + Serial.println(latitude_fixed); + } + if (idx == NMEA_LON) { + Serial.print(" longitude (DDMM.mmmm): "); + Serial.print(longitude, 4); + Serial.print(", lon: "); + Serial.print(lon); + Serial.print(", longitudeDegrees: "); + Serial.print(longitudeDegrees,8); + Serial.print(", longitude_fixed: "); + Serial.println(longitude_fixed); + } +} + +/**************************************************************************/ +/*! + @brief Check if it is a compound angle + @param idx The index for the data value + @return true if a compound angle requiring 3 contiguos data values. +*/ +/**************************************************************************/ +bool Adafruit_GPS::isCompoundAngle(nmea_index_t idx) { + if ((int)(val[idx].type / 10) == 1) // angle with sin/cos component recording + return true; + return false; +} + +/**************************************************************************/ +/*! + @brief Estimate a direction in -180 to 180 degree range from the values + of the sine and cosine of the compound angle, which could be noisy. + @param s The sin of the angle + @param c The cosine of the angle + @return The angle in -180 to 180 degree range. +*/ +/**************************************************************************/ +nmea_float_t Adafruit_GPS::boatAngle(nmea_float_t s, nmea_float_t c) { + nmea_float_t sAng = + asin(s) * RAD_TO_DEG; // put the sin angle in -90 to 90 range + while (sAng < -90) + sAng += 180.; + while (sAng > 90) + sAng -= 180.; + nmea_float_t cAng = + acos(c) * RAD_TO_DEG; // put the cos angle in 0 to 180 range + while (cAng < 0) + cAng += 180.; + while (cAng > 180) + cAng -= 180.; + // Pick the most accurate representation and translate + if (cAng < 45) + return sAng; // Close hauled + else { + if (cAng > 135) { // Running + if (sAng > 0) + return 180 - sAng; // on starboard tack + else + return -180 - sAng; // on port tack + } else { // Reaching + if (sAng < 0) + return -cAng; // on port tack + else + return cAng; // on starboard tack + } + } + return 9999; // you can't get here, but there must be an explicit return +} + +/**************************************************************************/ +/*! + @brief Estimate a direction in 0 to 360 degree range from the values + of the sine and cosine of the compound angle, which could be noisy. + @param s The sin of the angle + @param c The cosine of the angle + @return The angle in 0 to 360 degree range. +*/ +/**************************************************************************/ +nmea_float_t Adafruit_GPS::compassAngle(nmea_float_t s, nmea_float_t c) { + nmea_float_t ang = boatAngle(s, c); + if (ang < 5000) { // if reasonable range + while (ang < 0) + ang += 360.; // round up + while (ang > 360) + ang -= 360.; // round down + } + return ang; +} +#endif // NMEA_EXTENSIONS