static void QueueMessage(uint8_t kind, const char *msg, uint8_t msgSize) {
#if 0
/* format is: kind(8bit) dataSize(8bit) data */
uint8_t i, buf[RADIO_QUEUE_ITEM_SIZE];
signed portBASE_TYPE pxHigherPriorityTaskWoken;
buf[0] = kind;
buf[1] = msgSize;
i = 2;
while(msgSize>0 && i<sizeof(buf)) {
buf[i++] = *msg;
msg++;
msgSize--;
}
if (FRTOS1_xQueueSendToBackFromISR(RADIO_MsgQueue, &buf[0], &pxHigherPriorityTaskWoken)!=pdTRUE) {
/* was not able to send to the queue. Well, not much we can do here... */
}
#endif
}
void I2C_OnReceiveData(void) {
#if PL_IS_I2C_SLAVE
uint8_t data, res;
uint16_t nofBytesReceived;
res = I2C1_RecvBlock(&data, 1, &nofBytesReceived);
if (res == ERR_OK && nofBytesReceived==1) { /* ok, no error */
if (bufIdx==0 && data==I2C_PRE_BYTE0) {
buf[0] = data;
bufIdx++;
return;
} else if (bufIdx==1 && data==I2C_PRE_BYTE1 && buf[0]==I2C_PRE_BYTE0) {
buf[1] = data;
bufIdx++;
return;
} else if (bufIdx==2) {
buf[2] = data; /* size */
bufIdx++;
return;
/*------------------------------------------------*/
} else if (bufIdx==3 && data==I2C_MSG_MOTOR_SPEED && buf[2]==I2C_MOTOR_MSG_LEN) { /* start of motor speed message */
buf[3] = data; /* I2C_MSG_MOTOR_SPEED */
bufIdx++;
return;
} else if (bufIdx>=4 && buf[3]==I2C_MSG_MOTOR_SPEED && bufIdx <= I2C_MOTOR_MSG_MAX_IDX) { /* motor speed message */
buf[bufIdx++] = data;
/*------------------------------------------------*/
} else if (bufIdx==3 && data==I2C_MSG_PID_SPEED && buf[2]==I2C_PID_MSG_LEN) { /* start of motor speed message */
buf[3] = data; /* I2C_MSG_PID_SPEED */
bufIdx++;
return;
} else if (bufIdx>=4 && buf[3]==I2C_MSG_PID_SPEED && bufIdx <= I2C_PID_MSG_MAX_IDX) { /* motor speed message */
buf[bufIdx++] = data;
/*------------------------------------------------*/
} else if (bufIdx==3 && data==I2C_ACCEL_XYZ && buf[2]==I2C_ACCEL_MSG_LEN) { /* start of Accel message */
buf[3] = data; /* I2C_ACCEL_XYZ */
bufIdx++;
return;
} else if (bufIdx>=4 && buf[3]==I2C_ACCEL_XYZ && bufIdx <= I2C_ACCEL_MSG_MAX_IDX) { /* Accel message */
buf[bufIdx++] = data;
/*------------------------------------------------*/
} else if (bufIdx==3 && data==I2C_MSG_GET_CMD && buf[2]==I2C_GET_CMD_MSG_LEN) { /* start of GetCMD message */
buf[3] = data; /* I2C_GET_CMD */
bufIdx++;
return;
} else if (bufIdx>=4 && buf[3]==I2C_MSG_GET_CMD && bufIdx <= I2C_GET_CMD_MSG_MAX_IDX) { /* GetCmd message */
buf[bufIdx++] = data;
/*------------------------------------------------*/
#if PL_HAS_RUNNER
} else if (bufIdx==3 && data==I2C_MSG_RUNNER ) { /* start of runner message */
buf[3] = data; /* I2C_MSG_RUNNER */
bufIdx++;
return;
} else if (bufIdx>=4 && buf[3]==I2C_MSG_RUNNER && bufIdx < sizeof(buf)) { /* runner message */
buf[bufIdx++] = data;
if (data != '\0') {
return; /* get next byte */
}
#endif
} else {
bufIdx = 0;
return;
}
} else { /* error */
bufIdx = 0;
return;
}
if (data==0) { /* message finished? */
if (buf[3]==I2C_ACCEL_XYZ && buf[2]==I2C_ACCEL_MSG_LEN && bufIdx>I2C_ACCEL_MSG_MAX_IDX) { /* message finished */
AccelX = (int16_t)((buf[4]<<8)|buf[5]);
AccelY = (int16_t)((buf[6]<<8)|buf[7]);
AccelZ = (int16_t)((buf[8]<<8)|buf[9]);
bufIdx = 0; /* message completed */
} else if (buf[3]==I2C_MSG_MOTOR_SPEED && buf[2]==I2C_MOTOR_MSG_LEN && bufIdx>I2C_MOTOR_MSG_MAX_IDX) { /* message finished */
motorSpeed = (int32_t)((buf[4]<<24)|(buf[5]<<16)|(buf[6]<<8)|buf[7]);
bufIdx = 0; /* message completed */
} else if (buf[3]==I2C_MSG_PID_SPEED && buf[2]==I2C_PID_MSG_LEN && bufIdx>I2C_PID_MSG_MAX_IDX) { /* message finished */
PIDSpeed = (int32_t)((buf[4]<<24)|(buf[5]<<16)|(buf[6]<<8)|buf[7]);
bufIdx = 0; /* message completed */
} else if (buf[3]==I2C_MSG_GET_CMD && buf[2]==I2C_GET_CMD_MSG_LEN && bufIdx>I2C_GET_CMD_MSG_MAX_IDX) { /* message finished */
sendCmd = TRUE;
bufIdx = 0; /* message completed */
#if PL_HAS_RUNNER
} else if (buf[3]==I2C_MSG_RUNNER) { /* message finished */
UTIL1_strcpy(RunnerBuf, sizeof(RunnerBuf), &buf[4]);
bufIdx = 0; /* message completed */
if (I2C_RunnerQueue!=NULL) {
signed portBASE_TYPE higherTaskWoken;
FRTOS1_xQueueSendToBackFromISR(I2C_RunnerQueue, RunnerBuf, &higherTaskWoken);
}
#endif
}
}
if (bufIdx>sizeof(buf)) {
bufIdx = 0;
}
#else
#if PL_APP_MODE_I2C_TWR
byte res;
byte buf[1];
word nofBytesReceived;
res = I2C1_RecvBlock(&buf, sizeof(buf), &nofBytesReceived);
if (res == ERR_OK) {
switch(buf[0]) {
case EVNT1_BUTTON_UI_LEFT:
case EVNT1_BUTTON_UI_RIGHT:
case EVNT1_BUTTON_UI_UP:
case EVNT1_BUTTON_UI_DOWN:
case EVNT1_BUTTON_UI_MIDDLE:
EVNT1_SetEvent(buf[0]);
break;
} /* switch */
}
#endif
#endif
}
void TI1_OnInterrupt(LDD_TUserData *UserDataPtr)
{
ti2_counter++;
FRTOS1_xQueueSendToBackFromISR(eventHandle, NULL, NULL);
}
