Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Telemetry/Telemetry.cpp@25:bb5356402687, 2018-11-30 (annotated)

Committer:
shimniok
Date:
Fri Nov 30 16:11:53 2018 +0000
Revision:
25:bb5356402687
Parent:
18:c2f3df4ef5fe 
Initial publish of revised version.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 18:c2f3df4ef5fe 1 /*
shimniok 18:c2f3df4ef5fe 2 * Telemetry.cpp
shimniok 18:c2f3df4ef5fe 3 *
shimniok 18:c2f3df4ef5fe 4 * Created on: May 6, 2014
shimniok 18:c2f3df4ef5fe 5 * Author: mes
shimniok 18:c2f3df4ef5fe 6 */
shimniok 18:c2f3df4ef5fe 7
shimniok 18:c2f3df4ef5fe 8 #include "mbed.h"
shimniok 18:c2f3df4ef5fe 9 #include "Telemetry.h"
shimniok 18:c2f3df4ef5fe 10 #include "util.h"
shimniok 18:c2f3df4ef5fe 11
shimniok 18:c2f3df4ef5fe 12 /** Create a new telemetry object
shimniok 18:c2f3df4ef5fe 13 *
shimniok 18:c2f3df4ef5fe 14 * @param uart is the Serial object used to send data
shimniok 18:c2f3df4ef5fe 15 */
shimniok 18:c2f3df4ef5fe 16 Telemetry::Telemetry(Serial &uart) {
shimniok 18:c2f3df4ef5fe 17 _uart = &uart;
shimniok 18:c2f3df4ef5fe 18 }
shimniok 18:c2f3df4ef5fe 19
shimniok 18:c2f3df4ef5fe 20 /** Set baud rate for the serial connection
shimniok 18:c2f3df4ef5fe 21 *
shimniok 18:c2f3df4ef5fe 22 * @param baud is the integer baud rate
shimniok 18:c2f3df4ef5fe 23 */
shimniok 18:c2f3df4ef5fe 24 void Telemetry::baud(int baud) {
shimniok 18:c2f3df4ef5fe 25 _uart->baud(115200);
shimniok 18:c2f3df4ef5fe 26 }
shimniok 18:c2f3df4ef5fe 27
shimniok 18:c2f3df4ef5fe 28 /** Send waypoints to the GCS
shimniok 18:c2f3df4ef5fe 29 *
shimniok 18:c2f3df4ef5fe 30 * @param wpt is the array of CartPosition waypoints
shimniok 18:c2f3df4ef5fe 31 */
shimniok 18:c2f3df4ef5fe 32 void Telemetry::sendPacket(CartPosition wpt[], int wptCount) {
shimniok 18:c2f3df4ef5fe 33 if (wpt) {
shimniok 18:c2f3df4ef5fe 34 _uart->puts("`01, "); // waypoint message
shimniok 18:c2f3df4ef5fe 35 _uart->puts(cvitos(wptCount));
shimniok 18:c2f3df4ef5fe 36 _uart->puts(",");
shimniok 18:c2f3df4ef5fe 37 for (int i=0; i < wptCount; i++) {
shimniok 18:c2f3df4ef5fe 38 _uart->puts(cvftos(wpt[i].x, 5));
shimniok 18:c2f3df4ef5fe 39 _uart->puts(",");
shimniok 18:c2f3df4ef5fe 40 _uart->puts(cvftos(wpt[i].y, 5));
shimniok 18:c2f3df4ef5fe 41 if (i+1 < wptCount) _uart->puts(",");
shimniok 18:c2f3df4ef5fe 42 }
shimniok 18:c2f3df4ef5fe 43 _uart->puts("\n");
shimniok 18:c2f3df4ef5fe 44 }
shimniok 18:c2f3df4ef5fe 45 }
shimniok 18:c2f3df4ef5fe 46
shimniok 18:c2f3df4ef5fe 47 /** Send system state to the GCS
shimniok 18:c2f3df4ef5fe 48 *
shimniok 18:c2f3df4ef5fe 49 * @param s is the SystemState object to send
shimniok 18:c2f3df4ef5fe 50 */
shimniok 18:c2f3df4ef5fe 51 void Telemetry::sendPacket(SystemState *s) {
shimniok 18:c2f3df4ef5fe 52 if (s) {
shimniok 18:c2f3df4ef5fe 53 // Bearing is given as absolute; we want to send relative bearing
shimniok 18:c2f3df4ef5fe 54 float bearing = s->bearing - s->estHeading;
shimniok 18:c2f3df4ef5fe 55 while (bearing >= 360.0) {
shimniok 18:c2f3df4ef5fe 56 bearing -= 360.0;
shimniok 18:c2f3df4ef5fe 57 }
shimniok 18:c2f3df4ef5fe 58 while (bearing < 0) {
shimniok 18:c2f3df4ef5fe 59 bearing += 360.0;
shimniok 18:c2f3df4ef5fe 60 }
shimniok 18:c2f3df4ef5fe 61
shimniok 18:c2f3df4ef5fe 62 _uart->puts("`00, "); // standard status message
shimniok 18:c2f3df4ef5fe 63 _uart->puts(cvntos(s->millis));
shimniok 18:c2f3df4ef5fe 64 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 65 _uart->puts(cvftos(s->voltage, 2));
shimniok 18:c2f3df4ef5fe 66 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 67 _uart->puts(cvftos(s->current, 2));
shimniok 18:c2f3df4ef5fe 68 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 69 _uart->puts(cvftos(s->estHeading, 2));
shimniok 18:c2f3df4ef5fe 70 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 71 // _uart->puts(cvftos(s->gpsLatitude, 7));
shimniok 18:c2f3df4ef5fe 72 _uart->puts(cvftos(s->estX, 5));
shimniok 18:c2f3df4ef5fe 73 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 74 // _uart->puts(cvftos(s->gpsLongitude, 7));
shimniok 18:c2f3df4ef5fe 75 _uart->puts(cvftos(s->estY, 5));
shimniok 18:c2f3df4ef5fe 76 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 77 _uart->puts(cvftos(s->gpsHDOP, 1));
shimniok 18:c2f3df4ef5fe 78 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 79 _uart->puts(cvitos(s->gpsSats));
shimniok 18:c2f3df4ef5fe 80 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 81 _uart->puts(cvftos((s->lrEncSpeed + s->rrEncSpeed)/2.0, 1));
shimniok 18:c2f3df4ef5fe 82 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 83 _uart->puts(cvitos(s->nextWaypoint));
shimniok 18:c2f3df4ef5fe 84 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 85 _uart->puts(cvftos(bearing, 2));
shimniok 18:c2f3df4ef5fe 86 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 87 _uart->puts(cvftos(s->distance, 5));
shimniok 18:c2f3df4ef5fe 88 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 89 _uart->puts(cvftos(s->steerAngle, 2));
shimniok 18:c2f3df4ef5fe 90 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 91 _uart->puts(cvftos(s->LABrg, 1));
shimniok 18:c2f3df4ef5fe 92 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 93 _uart->puts(cvftos(s->LAx, 2));
shimniok 18:c2f3df4ef5fe 94 _uart->puts(", ");
shimniok 18:c2f3df4ef5fe 95 _uart->puts(cvftos(s->LAy, 2));
shimniok 18:c2f3df4ef5fe 96 _uart->puts("\n");
shimniok 18:c2f3df4ef5fe 97 }
shimniok 18:c2f3df4ef5fe 98 }
shimniok 18:c2f3df4ef5fe 99