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();
}
