Devin Alexander / Mbed OS Low_Power_Long_Distance_IR_Vision_Robot

This is example code that can get you started with building your own IR vision robot that communicates over LoRa

Dependencies:   Adafruit-MotorShield Adafruit-PWM-Servo-Driver Adafruit_GFX BufferedSerial MAX17055_EZconfig NEO-6m-GPS SX1276GenericLib USBDeviceHT max32630fthr max77650_charger_sample

Fork of MAX326xxFTHR_LoRa_Example_test by Devin Alexander

utils.cpp@43:b38b934d82e6, 2018-08-15 (annotated)

Committer:
dev_alexander
Date:
Wed Aug 15 04:12:27 2018 +0000
Revision:
43:b38b934d82e6
Parent:
19:9f035b9e65ec 
Commit after publishing fork of Adafruit_GFX library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Helmut64 17:98f2528e8399 1 /*
Helmut64 17:98f2528e8399 2 * Copyright (c) 2018 Helmut Tschemernjak
Helmut64 17:98f2528e8399 3 * 30826 Garbsen (Hannover) Germany
Helmut64 17:98f2528e8399 4 */
Helmut64 17:98f2528e8399 5 #include "main.h"
Helmut64 17:98f2528e8399 6
dev_alexander 19:9f035b9e65ec 7 #define BAUD_RATE 115200
dev_alexander 19:9f035b9e65ec 8
dev_alexander 19:9f035b9e65ec 9
Helmut64 17:98f2528e8399 10 time_t cvt_date(char const *date, char const *time);
Helmut64 17:98f2528e8399 11
dev_alexander 19:9f035b9e65ec 12
Helmut64 17:98f2528e8399 13 BufferedSerial *ser;
Helmut64 17:98f2528e8399 14 #ifdef FEATURE_USBSERIAL
Helmut64 17:98f2528e8399 15 USBSerialBuffered *usb;
Helmut64 17:98f2528e8399 16 #endif
Helmut64 17:98f2528e8399 17 bool _useDprintf;
Helmut64 17:98f2528e8399 18
Helmut64 17:98f2528e8399 19 void InitSerial(int timeout, DigitalOut *led)
Helmut64 17:98f2528e8399 20 {
Helmut64 17:98f2528e8399 21 _useDprintf = true;
Helmut64 17:98f2528e8399 22 bool uartActive;
Helmut64 17:98f2528e8399 23 {
Helmut64 17:98f2528e8399 24 {
Helmut64 17:98f2528e8399 25 // need to turn rx low to avoid floating signal
Helmut64 17:98f2528e8399 26 DigitalOut rx(USBRX);
Helmut64 17:98f2528e8399 27 rx = 0;
Helmut64 17:98f2528e8399 28 }
Helmut64 17:98f2528e8399 29 DigitalIn uartRX(USBRX);
Helmut64 17:98f2528e8399 30 uartActive = uartRX.read();
Helmut64 17:98f2528e8399 31 }
Helmut64 17:98f2528e8399 32 #ifdef FEATURE_USBSERIAL
Helmut64 17:98f2528e8399 33 if (!uartActive) {
Helmut64 17:98f2528e8399 34 usb = new USBSerialBuffered();
Helmut64 17:98f2528e8399 35 Timer t;
Helmut64 17:98f2528e8399 36 t.start();
Helmut64 17:98f2528e8399 37 while(!usb->connected()) {
Helmut64 17:98f2528e8399 38 if (led)
Helmut64 17:98f2528e8399 39 *led = !*led;
Helmut64 17:98f2528e8399 40 wait_ms(100);
Helmut64 17:98f2528e8399 41 if (timeout) {
Helmut64 17:98f2528e8399 42 if (t.read_ms() >= timeout)
Helmut64 17:98f2528e8399 43 return;
Helmut64 17:98f2528e8399 44 }
Helmut64 17:98f2528e8399 45 }
Helmut64 17:98f2528e8399 46 return;
Helmut64 17:98f2528e8399 47 } else {
Helmut64 17:98f2528e8399 48 #else
Helmut64 17:98f2528e8399 49 {
Helmut64 17:98f2528e8399 50 #endif
Helmut64 17:98f2528e8399 51 ser = new BufferedSerial(USBTX, USBRX);
dev_alexander 19:9f035b9e65ec 52 ser->baud(BAUD_RATE);
Helmut64 17:98f2528e8399 53 ser->format(8);
Helmut64 17:98f2528e8399 54 }
Helmut64 17:98f2528e8399 55 time_t t = cvt_date(__DATE__, __TIME__);
Helmut64 17:98f2528e8399 56 if (t > time(NULL)) {
Helmut64 17:98f2528e8399 57 set_time(t);
Helmut64 17:98f2528e8399 58 }
Helmut64 17:98f2528e8399 59
Helmut64 17:98f2528e8399 60 }
Helmut64 17:98f2528e8399 61
Helmut64 17:98f2528e8399 62 void printTimeStamp()
Helmut64 17:98f2528e8399 63 {
Helmut64 17:98f2528e8399 64 static LowPowerTimer *timer;
Helmut64 17:98f2528e8399 65 if (!timer) {
Helmut64 17:98f2528e8399 66 timer = new LowPowerTimer();
Helmut64 17:98f2528e8399 67 timer->start();
Helmut64 17:98f2528e8399 68 }
Helmut64 17:98f2528e8399 69 time_t seconds = time(NULL);
Helmut64 17:98f2528e8399 70 struct tm *tm = localtime(&seconds);
Helmut64 17:98f2528e8399 71 int usecs = timer->read_us();
Helmut64 17:98f2528e8399 72 if (usecs < 0) {
Helmut64 17:98f2528e8399 73 usecs = 0;
Helmut64 17:98f2528e8399 74 timer->stop();
Helmut64 17:98f2528e8399 75 timer->reset();
Helmut64 17:98f2528e8399 76 timer->start();
Helmut64 17:98f2528e8399 77 }
Helmut64 17:98f2528e8399 78 int msecs = usecs % 1000000;
Helmut64 17:98f2528e8399 79
Helmut64 17:98f2528e8399 80 rprintf("%02d:%02d:%02d.%06d ", tm->tm_hour, tm->tm_min, tm->tm_sec, msecs);
Helmut64 17:98f2528e8399 81 }
Helmut64 17:98f2528e8399 82
Helmut64 17:98f2528e8399 83 void dprintf(const char *format, ...)
Helmut64 17:98f2528e8399 84 {
Helmut64 17:98f2528e8399 85 std::va_list arg;
Helmut64 17:98f2528e8399 86
Helmut64 17:98f2528e8399 87 va_start(arg, format);
Helmut64 17:98f2528e8399 88 VAprintf(true, true, _useDprintf, format, arg);
Helmut64 17:98f2528e8399 89 va_end(arg);
Helmut64 17:98f2528e8399 90 }
Helmut64 17:98f2528e8399 91
Helmut64 17:98f2528e8399 92 void rprintf(const char *format, ...)
Helmut64 17:98f2528e8399 93 {
Helmut64 17:98f2528e8399 94 std::va_list arg;
Helmut64 17:98f2528e8399 95
Helmut64 17:98f2528e8399 96 va_start(arg, format);
Helmut64 17:98f2528e8399 97 VAprintf(false, false, _useDprintf, format, arg);
Helmut64 17:98f2528e8399 98 va_end(arg);
Helmut64 17:98f2528e8399 99 }
Helmut64 17:98f2528e8399 100
Helmut64 17:98f2528e8399 101 void VAprintf(bool timstamp, bool newline, bool printEnabled, const char *format, va_list arg)
Helmut64 17:98f2528e8399 102 {
Helmut64 17:98f2528e8399 103 if (!printEnabled)
Helmut64 17:98f2528e8399 104 return;
Helmut64 17:98f2528e8399 105
Helmut64 17:98f2528e8399 106 if (timstamp)
Helmut64 17:98f2528e8399 107 printTimeStamp();
Helmut64 17:98f2528e8399 108 #ifdef FEATURE_USBSERIAL
Helmut64 17:98f2528e8399 109 if (usb) {
Helmut64 17:98f2528e8399 110 usb->vprintf_irqsafe(format, arg);
Helmut64 17:98f2528e8399 111 if (newline)
Helmut64 17:98f2528e8399 112 usb->printf_irqsafe("\r\n");
Helmut64 17:98f2528e8399 113 #else
Helmut64 17:98f2528e8399 114 if (0) {
Helmut64 17:98f2528e8399 115 #endif
Helmut64 17:98f2528e8399 116 } else if (ser) {
Helmut64 17:98f2528e8399 117 // serial jas
Helmut64 17:98f2528e8399 118 int r = 0;
Helmut64 17:98f2528e8399 119 r = vsnprintf(NULL, 0, format, arg);
Helmut64 17:98f2528e8399 120 if (r < 82) {
Helmut64 17:98f2528e8399 121 char buffer[82+1];
Helmut64 17:98f2528e8399 122
Helmut64 17:98f2528e8399 123 vsnprintf(buffer, sizeof(buffer), format, arg);
Helmut64 17:98f2528e8399 124 r = ser->write(buffer, r);
Helmut64 17:98f2528e8399 125 } else {
Helmut64 17:98f2528e8399 126 char *buffer = new char[r+1];
Helmut64 17:98f2528e8399 127 if (buffer) {
Helmut64 17:98f2528e8399 128 vsnprintf(buffer, r+1, format, arg);
Helmut64 17:98f2528e8399 129 r = ser->write(buffer, r);
Helmut64 17:98f2528e8399 130 delete[] buffer;
Helmut64 17:98f2528e8399 131 } else {
Helmut64 17:98f2528e8399 132 error("%s %d cannot alloc memory (%d bytes)!\r\n", __FILE__, __LINE__, r+1);
Helmut64 17:98f2528e8399 133 r = 0;
Helmut64 17:98f2528e8399 134 }
Helmut64 17:98f2528e8399 135 }
Helmut64 17:98f2528e8399 136 if (newline)
Helmut64 17:98f2528e8399 137 ser->write("\r\n", 2);
Helmut64 17:98f2528e8399 138 }
Helmut64 17:98f2528e8399 139 }
Helmut64 17:98f2528e8399 140
Helmut64 17:98f2528e8399 141
Helmut64 17:98f2528e8399 142 void dump(const char *title, const void *data, int len, bool dwords)
Helmut64 17:98f2528e8399 143 {
Helmut64 17:98f2528e8399 144 dprintf("dump(\"%s\", 0x%x, %d bytes)", title, data, len);
Helmut64 17:98f2528e8399 145
Helmut64 17:98f2528e8399 146 int i, j, cnt;
Helmut64 17:98f2528e8399 147 unsigned char *u;
Helmut64 17:98f2528e8399 148 const int width = 16;
Helmut64 17:98f2528e8399 149 const int seppos = 7;
Helmut64 17:98f2528e8399 150
Helmut64 17:98f2528e8399 151 cnt = 0;
Helmut64 17:98f2528e8399 152 u = (unsigned char *)data;
Helmut64 17:98f2528e8399 153 while (len > 0) {
Helmut64 17:98f2528e8399 154 rprintf("%08x: ", (unsigned int)data + cnt);
Helmut64 17:98f2528e8399 155 if (dwords) {
Helmut64 17:98f2528e8399 156 unsigned int *ip = ( unsigned int *)u;
Helmut64 17:98f2528e8399 157 rprintf(" 0x%08x\r\n", *ip);
Helmut64 17:98f2528e8399 158 u+= 4;
Helmut64 17:98f2528e8399 159 len -= 4;
Helmut64 17:98f2528e8399 160 cnt += 4;
Helmut64 17:98f2528e8399 161 continue;
Helmut64 17:98f2528e8399 162 }
Helmut64 17:98f2528e8399 163 cnt += width;
Helmut64 17:98f2528e8399 164 j = len < width ? len : width;
Helmut64 17:98f2528e8399 165 for (i = 0; i < j; i++) {
Helmut64 17:98f2528e8399 166 rprintf("%2.2x ", *(u + i));
Helmut64 17:98f2528e8399 167 if (i == seppos)
Helmut64 17:98f2528e8399 168 rprintf(" ");
Helmut64 17:98f2528e8399 169 }
Helmut64 17:98f2528e8399 170 rprintf(" ");
Helmut64 17:98f2528e8399 171 if (j < width) {
Helmut64 17:98f2528e8399 172 i = width - j;
Helmut64 17:98f2528e8399 173 if (i > seppos + 1)
Helmut64 17:98f2528e8399 174 rprintf(" ");
Helmut64 17:98f2528e8399 175 while (i--) {
Helmut64 17:98f2528e8399 176 rprintf("%s", " ");
Helmut64 17:98f2528e8399 177 }
Helmut64 17:98f2528e8399 178 }
Helmut64 17:98f2528e8399 179 for (i = 0; i < j; i++) {
Helmut64 17:98f2528e8399 180 int c = *(u + i);
Helmut64 17:98f2528e8399 181 if (c >= ' ' && c <= '~')
Helmut64 17:98f2528e8399 182 rprintf("%c", c);
Helmut64 17:98f2528e8399 183 else
Helmut64 17:98f2528e8399 184 rprintf(".");
Helmut64 17:98f2528e8399 185 if (i == seppos)
Helmut64 17:98f2528e8399 186 rprintf(" ");
Helmut64 17:98f2528e8399 187 }
Helmut64 17:98f2528e8399 188 len -= width;
Helmut64 17:98f2528e8399 189 u += width;
Helmut64 17:98f2528e8399 190 rprintf("\r\n");
Helmut64 17:98f2528e8399 191 }
Helmut64 17:98f2528e8399 192 rprintf("--\r\n");
Helmut64 17:98f2528e8399 193 }
Helmut64 17:98f2528e8399 194
Helmut64 17:98f2528e8399 195 /*
Helmut64 17:98f2528e8399 196 * Convert compile time to system time
Helmut64 17:98f2528e8399 197 */
Helmut64 17:98f2528e8399 198 time_t
Helmut64 17:98f2528e8399 199 cvt_date(char const *date, char const *time)
Helmut64 17:98f2528e8399 200 {
Helmut64 17:98f2528e8399 201 char s_month[5];
Helmut64 17:98f2528e8399 202 int year;
Helmut64 17:98f2528e8399 203 struct tm t;
Helmut64 17:98f2528e8399 204 static const char month_names[] = "JanFebMarAprMayJunJulAugSepOctNovDec";
Helmut64 17:98f2528e8399 205 sscanf(date, "%s %d %d", s_month, &t.tm_mday, &year);
Helmut64 17:98f2528e8399 206 sscanf(time, "%2d %*c %2d %*c %2d", &t.tm_hour, &t.tm_min, &t.tm_sec);
Helmut64 17:98f2528e8399 207 // Find where is s_month in month_names. Deduce month value.
Helmut64 17:98f2528e8399 208 t.tm_mon = (strstr(month_names, s_month) - month_names) / 3;
Helmut64 17:98f2528e8399 209 t.tm_year = year - 1900;
Helmut64 17:98f2528e8399 210 return (int)mktime(&t);
Helmut64 17:98f2528e8399 211 }