static portTASK_FUNCTION(TraceTask, pvParameters) { static unsigned char buf[128]; /* global buffer, to reduce stack size. We want to send things in one piece */ (void)pvParameters; for(;;) { if (traceChannel==TRACE_TO_NONE) { FRTOS1_vTaskDelay(1000/portTICK_RATE_MS); } else if (traceChannel==TRACE_TO_SHELL) { buf[0] = '\0'; UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" => "); if (traceAccel) { int16_t x, y, z; ACCEL_GetValues(&x, &y, &z); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" X:"); UTIL1_strcatNum16sFormatted(buf, sizeof(buf), x, ' ', 6); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Y:"); UTIL1_strcatNum16sFormatted(buf, sizeof(buf), y, ' ', 6); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Z:"); UTIL1_strcatNum16sFormatted(buf, sizeof(buf), z, ' ', 6); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n"); CLS1_SendStr(&buf[0], CLS1_GetStdio()->stdOut); FRTOS1_vTaskDelay(100/portTICK_RATE_MS); } FRTOS1_vTaskDelay(25/portTICK_RATE_MS); } } }
static void strcatTimeFormatted(char *buf, size_t bufSize, unsigned char hour, unsigned char min, unsigned char sec, unsigned char sec10) { UTIL1_strcatNum16sFormatted(buf, bufSize, hour, '0', 2); UTIL1_strcat(buf, bufSize, ":"); UTIL1_strcatNum16sFormatted(buf, bufSize, min, '0', 2); UTIL1_strcat(buf, bufSize, ":"); UTIL1_strcatNum16sFormatted(buf, bufSize, sec, '0', 2); UTIL1_strcat(buf, bufSize, ","); UTIL1_strcatNum16sFormatted(buf, bufSize, sec10, '0', 1); }
static portTASK_FUNCTION(TraceTask, pvParameters) { unsigned char buf[32]; (void)pvParameters; //ACCEL_LowLevelInit(); /* cannot do this in ACCEL_Init(), as there interrupts are disabled */ for(;;) { if (traceChannel==TRACE_TO_NONE) { /* do nothing */ } else if (traceChannel==TRACE_TO_SHELL) { buf[0] = '\0'; UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" => "); #if PL_HAS_MOTOR if (traceMotor) { buf[0] = '\0'; if (MOT_GetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT))==MOT_DIR_FORWARD) { UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: fw 0x"); } else { UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: bw 0x"); } UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT))); UTIL1_chcat(buf, sizeof(buf), ';'); #if PL_HAS_SHELL SHELL_SendString(&buf[0]); #endif } #endif #if PL_HAS_MOTOR_TACHO if (traceTacho) { buf[0] = '\0'; UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" V:"); UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(FALSE), ' ', 6); UTIL1_chcat(buf, sizeof(buf), ' '); UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(TRUE), ' ', 6); UTIL1_chcat(buf, sizeof(buf), ';'); #if PL_HAS_SHELL SHELL_SendString(&buf[0]); #endif } #endif #if PL_HAS_QUADRATURE if (traceQuad) { buf[0] = '\0'; UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" P:"); UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_GetPos(), ' ', 6); UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_GetPos(), ' ', 6); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; E:"); UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_NofErrors(), ' ', 6); UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_NofErrors(), ' ', 6); UTIL1_chcat(buf, sizeof(buf), ';'); #if PL_HAS_SHELL SHELL_SendString(&buf[0]); #endif } #endif #if PL_HAS_ACCEL if (traceAccel) { int16_t x, y, z; buf[0] = '\0'; ACCEL_GetValues(&x, &y, &z); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" X:"); UTIL1_strcatNum16sFormatted(buf, sizeof(buf), x, ' ', 6); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Y:"); UTIL1_strcatNum16sFormatted(buf, sizeof(buf), y, ' ', 6); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Z:"); UTIL1_strcatNum16sFormatted(buf, sizeof(buf), z, ' ', 6); UTIL1_chcat(buf, sizeof(buf), ';'); #if PL_HAS_SHELL SHELL_SendString(&buf[0]); #endif } #endif UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"\r\n"); #if PL_HAS_SHELL SHELL_SendString(&buf[0]); #endif FRTOS1_vTaskDelay(20/portTICK_RATE_MS); /* slow down writing to console */ } FRTOS1_vTaskDelay(20/portTICK_RATE_MS); } /* for */ }