![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
LP Long Distance IR Vision Robot
Dependencies: max77650_charger_sample BufferedSerial SX1276GenericLib Adafruit-MotorShield NEO-6m-GPS MAX17055_EZconfig Adafruit_GFX USBDeviceHT Adafruit-PWM-Servo-Driver
utils.cpp@17:98f2528e8399, 2018-02-23 (annotated)
- Committer:
- Helmut64
- Date:
- Fri Feb 23 12:57:25 2018 +0000
- Revision:
- 17:98f2528e8399
- Child:
- 19:9f035b9e65ec
Added Heltec L4 board Pins into PinMap.h; Added USBDevice library to support USBSerial console IO; Moved helper code into utils.cpp; Moved dprintf support with var args and serial or USBSerial output.; Added compile time version support to set RTC to compi...
Who changed what in which revision?
User | Revision | Line number | New 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 | |
Helmut64 | 17:98f2528e8399 | 7 | time_t cvt_date(char const *date, char const *time); |
Helmut64 | 17:98f2528e8399 | 8 | |
Helmut64 | 17:98f2528e8399 | 9 | BufferedSerial *ser; |
Helmut64 | 17:98f2528e8399 | 10 | #ifdef FEATURE_USBSERIAL |
Helmut64 | 17:98f2528e8399 | 11 | USBSerialBuffered *usb; |
Helmut64 | 17:98f2528e8399 | 12 | #endif |
Helmut64 | 17:98f2528e8399 | 13 | bool _useDprintf; |
Helmut64 | 17:98f2528e8399 | 14 | |
Helmut64 | 17:98f2528e8399 | 15 | void InitSerial(int timeout, DigitalOut *led) |
Helmut64 | 17:98f2528e8399 | 16 | { |
Helmut64 | 17:98f2528e8399 | 17 | _useDprintf = true; |
Helmut64 | 17:98f2528e8399 | 18 | bool uartActive; |
Helmut64 | 17:98f2528e8399 | 19 | { |
Helmut64 | 17:98f2528e8399 | 20 | { |
Helmut64 | 17:98f2528e8399 | 21 | // need to turn rx low to avoid floating signal |
Helmut64 | 17:98f2528e8399 | 22 | DigitalOut rx(USBRX); |
Helmut64 | 17:98f2528e8399 | 23 | rx = 0; |
Helmut64 | 17:98f2528e8399 | 24 | } |
Helmut64 | 17:98f2528e8399 | 25 | DigitalIn uartRX(USBRX); |
Helmut64 | 17:98f2528e8399 | 26 | uartActive = uartRX.read(); |
Helmut64 | 17:98f2528e8399 | 27 | } |
Helmut64 | 17:98f2528e8399 | 28 | #ifdef FEATURE_USBSERIAL |
Helmut64 | 17:98f2528e8399 | 29 | if (!uartActive) { |
Helmut64 | 17:98f2528e8399 | 30 | usb = new USBSerialBuffered(); |
Helmut64 | 17:98f2528e8399 | 31 | Timer t; |
Helmut64 | 17:98f2528e8399 | 32 | t.start(); |
Helmut64 | 17:98f2528e8399 | 33 | while(!usb->connected()) { |
Helmut64 | 17:98f2528e8399 | 34 | if (led) |
Helmut64 | 17:98f2528e8399 | 35 | *led = !*led; |
Helmut64 | 17:98f2528e8399 | 36 | wait_ms(100); |
Helmut64 | 17:98f2528e8399 | 37 | if (timeout) { |
Helmut64 | 17:98f2528e8399 | 38 | if (t.read_ms() >= timeout) |
Helmut64 | 17:98f2528e8399 | 39 | return; |
Helmut64 | 17:98f2528e8399 | 40 | } |
Helmut64 | 17:98f2528e8399 | 41 | } |
Helmut64 | 17:98f2528e8399 | 42 | return; |
Helmut64 | 17:98f2528e8399 | 43 | } else { |
Helmut64 | 17:98f2528e8399 | 44 | #else |
Helmut64 | 17:98f2528e8399 | 45 | { |
Helmut64 | 17:98f2528e8399 | 46 | #endif |
Helmut64 | 17:98f2528e8399 | 47 | ser = new BufferedSerial(USBTX, USBRX); |
Helmut64 | 17:98f2528e8399 | 48 | ser->baud(230400); |
Helmut64 | 17:98f2528e8399 | 49 | ser->format(8); |
Helmut64 | 17:98f2528e8399 | 50 | } |
Helmut64 | 17:98f2528e8399 | 51 | time_t t = cvt_date(__DATE__, __TIME__); |
Helmut64 | 17:98f2528e8399 | 52 | if (t > time(NULL)) { |
Helmut64 | 17:98f2528e8399 | 53 | set_time(t); |
Helmut64 | 17:98f2528e8399 | 54 | } |
Helmut64 | 17:98f2528e8399 | 55 | |
Helmut64 | 17:98f2528e8399 | 56 | } |
Helmut64 | 17:98f2528e8399 | 57 | |
Helmut64 | 17:98f2528e8399 | 58 | void printTimeStamp() |
Helmut64 | 17:98f2528e8399 | 59 | { |
Helmut64 | 17:98f2528e8399 | 60 | static LowPowerTimer *timer; |
Helmut64 | 17:98f2528e8399 | 61 | if (!timer) { |
Helmut64 | 17:98f2528e8399 | 62 | timer = new LowPowerTimer(); |
Helmut64 | 17:98f2528e8399 | 63 | timer->start(); |
Helmut64 | 17:98f2528e8399 | 64 | } |
Helmut64 | 17:98f2528e8399 | 65 | time_t seconds = time(NULL); |
Helmut64 | 17:98f2528e8399 | 66 | struct tm *tm = localtime(&seconds); |
Helmut64 | 17:98f2528e8399 | 67 | int usecs = timer->read_us(); |
Helmut64 | 17:98f2528e8399 | 68 | if (usecs < 0) { |
Helmut64 | 17:98f2528e8399 | 69 | usecs = 0; |
Helmut64 | 17:98f2528e8399 | 70 | timer->stop(); |
Helmut64 | 17:98f2528e8399 | 71 | timer->reset(); |
Helmut64 | 17:98f2528e8399 | 72 | timer->start(); |
Helmut64 | 17:98f2528e8399 | 73 | } |
Helmut64 | 17:98f2528e8399 | 74 | int msecs = usecs % 1000000; |
Helmut64 | 17:98f2528e8399 | 75 | |
Helmut64 | 17:98f2528e8399 | 76 | rprintf("%02d:%02d:%02d.%06d ", tm->tm_hour, tm->tm_min, tm->tm_sec, msecs); |
Helmut64 | 17:98f2528e8399 | 77 | } |
Helmut64 | 17:98f2528e8399 | 78 | |
Helmut64 | 17:98f2528e8399 | 79 | void dprintf(const char *format, ...) |
Helmut64 | 17:98f2528e8399 | 80 | { |
Helmut64 | 17:98f2528e8399 | 81 | std::va_list arg; |
Helmut64 | 17:98f2528e8399 | 82 | |
Helmut64 | 17:98f2528e8399 | 83 | va_start(arg, format); |
Helmut64 | 17:98f2528e8399 | 84 | VAprintf(true, true, _useDprintf, format, arg); |
Helmut64 | 17:98f2528e8399 | 85 | va_end(arg); |
Helmut64 | 17:98f2528e8399 | 86 | } |
Helmut64 | 17:98f2528e8399 | 87 | |
Helmut64 | 17:98f2528e8399 | 88 | void rprintf(const char *format, ...) |
Helmut64 | 17:98f2528e8399 | 89 | { |
Helmut64 | 17:98f2528e8399 | 90 | std::va_list arg; |
Helmut64 | 17:98f2528e8399 | 91 | |
Helmut64 | 17:98f2528e8399 | 92 | va_start(arg, format); |
Helmut64 | 17:98f2528e8399 | 93 | VAprintf(false, false, _useDprintf, format, arg); |
Helmut64 | 17:98f2528e8399 | 94 | va_end(arg); |
Helmut64 | 17:98f2528e8399 | 95 | } |
Helmut64 | 17:98f2528e8399 | 96 | |
Helmut64 | 17:98f2528e8399 | 97 | void VAprintf(bool timstamp, bool newline, bool printEnabled, const char *format, va_list arg) |
Helmut64 | 17:98f2528e8399 | 98 | { |
Helmut64 | 17:98f2528e8399 | 99 | if (!printEnabled) |
Helmut64 | 17:98f2528e8399 | 100 | return; |
Helmut64 | 17:98f2528e8399 | 101 | |
Helmut64 | 17:98f2528e8399 | 102 | if (timstamp) |
Helmut64 | 17:98f2528e8399 | 103 | printTimeStamp(); |
Helmut64 | 17:98f2528e8399 | 104 | #ifdef FEATURE_USBSERIAL |
Helmut64 | 17:98f2528e8399 | 105 | if (usb) { |
Helmut64 | 17:98f2528e8399 | 106 | usb->vprintf_irqsafe(format, arg); |
Helmut64 | 17:98f2528e8399 | 107 | if (newline) |
Helmut64 | 17:98f2528e8399 | 108 | usb->printf_irqsafe("\r\n"); |
Helmut64 | 17:98f2528e8399 | 109 | #else |
Helmut64 | 17:98f2528e8399 | 110 | if (0) { |
Helmut64 | 17:98f2528e8399 | 111 | #endif |
Helmut64 | 17:98f2528e8399 | 112 | } else if (ser) { |
Helmut64 | 17:98f2528e8399 | 113 | // serial jas |
Helmut64 | 17:98f2528e8399 | 114 | int r = 0; |
Helmut64 | 17:98f2528e8399 | 115 | r = vsnprintf(NULL, 0, format, arg); |
Helmut64 | 17:98f2528e8399 | 116 | if (r < 82) { |
Helmut64 | 17:98f2528e8399 | 117 | char buffer[82+1]; |
Helmut64 | 17:98f2528e8399 | 118 | |
Helmut64 | 17:98f2528e8399 | 119 | vsnprintf(buffer, sizeof(buffer), format, arg); |
Helmut64 | 17:98f2528e8399 | 120 | r = ser->write(buffer, r); |
Helmut64 | 17:98f2528e8399 | 121 | } else { |
Helmut64 | 17:98f2528e8399 | 122 | char *buffer = new char[r+1]; |
Helmut64 | 17:98f2528e8399 | 123 | if (buffer) { |
Helmut64 | 17:98f2528e8399 | 124 | vsnprintf(buffer, r+1, format, arg); |
Helmut64 | 17:98f2528e8399 | 125 | r = ser->write(buffer, r); |
Helmut64 | 17:98f2528e8399 | 126 | delete[] buffer; |
Helmut64 | 17:98f2528e8399 | 127 | } else { |
Helmut64 | 17:98f2528e8399 | 128 | error("%s %d cannot alloc memory (%d bytes)!\r\n", __FILE__, __LINE__, r+1); |
Helmut64 | 17:98f2528e8399 | 129 | r = 0; |
Helmut64 | 17:98f2528e8399 | 130 | } |
Helmut64 | 17:98f2528e8399 | 131 | } |
Helmut64 | 17:98f2528e8399 | 132 | if (newline) |
Helmut64 | 17:98f2528e8399 | 133 | ser->write("\r\n", 2); |
Helmut64 | 17:98f2528e8399 | 134 | } |
Helmut64 | 17:98f2528e8399 | 135 | } |
Helmut64 | 17:98f2528e8399 | 136 | |
Helmut64 | 17:98f2528e8399 | 137 | |
Helmut64 | 17:98f2528e8399 | 138 | void dump(const char *title, const void *data, int len, bool dwords) |
Helmut64 | 17:98f2528e8399 | 139 | { |
Helmut64 | 17:98f2528e8399 | 140 | dprintf("dump(\"%s\", 0x%x, %d bytes)", title, data, len); |
Helmut64 | 17:98f2528e8399 | 141 | |
Helmut64 | 17:98f2528e8399 | 142 | int i, j, cnt; |
Helmut64 | 17:98f2528e8399 | 143 | unsigned char *u; |
Helmut64 | 17:98f2528e8399 | 144 | const int width = 16; |
Helmut64 | 17:98f2528e8399 | 145 | const int seppos = 7; |
Helmut64 | 17:98f2528e8399 | 146 | |
Helmut64 | 17:98f2528e8399 | 147 | cnt = 0; |
Helmut64 | 17:98f2528e8399 | 148 | u = (unsigned char *)data; |
Helmut64 | 17:98f2528e8399 | 149 | while (len > 0) { |
Helmut64 | 17:98f2528e8399 | 150 | rprintf("%08x: ", (unsigned int)data + cnt); |
Helmut64 | 17:98f2528e8399 | 151 | if (dwords) { |
Helmut64 | 17:98f2528e8399 | 152 | unsigned int *ip = ( unsigned int *)u; |
Helmut64 | 17:98f2528e8399 | 153 | rprintf(" 0x%08x\r\n", *ip); |
Helmut64 | 17:98f2528e8399 | 154 | u+= 4; |
Helmut64 | 17:98f2528e8399 | 155 | len -= 4; |
Helmut64 | 17:98f2528e8399 | 156 | cnt += 4; |
Helmut64 | 17:98f2528e8399 | 157 | continue; |
Helmut64 | 17:98f2528e8399 | 158 | } |
Helmut64 | 17:98f2528e8399 | 159 | cnt += width; |
Helmut64 | 17:98f2528e8399 | 160 | j = len < width ? len : width; |
Helmut64 | 17:98f2528e8399 | 161 | for (i = 0; i < j; i++) { |
Helmut64 | 17:98f2528e8399 | 162 | rprintf("%2.2x ", *(u + i)); |
Helmut64 | 17:98f2528e8399 | 163 | if (i == seppos) |
Helmut64 | 17:98f2528e8399 | 164 | rprintf(" "); |
Helmut64 | 17:98f2528e8399 | 165 | } |
Helmut64 | 17:98f2528e8399 | 166 | rprintf(" "); |
Helmut64 | 17:98f2528e8399 | 167 | if (j < width) { |
Helmut64 | 17:98f2528e8399 | 168 | i = width - j; |
Helmut64 | 17:98f2528e8399 | 169 | if (i > seppos + 1) |
Helmut64 | 17:98f2528e8399 | 170 | rprintf(" "); |
Helmut64 | 17:98f2528e8399 | 171 | while (i--) { |
Helmut64 | 17:98f2528e8399 | 172 | rprintf("%s", " "); |
Helmut64 | 17:98f2528e8399 | 173 | } |
Helmut64 | 17:98f2528e8399 | 174 | } |
Helmut64 | 17:98f2528e8399 | 175 | for (i = 0; i < j; i++) { |
Helmut64 | 17:98f2528e8399 | 176 | int c = *(u + i); |
Helmut64 | 17:98f2528e8399 | 177 | if (c >= ' ' && c <= '~') |
Helmut64 | 17:98f2528e8399 | 178 | rprintf("%c", c); |
Helmut64 | 17:98f2528e8399 | 179 | else |
Helmut64 | 17:98f2528e8399 | 180 | rprintf("."); |
Helmut64 | 17:98f2528e8399 | 181 | if (i == seppos) |
Helmut64 | 17:98f2528e8399 | 182 | rprintf(" "); |
Helmut64 | 17:98f2528e8399 | 183 | } |
Helmut64 | 17:98f2528e8399 | 184 | len -= width; |
Helmut64 | 17:98f2528e8399 | 185 | u += width; |
Helmut64 | 17:98f2528e8399 | 186 | rprintf("\r\n"); |
Helmut64 | 17:98f2528e8399 | 187 | } |
Helmut64 | 17:98f2528e8399 | 188 | rprintf("--\r\n"); |
Helmut64 | 17:98f2528e8399 | 189 | } |
Helmut64 | 17:98f2528e8399 | 190 | |
Helmut64 | 17:98f2528e8399 | 191 | /* |
Helmut64 | 17:98f2528e8399 | 192 | * Convert compile time to system time |
Helmut64 | 17:98f2528e8399 | 193 | */ |
Helmut64 | 17:98f2528e8399 | 194 | time_t |
Helmut64 | 17:98f2528e8399 | 195 | cvt_date(char const *date, char const *time) |
Helmut64 | 17:98f2528e8399 | 196 | { |
Helmut64 | 17:98f2528e8399 | 197 | char s_month[5]; |
Helmut64 | 17:98f2528e8399 | 198 | int year; |
Helmut64 | 17:98f2528e8399 | 199 | struct tm t; |
Helmut64 | 17:98f2528e8399 | 200 | static const char month_names[] = "JanFebMarAprMayJunJulAugSepOctNovDec"; |
Helmut64 | 17:98f2528e8399 | 201 | sscanf(date, "%s %d %d", s_month, &t.tm_mday, &year); |
Helmut64 | 17:98f2528e8399 | 202 | sscanf(time, "%2d %*c %2d %*c %2d", &t.tm_hour, &t.tm_min, &t.tm_sec); |
Helmut64 | 17:98f2528e8399 | 203 | // Find where is s_month in month_names. Deduce month value. |
Helmut64 | 17:98f2528e8399 | 204 | t.tm_mon = (strstr(month_names, s_month) - month_names) / 3; |
Helmut64 | 17:98f2528e8399 | 205 | t.tm_year = year - 1900; |
Helmut64 | 17:98f2528e8399 | 206 | return (int)mktime(&t); |
Helmut64 | 17:98f2528e8399 | 207 | } |