PushToGo on STM32F429-Disco Board

Dependencies:   BSP_DISCO_F429ZI LCD_DISCO_F429ZI pushtogo usb

Committer:
caoyuan9642
Date:
Sun Aug 19 05:20:21 2018 +0000
Revision:
0:084d1dae2ea1
Child:
1:64c1fd738059
v0.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
caoyuan9642 0:084d1dae2ea1 1 /*
caoyuan9642 0:084d1dae2ea1 2 * LCDStreamHandle.cpp
caoyuan9642 0:084d1dae2ea1 3 *
caoyuan9642 0:084d1dae2ea1 4 * Created on: 2018Äê2ÔÂ25ÈÕ
caoyuan9642 0:084d1dae2ea1 5 * Author: caoyuan9642
caoyuan9642 0:084d1dae2ea1 6 */
caoyuan9642 0:084d1dae2ea1 7
caoyuan9642 0:084d1dae2ea1 8 #include "LCDConsole.h"
caoyuan9642 0:084d1dae2ea1 9 #include <math.h>
caoyuan9642 0:084d1dae2ea1 10 #include <ctype.h>
caoyuan9642 0:084d1dae2ea1 11 #include "MCULoadMeasurement.h"
caoyuan9642 0:084d1dae2ea1 12
caoyuan9642 0:084d1dae2ea1 13 bool LCDConsole::inited = false;
caoyuan9642 0:084d1dae2ea1 14 Mutex LCDConsole::mutex;
caoyuan9642 0:084d1dae2ea1 15 LCD_DISCO_F429ZI LCDConsole::lcd;
caoyuan9642 0:084d1dae2ea1 16 int LCDConsole::x0 = 0, LCDConsole::y0 = 0, LCDConsole::width = lcd.GetXSize(),
caoyuan9642 0:084d1dae2ea1 17 LCDConsole::height = lcd.GetYSize();
caoyuan9642 0:084d1dae2ea1 18 int LCDConsole::textheight = 0, LCDConsole::textwidth = 0,
caoyuan9642 0:084d1dae2ea1 19 LCDConsole::buffersize = 0;
caoyuan9642 0:084d1dae2ea1 20 int *LCDConsole::buffer, *LCDConsole::head, *LCDConsole::tail;
caoyuan9642 0:084d1dae2ea1 21 Semaphore LCDConsole::sem_update(0, 1);
caoyuan9642 0:084d1dae2ea1 22 Thread LCDConsole::thread(osPriorityLow, OS_STACK_SIZE, NULL, "LCD Console");
caoyuan9642 0:084d1dae2ea1 23
caoyuan9642 0:084d1dae2ea1 24 LCDConsole lcd_handle_out("lcd_stdout", LCD_COLOR_WHITE);
caoyuan9642 0:084d1dae2ea1 25 LCDConsole lcd_handle_err("lcd_stderr", LCD_COLOR_YELLOW);
caoyuan9642 0:084d1dae2ea1 26
caoyuan9642 0:084d1dae2ea1 27
caoyuan9642 0:084d1dae2ea1 28 #define BG_COLOR LCD_COLOR_BLACK
caoyuan9642 0:084d1dae2ea1 29
caoyuan9642 0:084d1dae2ea1 30 void idle_hook()
caoyuan9642 0:084d1dae2ea1 31 {
caoyuan9642 0:084d1dae2ea1 32 core_util_critical_section_enter();
caoyuan9642 0:084d1dae2ea1 33 MCULoadMeasurement::getInstance().setMCUActive(false);
caoyuan9642 0:084d1dae2ea1 34 // sleep_manager_lock_deep_sleep();
caoyuan9642 0:084d1dae2ea1 35 // sleep();
caoyuan9642 0:084d1dae2ea1 36 // __WFI();
caoyuan9642 0:084d1dae2ea1 37 // sleep_manager_unlock_deep_sleep();
caoyuan9642 0:084d1dae2ea1 38 int i = 1000;
caoyuan9642 0:084d1dae2ea1 39 while (i--)
caoyuan9642 0:084d1dae2ea1 40 ;
caoyuan9642 0:084d1dae2ea1 41 MCULoadMeasurement::getInstance().setMCUActive(true);
caoyuan9642 0:084d1dae2ea1 42 core_util_critical_section_exit();
caoyuan9642 0:084d1dae2ea1 43 }
caoyuan9642 0:084d1dae2ea1 44
caoyuan9642 0:084d1dae2ea1 45 void LCDConsole::init(int x0, int y0, int width, int height)
caoyuan9642 0:084d1dae2ea1 46 {
caoyuan9642 0:084d1dae2ea1 47 LCDConsole::x0 = x0;
caoyuan9642 0:084d1dae2ea1 48 LCDConsole::y0 = y0;
caoyuan9642 0:084d1dae2ea1 49 LCDConsole::width = width;
caoyuan9642 0:084d1dae2ea1 50 LCDConsole::height = height;
caoyuan9642 0:084d1dae2ea1 51
caoyuan9642 0:084d1dae2ea1 52 // Clear area with background color
caoyuan9642 0:084d1dae2ea1 53 lcd.SetTextColor(BG_COLOR);
caoyuan9642 0:084d1dae2ea1 54 lcd.FillRect(x0, y0, width, height);
caoyuan9642 0:084d1dae2ea1 55 BSP_LCD_SetFont(&Font12);
caoyuan9642 0:084d1dae2ea1 56
caoyuan9642 0:084d1dae2ea1 57 // Calculate width and height of the text area
caoyuan9642 0:084d1dae2ea1 58 textwidth = width / BSP_LCD_GetFont()->Width;
caoyuan9642 0:084d1dae2ea1 59 textheight = height / BSP_LCD_GetFont()->Height;
caoyuan9642 0:084d1dae2ea1 60 buffersize = textwidth * textheight;
caoyuan9642 0:084d1dae2ea1 61
caoyuan9642 0:084d1dae2ea1 62 // Init buffer
caoyuan9642 0:084d1dae2ea1 63 buffer = new int[buffersize](); // Will be init to zero
caoyuan9642 0:084d1dae2ea1 64 head = tail = buffer;
caoyuan9642 0:084d1dae2ea1 65
caoyuan9642 0:084d1dae2ea1 66 // Start task thread
caoyuan9642 0:084d1dae2ea1 67 thread.start(task_thread);
caoyuan9642 0:084d1dae2ea1 68
caoyuan9642 0:084d1dae2ea1 69 // Register idle hook
caoyuan9642 0:084d1dae2ea1 70 Thread::attach_idle_hook(idle_hook);
caoyuan9642 0:084d1dae2ea1 71 MCULoadMeasurement::getInstance().reset();
caoyuan9642 0:084d1dae2ea1 72 }
caoyuan9642 0:084d1dae2ea1 73
caoyuan9642 0:084d1dae2ea1 74 void LCDConsole::task_thread()
caoyuan9642 0:084d1dae2ea1 75 {
caoyuan9642 0:084d1dae2ea1 76 int *buffer0 = new int[buffersize](); // Local buffer
caoyuan9642 0:084d1dae2ea1 77 char sbuf[64];
caoyuan9642 0:084d1dae2ea1 78 // Main loop
caoyuan9642 0:084d1dae2ea1 79 while (true)
caoyuan9642 0:084d1dae2ea1 80 {
caoyuan9642 0:084d1dae2ea1 81 // Wait for update signal.
caoyuan9642 0:084d1dae2ea1 82 int s = sem_update.wait(1000);
caoyuan9642 0:084d1dae2ea1 83 if (s == 0)
caoyuan9642 0:084d1dae2ea1 84 {
caoyuan9642 0:084d1dae2ea1 85 // Timeout, update CPU usage
caoyuan9642 0:084d1dae2ea1 86 lcd.SetBackColor(LCD_COLOR_BLUE);
caoyuan9642 0:084d1dae2ea1 87 lcd.SetTextColor(LCD_COLOR_WHITE);
caoyuan9642 0:084d1dae2ea1 88 int len = sprintf(sbuf, "Load: %4.1f%% ",
caoyuan9642 0:084d1dae2ea1 89 MCULoadMeasurement::getInstance().getCPUUsage() * 100);
caoyuan9642 0:084d1dae2ea1 90 lcd.DisplayStringAt(0, lcd.GetYSize() - BSP_LCD_GetFont()->Height,
caoyuan9642 0:084d1dae2ea1 91 (unsigned char*) sbuf, LEFT_MODE);
caoyuan9642 0:084d1dae2ea1 92 MCULoadMeasurement::getInstance().reset();
caoyuan9642 0:084d1dae2ea1 93
caoyuan9642 0:084d1dae2ea1 94 time_t t = time(NULL);
caoyuan9642 0:084d1dae2ea1 95 struct tm ts;
caoyuan9642 0:084d1dae2ea1 96 gmtime_r(&t, &ts);
caoyuan9642 0:084d1dae2ea1 97 strftime(sbuf, sizeof(sbuf), "%T, %x", &ts);
caoyuan9642 0:084d1dae2ea1 98 strcat(sbuf, " UTC"); // append
caoyuan9642 0:084d1dae2ea1 99
caoyuan9642 0:084d1dae2ea1 100 lcd.SetBackColor(0xFF00AF7F);
caoyuan9642 0:084d1dae2ea1 101 lcd.DisplayStringAt(BSP_LCD_GetFont()->Width * len,
caoyuan9642 0:084d1dae2ea1 102 lcd.GetYSize() - BSP_LCD_GetFont()->Height,
caoyuan9642 0:084d1dae2ea1 103 (unsigned char*) sbuf, LEFT_MODE);
caoyuan9642 0:084d1dae2ea1 104
caoyuan9642 0:084d1dae2ea1 105 continue;
caoyuan9642 0:084d1dae2ea1 106 }
caoyuan9642 0:084d1dae2ea1 107
caoyuan9642 0:084d1dae2ea1 108 mutex.lock(); // Lock the buffer. If any thread is still printing stuff to the buffer, this will wait for it to finish
caoyuan9642 0:084d1dae2ea1 109 memcpy(buffer0, buffer, buffersize * sizeof(*buffer)); // Copy buffer to a private one to work in
caoyuan9642 0:084d1dae2ea1 110 mutex.unlock(); // Unlock the buffer. If any thread is waiting for the mutex, it can now run (and potentially generating another update signal).
caoyuan9642 0:084d1dae2ea1 111
caoyuan9642 0:084d1dae2ea1 112 int *head0 = buffer0 + (head - buffer);
caoyuan9642 0:084d1dae2ea1 113 // clear area;
caoyuan9642 0:084d1dae2ea1 114 // lcd.SetTextColor(BG_COLOR);
caoyuan9642 0:084d1dae2ea1 115 // lcd.FillRect(x0, y0, width, height);
caoyuan9642 0:084d1dae2ea1 116
caoyuan9642 0:084d1dae2ea1 117 int line = 0, col = 0;
caoyuan9642 0:084d1dae2ea1 118 int x = x0, y = y0;
caoyuan9642 0:084d1dae2ea1 119 lcd.SetBackColor(BG_COLOR);
caoyuan9642 0:084d1dae2ea1 120 for (int *p = head0, i = 0; i < buffersize; i++)
caoyuan9642 0:084d1dae2ea1 121 {
caoyuan9642 0:084d1dae2ea1 122 // Set color from the buffer
caoyuan9642 0:084d1dae2ea1 123 lcd.SetTextColor((uint32_t(0xFF000000 | (*p >> 8))));
caoyuan9642 0:084d1dae2ea1 124
caoyuan9642 0:084d1dae2ea1 125 // Content to draw
caoyuan9642 0:084d1dae2ea1 126 unsigned char c = (*p) & 0xFF;
caoyuan9642 0:084d1dae2ea1 127
caoyuan9642 0:084d1dae2ea1 128 // Display the char if displayable, otherwise put white space
caoyuan9642 0:084d1dae2ea1 129 if (isprint(c))
caoyuan9642 0:084d1dae2ea1 130 lcd.DisplayChar(x, y, c);
caoyuan9642 0:084d1dae2ea1 131 else
caoyuan9642 0:084d1dae2ea1 132 lcd.DisplayChar(x, y, ' ');
caoyuan9642 0:084d1dae2ea1 133
caoyuan9642 0:084d1dae2ea1 134 x += BSP_LCD_GetFont()->Width;
caoyuan9642 0:084d1dae2ea1 135 col++;
caoyuan9642 0:084d1dae2ea1 136 if (col == textwidth)
caoyuan9642 0:084d1dae2ea1 137 {
caoyuan9642 0:084d1dae2ea1 138 // Next line
caoyuan9642 0:084d1dae2ea1 139 line++;
caoyuan9642 0:084d1dae2ea1 140 y += BSP_LCD_GetFont()->Height;
caoyuan9642 0:084d1dae2ea1 141 col = 0;
caoyuan9642 0:084d1dae2ea1 142 x = 0;
caoyuan9642 0:084d1dae2ea1 143 }
caoyuan9642 0:084d1dae2ea1 144 p++;
caoyuan9642 0:084d1dae2ea1 145 if (p - buffer0 == buffersize)
caoyuan9642 0:084d1dae2ea1 146 p = buffer0; // wrap around the buffer
caoyuan9642 0:084d1dae2ea1 147 }
caoyuan9642 0:084d1dae2ea1 148 }
caoyuan9642 0:084d1dae2ea1 149 }
caoyuan9642 0:084d1dae2ea1 150
caoyuan9642 0:084d1dae2ea1 151 LCDConsole::LCDConsole(const char * name, uint32_t color) :
caoyuan9642 0:084d1dae2ea1 152 FileLike(name)
caoyuan9642 0:084d1dae2ea1 153 {
caoyuan9642 0:084d1dae2ea1 154 this->color = color;
caoyuan9642 0:084d1dae2ea1 155 }
caoyuan9642 0:084d1dae2ea1 156
caoyuan9642 0:084d1dae2ea1 157 ssize_t LCDConsole::read(void* buffer, size_t size)
caoyuan9642 0:084d1dae2ea1 158 {
caoyuan9642 0:084d1dae2ea1 159 // Not supported
caoyuan9642 0:084d1dae2ea1 160 return -1;
caoyuan9642 0:084d1dae2ea1 161 }
caoyuan9642 0:084d1dae2ea1 162
caoyuan9642 0:084d1dae2ea1 163 ssize_t LCDConsole::write(const void* str, size_t size)
caoyuan9642 0:084d1dae2ea1 164 {
caoyuan9642 0:084d1dae2ea1 165 mutex.lock();
caoyuan9642 0:084d1dae2ea1 166 char *pb = (char*) str;
caoyuan9642 0:084d1dae2ea1 167 for (char *p = pb; p < pb + size; p++)
caoyuan9642 0:084d1dae2ea1 168 {
caoyuan9642 0:084d1dae2ea1 169 char c = (int) (*p);
caoyuan9642 0:084d1dae2ea1 170 bool scroll = false;
caoyuan9642 0:084d1dae2ea1 171 if (isprint(c))
caoyuan9642 0:084d1dae2ea1 172 {
caoyuan9642 0:084d1dae2ea1 173 *(tail++) = (color << 8) | c; // Put the current char and color into the buffer
caoyuan9642 0:084d1dae2ea1 174 if (tail >= buffer + buffersize)
caoyuan9642 0:084d1dae2ea1 175 tail -= buffersize;
caoyuan9642 0:084d1dae2ea1 176 if (tail == head)
caoyuan9642 0:084d1dae2ea1 177 scroll = true;
caoyuan9642 0:084d1dae2ea1 178 }
caoyuan9642 0:084d1dae2ea1 179 else if (c == '\b')
caoyuan9642 0:084d1dae2ea1 180 {
caoyuan9642 0:084d1dae2ea1 181 // Backspace, tail pointer go back by one
caoyuan9642 0:084d1dae2ea1 182 if (tail != head) // If the buffer is empty, do nothing
caoyuan9642 0:084d1dae2ea1 183 {
caoyuan9642 0:084d1dae2ea1 184 tail--;
caoyuan9642 0:084d1dae2ea1 185 if (tail < buffer)
caoyuan9642 0:084d1dae2ea1 186 tail += buffersize;
caoyuan9642 0:084d1dae2ea1 187 }
caoyuan9642 0:084d1dae2ea1 188
caoyuan9642 0:084d1dae2ea1 189 }
caoyuan9642 0:084d1dae2ea1 190 else if (c == '\r')
caoyuan9642 0:084d1dae2ea1 191 {
caoyuan9642 0:084d1dae2ea1 192 // Roll back to start of line
caoyuan9642 0:084d1dae2ea1 193 int currpos = (tail - head + buffersize) % buffersize; // current position in buffer
caoyuan9642 0:084d1dae2ea1 194 int linestart = currpos - currpos % textwidth; // Position of the start of the line
caoyuan9642 0:084d1dae2ea1 195 tail = head + linestart;
caoyuan9642 0:084d1dae2ea1 196 if (tail >= buffer + buffersize)
caoyuan9642 0:084d1dae2ea1 197 tail -= buffersize;
caoyuan9642 0:084d1dae2ea1 198 }
caoyuan9642 0:084d1dae2ea1 199 else if (c == '\n')
caoyuan9642 0:084d1dae2ea1 200 {
caoyuan9642 0:084d1dae2ea1 201 // Newline
caoyuan9642 0:084d1dae2ea1 202 int currpos = (tail - head + buffersize) % buffersize; // current position in buffer
caoyuan9642 0:084d1dae2ea1 203 int nextlinestart = currpos - currpos % textwidth + textwidth; // Position of a new line start
caoyuan9642 0:084d1dae2ea1 204 tail = head + nextlinestart;
caoyuan9642 0:084d1dae2ea1 205 if (tail >= buffer + buffersize)
caoyuan9642 0:084d1dae2ea1 206 tail -= buffersize;
caoyuan9642 0:084d1dae2ea1 207 if (nextlinestart >= buffersize) // Scroll if the tail will overrun the head
caoyuan9642 0:084d1dae2ea1 208 scroll = true;
caoyuan9642 0:084d1dae2ea1 209 }
caoyuan9642 0:084d1dae2ea1 210
caoyuan9642 0:084d1dae2ea1 211 // wrap
caoyuan9642 0:084d1dae2ea1 212
caoyuan9642 0:084d1dae2ea1 213 if (scroll)
caoyuan9642 0:084d1dae2ea1 214 {
caoyuan9642 0:084d1dae2ea1 215 // Scroll a line
caoyuan9642 0:084d1dae2ea1 216 head += textwidth; // Increase head by one line
caoyuan9642 0:084d1dae2ea1 217 if (head >= buffer + buffersize)
caoyuan9642 0:084d1dae2ea1 218 {
caoyuan9642 0:084d1dae2ea1 219 head -= buffersize; // wrap
caoyuan9642 0:084d1dae2ea1 220 }
caoyuan9642 0:084d1dae2ea1 221 for (int *p = tail; p != head;)
caoyuan9642 0:084d1dae2ea1 222 {
caoyuan9642 0:084d1dae2ea1 223 *(p++) = 0; // Set everything between tail and head to null
caoyuan9642 0:084d1dae2ea1 224 if (p >= buffer + buffersize)
caoyuan9642 0:084d1dae2ea1 225 {
caoyuan9642 0:084d1dae2ea1 226 p -= buffersize;
caoyuan9642 0:084d1dae2ea1 227 }
caoyuan9642 0:084d1dae2ea1 228 }
caoyuan9642 0:084d1dae2ea1 229 }
caoyuan9642 0:084d1dae2ea1 230 }
caoyuan9642 0:084d1dae2ea1 231 sem_update.release(); // Signal the task to update the graphics
caoyuan9642 0:084d1dae2ea1 232
caoyuan9642 0:084d1dae2ea1 233 mutex.unlock();
caoyuan9642 0:084d1dae2ea1 234 return size;
caoyuan9642 0:084d1dae2ea1 235 }
caoyuan9642 0:084d1dae2ea1 236
caoyuan9642 0:084d1dae2ea1 237 off_t LCDConsole::seek(off_t offset, int whence)
caoyuan9642 0:084d1dae2ea1 238 {
caoyuan9642 0:084d1dae2ea1 239 // Not supported
caoyuan9642 0:084d1dae2ea1 240 return -1;
caoyuan9642 0:084d1dae2ea1 241 }
caoyuan9642 0:084d1dae2ea1 242
caoyuan9642 0:084d1dae2ea1 243 int LCDConsole::close()
caoyuan9642 0:084d1dae2ea1 244 {
caoyuan9642 0:084d1dae2ea1 245 return 0;
caoyuan9642 0:084d1dae2ea1 246 }
caoyuan9642 0:084d1dae2ea1 247
caoyuan9642 0:084d1dae2ea1 248 void LCDConsole::redirect(bool tolcd)
caoyuan9642 0:084d1dae2ea1 249 {
caoyuan9642 0:084d1dae2ea1 250 if (tolcd)
caoyuan9642 0:084d1dae2ea1 251 {
caoyuan9642 0:084d1dae2ea1 252 freopen("/lcd_stdout", "w", stdout);
caoyuan9642 0:084d1dae2ea1 253 freopen("/lcd_stderr", "w", stderr);
caoyuan9642 0:084d1dae2ea1 254 }
caoyuan9642 0:084d1dae2ea1 255 else
caoyuan9642 0:084d1dae2ea1 256 {
caoyuan9642 0:084d1dae2ea1 257 freopen("/stdout", "w", stdout);
caoyuan9642 0:084d1dae2ea1 258 freopen("/stderr", "w", stderr);
caoyuan9642 0:084d1dae2ea1 259 }
caoyuan9642 0:084d1dae2ea1 260 }
caoyuan9642 0:084d1dae2ea1 261
caoyuan9642 0:084d1dae2ea1 262 // Override the default fatal error handler
caoyuan9642 0:084d1dae2ea1 263 extern "C" void error(const char* format, ...)
caoyuan9642 0:084d1dae2ea1 264 {
caoyuan9642 0:084d1dae2ea1 265 static unsigned char buffer[257];
caoyuan9642 0:084d1dae2ea1 266 core_util_critical_section_enter();
caoyuan9642 0:084d1dae2ea1 267 #ifndef NDEBUG
caoyuan9642 0:084d1dae2ea1 268
caoyuan9642 0:084d1dae2ea1 269 LCD_DISCO_F429ZI lcd; // Use local copy of the lcd, so that this function can be used at any stage of the initialization.
caoyuan9642 0:084d1dae2ea1 270 va_list arg;
caoyuan9642 0:084d1dae2ea1 271 va_start(arg, format);
caoyuan9642 0:084d1dae2ea1 272 int size = vsnprintf((char*) buffer, 256, format, arg);
caoyuan9642 0:084d1dae2ea1 273 if (size > 256)
caoyuan9642 0:084d1dae2ea1 274 {
caoyuan9642 0:084d1dae2ea1 275 // Properly terminate the string
caoyuan9642 0:084d1dae2ea1 276 size = 256;
caoyuan9642 0:084d1dae2ea1 277 buffer[256] = '\0';
caoyuan9642 0:084d1dae2ea1 278 }
caoyuan9642 0:084d1dae2ea1 279 va_end(arg);
caoyuan9642 0:084d1dae2ea1 280
caoyuan9642 0:084d1dae2ea1 281 BSP_LCD_SetFont(&Font20);
caoyuan9642 0:084d1dae2ea1 282
caoyuan9642 0:084d1dae2ea1 283 lcd.Clear(LCD_COLOR_WHITE);
caoyuan9642 0:084d1dae2ea1 284 lcd.SetBackColor(LCD_COLOR_WHITE);
caoyuan9642 0:084d1dae2ea1 285 lcd.SetTextColor(LCD_COLOR_RED);
caoyuan9642 0:084d1dae2ea1 286
caoyuan9642 0:084d1dae2ea1 287 int i = 0, line = 0;
caoyuan9642 0:084d1dae2ea1 288 while (i < size)
caoyuan9642 0:084d1dae2ea1 289 {
caoyuan9642 0:084d1dae2ea1 290 unsigned char c = buffer[i + 16];
caoyuan9642 0:084d1dae2ea1 291 buffer[i + 16] = '\0';
caoyuan9642 0:084d1dae2ea1 292 lcd.DisplayStringAtLine(line++, buffer + i);
caoyuan9642 0:084d1dae2ea1 293 buffer[i + 16] = c;
caoyuan9642 0:084d1dae2ea1 294 i += 16;
caoyuan9642 0:084d1dae2ea1 295 }
caoyuan9642 0:084d1dae2ea1 296 #endif
caoyuan9642 0:084d1dae2ea1 297
caoyuan9642 0:084d1dae2ea1 298 core_util_critical_section_exit();
caoyuan9642 0:084d1dae2ea1 299 exit(1);
caoyuan9642 0:084d1dae2ea1 300 }
caoyuan9642 0:084d1dae2ea1 301
caoyuan9642 0:084d1dae2ea1 302