Revision 18:c2f3df4ef5fe, committed 2018-11-29

Comitter:

shimniok

Date:

Thu Nov 29 17:21:37 2018 +0000

Parent:

17:08f6ee55abbe

Child:

19:ce7fdade3534

Commit message:

Added Telemetry, updated util files

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Telemetry/Telemetry.cpp	Thu Nov 29 17:21:37 2018 +0000
@@ -0,0 +1,99 @@
+/*
+ * Telemetry.cpp
+ *
+ *  Created on: May 6, 2014
+ *      Author: mes
+ */
+
+#include "mbed.h"
+#include "Telemetry.h"
+#include "util.h"
+
+/** Create a new telemetry object
+ *
+ * @param uart is the Serial object used to send data
+ */
+Telemetry::Telemetry(Serial &uart) {
+    _uart = &uart;
+}
+
+/** Set baud rate for the serial connection
+ *
+ * @param baud is the integer baud rate
+ */
+void Telemetry::baud(int baud) {
+    _uart->baud(115200);
+}
+
+/** Send waypoints to the GCS
+ *
+ * @param wpt is the array of CartPosition waypoints
+ */
+void Telemetry::sendPacket(CartPosition wpt[], int wptCount) {
+    if (wpt) {
+        _uart->puts("`01, "); // waypoint message
+        _uart->puts(cvitos(wptCount));
+        _uart->puts(",");
+        for (int i=0; i < wptCount; i++) {
+            _uart->puts(cvftos(wpt[i].x, 5));
+            _uart->puts(",");
+            _uart->puts(cvftos(wpt[i].y, 5));
+            if (i+1 < wptCount) _uart->puts(",");
+        }
+        _uart->puts("\n");
+    }
+}
+
+/** Send system state to the GCS
+ *
+ * @param s is the SystemState object to send
+ */
+void Telemetry::sendPacket(SystemState *s) {
+    if (s) {
+        // Bearing is given as absolute; we want to send relative bearing
+        float bearing = s->bearing - s->estHeading;
+        while (bearing >= 360.0) {
+            bearing -= 360.0;
+        }
+        while (bearing < 0) {
+            bearing += 360.0;
+        }
+
+        _uart->puts("`00, "); // standard status message
+        _uart->puts(cvntos(s->millis));
+        _uart->puts(", ");
+        _uart->puts(cvftos(s->voltage, 2));
+        _uart->puts(", ");
+        _uart->puts(cvftos(s->current, 2));
+        _uart->puts(", ");
+        _uart->puts(cvftos(s->estHeading, 2));
+        _uart->puts(", ");
+//      _uart->puts(cvftos(s->gpsLatitude, 7));
+        _uart->puts(cvftos(s->estX, 5));
+        _uart->puts(", ");
+//      _uart->puts(cvftos(s->gpsLongitude, 7));
+        _uart->puts(cvftos(s->estY, 5));
+        _uart->puts(", ");
+        _uart->puts(cvftos(s->gpsHDOP, 1));
+        _uart->puts(", ");
+        _uart->puts(cvitos(s->gpsSats));
+        _uart->puts(", ");
+        _uart->puts(cvftos((s->lrEncSpeed + s->rrEncSpeed)/2.0, 1));
+        _uart->puts(", ");
+        _uart->puts(cvitos(s->nextWaypoint));
+        _uart->puts(", ");
+        _uart->puts(cvftos(bearing, 2));
+        _uart->puts(", ");
+        _uart->puts(cvftos(s->distance, 5));
+        _uart->puts(", ");
+        _uart->puts(cvftos(s->steerAngle, 2));
+        _uart->puts(", ");
+        _uart->puts(cvftos(s->LABrg, 1));
+        _uart->puts(", ");
+        _uart->puts(cvftos(s->LAx, 2));
+        _uart->puts(", ");
+        _uart->puts(cvftos(s->LAy, 2));
+        _uart->puts("\n");
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Telemetry/Telemetry.h	Thu Nov 29 17:21:37 2018 +0000
@@ -0,0 +1,46 @@
+/*
+ * Telemetry.h
+ *
+ *  Created on: May 6, 2014
+ *      Author: mes
+ */
+
+#ifndef TELEMETRY_H_
+#define TELEMETRY_H_
+
+#include "mbed.h"
+#include "SystemState.h"
+#include "CartPosition.h"
+
+class Telemetry {
+public:
+
+    /** Create a new telemetry object
+     *
+     * @param uart is the Serial object used to send data
+     */
+    Telemetry(Serial &uart);
+
+    /** Set baud rate for the serial connection
+     *
+     * @param baud is the integer baud rate
+     */
+    void baud(int baud);
+
+    /** Send waypoints to the GCS
+     *
+     * @param wpt is the array of CartPosition waypoints
+     */
+    void sendPacket(SystemState *s);
+
+    /** Send waypoints to the GCS
+     *
+     * @param wpt is the array of CartPosition waypoints
+     */
+    void sendPacket(CartPosition wpt[], int wptCount);
+
+private:
+    Serial *_uart;
+};
+
+#endif /* TELEMETRY_H_ */

--- a/util.cpp	Thu Nov 29 17:17:52 2018 +0000
+++ b/util.cpp	Thu Nov 29 17:21:37 2018 +0000
@@ -1,4 +1,8 @@
 #include <math.h>
+#include <string.h>
+
+#define MAXBUF 32
+#define MAXDIGITS 10
 
 /** 
  * Clamp a value (angle) between min (non-inclusive) and max (inclusive)
@@ -81,13 +85,112 @@
   return f;
 }
 
+
+char *cvntos(unsigned long n)
+{
+    static char buf[MAXBUF+1]; // +1 null termination
+    char *str = buf+MAXBUF;
+    int i;
+
+    // ensure null termination
+    *str-- = '\0';
+
+    for (i = 0; i < MAXBUF; i++) {
+        unsigned long m = n;
+        n /= 10;
+        char c = m - 10 * n;
+        *--str = c + '0';
+        if (n == 0) break;
+    }
+
+    return str;
+}
+
+
+
+char *cvitos(long n)
+{
+    static char buf[MAXBUF+2]; // +1 for sign, +1 for null termination
+    char *str = buf;
+
+    *str = '\0';
+    if (n < 0) {
+        *str++ = '-';
+        *str = '\0';
+        n = -n;
+    }
+    strcat(str, cvntos(n));
+
+    return buf;
+}
+
+
+
+char *cvftos(double number, int digits)
+{
+    static char buf[MAXBUF+3+MAXDIGITS]; // +1 for termination, +1 for sign, +1 for ., +MAXDIGITS for digits
+    char *str = buf;
+    int i;
+
+    // ensure proper null termination
+    *str = '\0';
+
+    // Limited buffer space for decimals
+    if (digits > MAXDIGITS)
+        digits = MAXDIGITS;
+
+    if (isnan(number)) {
+        strcpy(buf, "nan");
+    } else if (isinf(number)) {
+        strcpy(buf, "inf");
+    } else if (number > 4294967040.0 || number < -4294967040.0) {  // constant determined empirically
+        strcpy(buf, "ovf");
+    } else {
+
+        // Handle negative numbers
+        if (number < 0.0) {
+            // Add the sign
+            strcat(str, "-");
+            number = -number;
+        }
+
+        // Round correctly so that print(1.999, 2) prints as "2.00"
+        double rounding = 0.5;
+        for (i=0; i < digits; i++)
+            rounding /= 10.0;
+        number += rounding;
+
+        // Extract the integer part of the number and print it
+        unsigned long int_part = (unsigned long)number;
+        double remainder = number - (double)int_part;
+
+        // Add the integer part
+        strcat(str, cvntos(int_part));
+        // Add the decimal point
+        char *s = str + strlen(str);
+        *s++ = '.';
+
+        // Extract digits from the remainder one at a time
+        while (digits-- > 0) {
+            remainder *= 10.0;
+            int toPrint = (int) remainder;
+            *s++ = toPrint + '0';
+            remainder -= (double) toPrint;
+        }
+        *s = '\0';
+    }
+
+    return str;
+}
+
+
 // copy t to s until delimiter is reached
 // return location of delimiter+1 in t
 // if s or t null, return null
 char *split(char *s, char *t, int max, char delim)
 {
   int i = 0;
-  
+
   if (s == 0 || t == 0)
     return 0;
 
@@ -96,6 +199,6 @@
     i++;
   }
   *s = 0;
-    
+
   return t+1;
 }

--- a/util.h	Thu Nov 29 17:17:52 2018 +0000
+++ b/util.h	Thu Nov 29 17:21:37 2018 +0000
@@ -26,6 +26,28 @@
 /** Convert string to floating point */
 double cvstof(char *s);
 
+/** Convert unsigned long to string
+ *
+ * @param n is the unsigned long to convert
+ * @returns char * to static char array
+ */
+char *cvntos(unsigned long n);
+
+/** Convert signed long to string
+ *
+ * @param n is the signed long to convert
+ * @returns char * to static char array
+ */
+char *cvitos(long n);
+
+/** Convert float/double to string
+ *
+ * @param number is the floating point number to convert
+ * @param digits is the number of digits after the decimal point
+ * @return char * to static char array
+ */
+char *cvftos(double number, int digits);
+
 /** Tokenize a string 
  * @param s is the string to tokenize
  * @param max is the maximum number of characters
@@ -34,4 +56,4 @@
  */
 char *split(char *s, char *t, int max, char delim);
 
-#endif
\ No newline at end of file
+#endif