ISR(TIMER5_COMPA_vect, ISR_NOBLOCK){ if (watchDogStartCount == true){ watchDogFailSafeCounter++; if (rcDetected == true){ RCFailSafeCounter++; } groundFSCount++; GPSFailSafeCounter++; baroFSCount++; telemFSCount++; } else{ LEDPatternHandler(micros()); } if (rcType != RC){ FeedLine(); } if (watchDogFailSafeCounter >=200){ TIMSK5 = (0<<OCIE5A); Motor1WriteMicros(0); Motor2WriteMicros(0); Motor3WriteMicros(0); Motor4WriteMicros(0); Motor5WriteMicros(0); Motor6WriteMicros(0); Motor7WriteMicros(0); Motor8WriteMicros(0); LEDPatternSet(0,1,0,1); while(1){ LEDPatternHandler(micros()); } } }
CON(int, wputs(const wchar_t* str)) { PrintConsole(str); if(!sConsole.postponedLineFeed) FeedLine(); return 0; }
void PrintConsole(const wchar_t *str) { int length, pos = 0; wchar_t* line; if (gConsoleLogging) { static const char prefix[] = "PrintConsole: "; maWriteLog(prefix, strlen(prefix)); length = wcslen(str); if (length > 0) { #ifdef MAPIP char buf8[length * MB_LEN_MAX]; #else char* buf8 = (char*)malloc(length * MB_LEN_MAX); #endif int len8; //maWriteLog(str, length*sizeof(wchar_t)); //convert to utf-8 memset(buf8, 0, length * MB_LEN_MAX); len8 = wcstombs(buf8, str, length * MB_LEN_MAX); maWriteLog(buf8, len8); if (str[length - 1] != '\n') maWriteLog("\n", 1); #ifndef MAPIP free(buf8); #endif } } if(gConsoleFile > 0) { int res = maFileWrite(gConsoleFile, str, wcslen(str)); if(res < 0) { maPanic(res, "PrintConsole maFileWrite"); } } if (!sConsole.initialized) InitConsole(); if (sConsole.postponedLineFeed) { FeedLine(); sConsole.postponedLineFeed = 0; } while (str[pos] != '\0') { if (str[pos] == '\r') sConsole.cursorPos.x = 0; else if (str[pos] == '\n') { if (str[pos + 1] == '\0') sConsole.postponedLineFeed = 1; else FeedLine(); } else { line = sConsole.lines[(sConsole.cursorPos.y + sConsole.firstLine) % sConsole.height].line; line[sConsole.cursorPos.x++] = str[pos]; if (sConsole.cursorPos.x >= CONSOLE_WIDTH) { if (str[pos + 1] == '\0') sConsole.postponedLineFeed = 1; else FeedLine(); } } pos++; } if(gConsoleForceDisplay) DisplayConsole(); }