unsigned char* read(void)
{
askAboutSending();
unsigned char *readBuffer = NULL;
int statusRxFifoFilling = 0;
int howManyRead = 0;
statusRxFifoFilling = USIC_GetRxFIFOFillingLevel(UART001_0_USIC_CH);
if(((UART001_GetFlagStatus(&UART001_Handle0,UART001_FIFO_STD_RECV_BUF_FLAG)) == UART001_SET))
{
statusRxFifoFilling = USIC_GetRxFIFOFillingLevel(UART001_0_USIC_CH);
if(statusRxFifoFilling != 0)
{
readBuffer = (unsigned char *)malloc((sizeof(unsigned char)*statusRxFifoFilling) + 1);
howManyRead = UART001_ReadDataBytes(&UART001_Handle0, readBuffer, statusRxFifoFilling);
readBuffer[statusRxFifoFilling] = '\0';
}
UART001_ClearFlag(&UART001_Handle0,UART001_FIFO_STD_RECV_BUF_FLAG);
while(!USIC_ubIsRxFIFOempty(UART001_0_USIC_CH))
{
USIC_FlushRxFIFO(UART001_0_USIC_CH);
}
}
stopAskingAboutSending();
return readBuffer;
}
void read(void)
{
emptyReadData();
if(((UART001_GetFlagStatus(&UART001_Handle0,UART001_FIFO_STD_RECV_BUF_FLAG)) == UART001_SET))
{
IO004_TogglePin(IO004_Handle1);
statusRxFifoFilling = USIC_GetRxFIFOFillingLevel(UART001_0_USIC_CH);
howManyRead = UART001_ReadDataBytes(&UART001_Handle0, readData, statusRxFifoFilling);
if(strcmp(readData, "LSM9DS1") == 0)
{
chosenSensor = 1;
}
else if(strcmp(readData, "TEMP_SENSOR") == 0)
{
chosenSensor = 2;
}
UART001_ClearFlag(&UART001_Handle0,UART001_FIFO_STD_RECV_BUF_FLAG);
while(!USIC_ubIsRxFIFOempty(UART001_0_USIC_CH))
{
USIC_FlushRxFIFO(UART001_0_USIC_CH);
}
copyData();
stopAskingAboutSending();
}
}
void DaisyChain(void)
{
uint8_t i=0;
status_t status=0;
uint32_t data=0;
static enum MotorState LastmotorState = Stopped;
if ( motorState == StartUp )
{
CharCNT = 0;
USIC_FlushRxFIFO(UART001_Handle0.UartRegs);
}
if ( (LastmotorState == StartUp) && (motorState != Running) )//(motorState == Running)StartUp
{
CCU40_CC42->TCCLR |= 0x02; //定时器清零
//Start slicesCCU4定时器运行
CCU40_CC42->TCSET |= 0x01UL;
CharCNT++;
if ( CharCNT >= 13 ) //能收到连续的13字节完整包
{
CharCNT = 0;
LastmotorState = motorState;
CCU40_CC42->TCCLR |= 3UL; //定时器停止运行
USIC_FlushRxFIFO(UART001_Handle0.UartRegs);///added
}
else
{
return;
}
}
else
LastmotorState = motorState;
// if (DaisyTimeOut)
// StopMotor();
if(USIC_GetRxFIFOFillingLevel(UART001_Handle0.UartRegs) >= DAISY_BUFFER_SIZE)
{
CCU40_CC42->TCCLR |= 3UL; //定时器停止运行
//Read data from UART buffer
UART001_ReadDataBytes(&UART001_Handle0,FifoRecBuffer,DAISY_BUFFER_SIZE);
//Assumption that communication is lost --> emtpy Receive Buffer
if (FifoRecBuffer[DAISY_BUFFER_SIZE-1] != DAISY_STOP_BYTE)
{
//IO004_TogglePin(IO004_Handle1);
USIC_FlushRxFIFO(UART001_Handle0.UartRegs);
return;
}
uint8_t cmd = FifoRecBuffer[0];
uint16_t params = (FifoRecBuffer[1] << 8 | FifoRecBuffer[2]);
switch (cmd)
{
case START_MOTOR:
StartMotor();
break;
case STOP_MOTOR:
StopMotor();
break;
case SET_REF_CURRENT:
SetReferenceCurrent(params);
break;
}
for(i=DAISY_MESSAGE_LENGTH; i<DAISY_BUFFER_SIZE-1; i++)
FifoTransBuffer[i-DAISY_MESSAGE_LENGTH]=FifoRecBuffer[i];
//Status-Code
FifoTransBuffer[i-DAISY_MESSAGE_LENGTH]=status;
i++;
//Data
FifoTransBuffer[i-DAISY_MESSAGE_LENGTH]=(uint8_t)(data >> 8);
i++;
FifoTransBuffer[i-DAISY_MESSAGE_LENGTH]=(uint8_t)data;
i++;
FifoTransBuffer[i-DAISY_MESSAGE_LENGTH]=DAISY_STOP_BYTE;
DaisyTimeOut = 0;
DaisyCount++;
UART001_WriteDataBytes(&UART001_Handle0, FifoTransBuffer, DAISY_BUFFER_SIZE);
}
void Initialize()
{
NVIC002_DisableIRQ(&ESC_UART_RX_NVIC_Handle);
//Stop Monitoring-Timer
PWMSP001_Stop(&Monitoring_Timer);
// initialize device
MPU9150_Setup();
delay(1000); //Delay 4000?
// initilize controller polynomials
b_roll[0]=P_roll-I_roll*T-P_roll*N_roll*T+N_roll*I_roll*T*T+D_roll*N_roll;
b_roll[1]=I_roll*T-2*P_roll+P_roll*N_roll*T-2*D_roll*N_roll;
b_roll[2]=P_roll+D_roll*N_roll;
a_roll[0]=1-N_roll*T;
a_roll[1]=N_roll*T-2;
b_pitch[0]=P_pitch-I_pitch*T-P_pitch*N_pitch*T+N_pitch*I_pitch*T*T+D_pitch*N_pitch;
b_pitch[1]=I_pitch*T-2*P_pitch+P_pitch*N_pitch*T-2*D_pitch*N_pitch;
b_pitch[2]=P_pitch+D_pitch*N_pitch;
a_pitch[0]=1-N_pitch*T;
a_pitch[1]=N_pitch*T-2;
WatchRC_Init(); //Initialize RC watchdog
#ifdef UART_SC_IF
uint32_t data=0;
uint8_t trans = MCI_DAISY_INIT_BYTE;
uint8_t rec = 0;
UART001_WriteDataBytes(&ESC_UART_Handle, &trans, 1);
while (rec != 111)
while(!UART001_ReadDataBytes(&ESC_UART_Handle, &rec, 1));
// if init-Byte has been received empty IO buffer
while(UART001_ReadDataBytes(&ESC_UART_Handle, &rec, 1));
//Motor 1
DaisyTransmit[0]=MOTOR_START;
data=0;
DaisyTransmit[1]=(uint8_t)(data>>8);
DaisyTransmit[2]=(uint8_t)data;
//Motor 2
DaisyTransmit[3]=MOTOR_START;
data=0;
DaisyTransmit[4]=(uint8_t)(data>>8);
DaisyTransmit[5]=(uint8_t) data;
//Motor 3
DaisyTransmit[6]=MOTOR_START;
data=0;
DaisyTransmit[7]=(uint8_t)(data>>8);
DaisyTransmit[8]=(uint8_t) data;
//Motor 4
DaisyTransmit[9]=MOTOR_START;
data=0;
DaisyTransmit[10]=(uint8_t)(data>>8);
DaisyTransmit[11]=(uint8_t) data;
UART001_WriteDataBytes(&ESC_UART_Handle, DaisyTransmit, 12);
#endif
PWMSP001_Start(&AttControl_Timer);
}
