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);
}
}
}
void TaskLoop(void *pvParameters){
int16_t x,y,z;
#if PL_HAS_ACCEL
ACCEL_LowLevelInit();
#endif
(void)pvParameters;
for(;;){
#if PL_HAS_KEYS
KEY_Scan();
#endif
#if PL_HAS_LINE_SENSOR
typ = Line_Detection();
if(typ!=LINE_STATE_NO_LINE){
EVNT_SetEvent(EVNT_LINE);
}
switch(typ) {
case LINE_STATE_LEFT:
EVNT_SetEvent(EVNT_LINE_LEFT);
break;
case LINE_STATE_RIGHT:
EVNT_SetEvent(EVNT_LINE_RIGHT);
break;
case LINE_STATE_AHEAD:
EVNT_SetEvent(EVNT_LINE);
break;
case LINE_STATE_MIDDLE:
EVNT_SetEvent(EVNT_LINE);
break;
case LINE_STATE_NO_LINE:
/* No event */
break;
case LINE_STATE_LINE:
EVNT_SetEvent(EVNT_LINE);
break;
case LINE_STATE_ERR:
EVNT_SetEvent(EVNT_LINE);
break;
default:
/* No event */
break;
}
#endif
#if PL_HAS_ACCEL
ACCEL_GetValues(&x,&y,&z);
if(z < Z_ACCEL_THRESHOLD){
EVNT_SetEvent(EVNT_ACCEL);
}
#endif
#if PL_HAS_EVENTS
EVNT_HandleEvent(APP_EventHandler);
#endif
FRTOS1_vTaskDelay(5/portTICK_RATE_MS);
}
}
static portTASK_FUNCTION(AccelObserv, pvParameters) {
for(;;) {
static int16_t x,y,z;
ACCEL_GetValues(&x, &y, &z);
if(z < -200){
EVNT_SetEvent(EVNT_FREEFALL);
}
FRTOS1_vTaskDelay(10/TRG_TICKS_MS);
}
}
static portTASK_FUNCTION(RemoteTask, pvParameters) {
#if PL_HAS_WATCHDOG
int i;
#endif
(void)pvParameters;
#if PL_HAS_JOYSTICK
(void)APP_GetXY(&midPointX, &midPointY, NULL, NULL);
#endif
for(;;) {
if (REMOTE_isOn) {
#if PL_HAS_ACCEL
if (REMOTE_useAccelerometer) {
#if PL_HAS_KEYS
uint8_t buf[7];
uint8_t keys;
#else /* PL_HAS_KEYS */
uint8_t buf[6];
#endif /* PL_HAS_KEYS */
int16_t x, y, z;
/* send periodically accelerometer messages */
#if PL_HAS_KEYS
APP_GetKeys(&keys);
#endif /* PL_HAS_KEYS */
ACCEL_GetValues(&x, &y, &z);
buf[0] = (uint8_t)(x&0xFF);
buf[1] = (uint8_t)(x>>8);
buf[2] = (uint8_t)(y&0xFF);
buf[3] = (uint8_t)(y>>8);
buf[4] = (uint8_t)(z&0xFF);
buf[5] = (uint8_t)(z>>8);
#if PL_HAS_KEYS
buf[6] = keys;
#endif /* PL_HAS_KEYS */
if (REMOTE_isVerbose) {
uint8_t txtBuf[48];
UTIL1_strcpy(txtBuf, sizeof(txtBuf), (unsigned char*)"TX: x: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), x);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" y: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), y);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" z: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), z);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" to addr 0x");
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#else
UTIL1_strcatNum16Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#endif
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)"\r\n");
SHELL_SendString(txtBuf);
}
(void)RAPP_SendPayloadDataBlock(buf, sizeof(buf), RAPP_MSG_TYPE_ACCEL, RNETA_GetDestAddr(), RPHY_PACKET_FLAGS_REQ_ACK);
LED1_Neg();
}
#endif
#if PL_HAS_JOYSTICK
if (REMOTE_useJoystick) {
#if PL_HAS_KEYS
uint8_t buf[3];
uint8_t keys;
#else /* PL_HAS_KEYS */
uint8_t buf[2];
#endif /* PL_HAS_KEYS */
int16_t x, y;
int8_t x8, y8;
/* send periodically accelerometer messages */
#if PL_HAS_KEYS
APP_GetKeys(&keys);
#endif /* PL_HAS_KEYS */
APP_GetXY(&x, &y, &x8, &y8);
buf[0] = x8;
buf[1] = y8;
#if PL_HAS_KEYS
buf[2] = keys;
#endif /* PL_HAS_KEYS */
if (REMOTE_isVerbose) {
uint8_t txtBuf[48];
UTIL1_strcpy(txtBuf, sizeof(txtBuf), (unsigned char*)"TX: x: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), x8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" y: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), y8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" to addr 0x");
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#else
UTIL1_strcatNum16Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#endif
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)"\r\n");
SHELL_SendString(txtBuf);
}
(void)RAPP_SendPayloadDataBlock(buf, sizeof(buf), RAPP_MSG_TYPE_JOYSTICK_XY, RNETA_GetDestAddr(), RPHY_PACKET_FLAGS_REQ_ACK);
LED1_Neg();
}
#endif
#if PL_HAS_WATCHDOG
for(i=0; i<2; i++) { /* do it in smaller steps */
WDT_IncTaskCntr(WDT_TASK_ID_REMOTE, 100);
FRTOS1_vTaskDelay(100/portTICK_RATE_MS);
}
#else
FRTOS1_vTaskDelay(200/portTICK_RATE_MS);
#endif
} else {
#if PL_HAS_WATCHDOG
for(i=0; i<10; i++) { /* do it in smaller steps */
WDT_IncTaskCntr(WDT_TASK_ID_REMOTE, 100);
FRTOS1_vTaskDelay(100/portTICK_RATE_MS);
}
#else
FRTOS1_vTaskDelay(1000/portTICK_RATE_MS);
#endif
}
} /* for */
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 */
}