void Monitoring_TIMER_ISR()
{
// Start byte
MonitorBuffer[0] = 0x02;
// YAW desired
MonitorBuffer[1] = (int8_t)((int16_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_YAW) * (yawD_dot - SCALE_YAW)) >> 8);
MonitorBuffer[2] = (int8_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_YAW) * (yawD_dot - SCALE_YAW));
// YAW
MonitorBuffer[3] = (int8_t)((int16_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_YAW) * (yaw_dot - SCALE_YAW)) >> 8);
MonitorBuffer[4] = (int8_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_YAW) * (yaw_dot - SCALE_YAW));
// Pitch desired
MonitorBuffer[5] = (int8_t)((int16_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_PITCH) * (pitchD - SCALE_PITCH)) >> 8);
MonitorBuffer[6] = (int8_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_PITCH) * (pitchD - SCALE_PITCH));
// Pitch
MonitorBuffer[7] = (int8_t)((int16_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_PITCH) * (YPR[1] - SCALE_PITCH)) >> 8);
MonitorBuffer[8] = (int8_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_PITCH) * (YPR[1] - SCALE_PITCH));
// Roll desired
MonitorBuffer[9] = (int8_t)((int16_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_ROLL) * (rollD - SCALE_ROLL)) >> 8);
MonitorBuffer[10] = (int8_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_ROLL) * (rollD - SCALE_ROLL));
// Roll
MonitorBuffer[11] = (int8_t)((int16_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_ROLL) * (YPR[2] - SCALE_ROLL)) >> 8);
MonitorBuffer[12] = (int8_t)(SHRT_MAX + (SHRT_MAX - SHRT_MIN) / (2*SCALE_ROLL) * (YPR[2] - SCALE_YAW));
// Stop byte
MonitorBuffer[13] = 0x03;
UART001_WriteDataBytes(&BT_UART_Handle, (uint8_t*)MonitorBuffer, 14);
}
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);
}
int main(void)
{
Control_P0_9(OUTPUT_PP_GP, VERYSTRONG);
Control_P3_2(OUTPUT_PP_GP, VERYSTRONG);
Control_P3_1(OUTPUT_PP_GP, VERYSTRONG);
Control_P3_0(OUTPUT_PP_GP, VERYSTRONG);
Control_P0_2(OUTPUT_PP_GP, VERYSTRONG);
Control_P0_3(OUTPUT_PP_GP, VERYSTRONG);
Control_P2_0(OUTPUT_PP_GP, VERYSTRONG);
Control_P2_7(OUTPUT_PP_GP, VERYSTRONG);
SET_P0_2;
SET_P0_3;
SET_P2_0;
SET_P2_7;
#ifdef LED_test
int ortime=0L;
while(ortime<10){
for (int orcount = 0; orcount<200000; orcount++)__NOP();
TOGGLE_P3_0;
for (int orcount = 0; orcount<200000; orcount++)__NOP();
TOGGLE_P3_1;
for (int orcount = 0; orcount<200000; orcount++)__NOP();
TOGGLE_P3_2;
for (int orcount = 0; orcount<200000; orcount++)__NOP();
TOGGLE_P0_9;
ortime++;
}
#endif
DAVE_Init(); // Initialization of DAVE Apps
Initialize();
Control_P0_0(INPUT, STRONG); //SET UART PIN to TRISTATE: necessary with LARIX_V3
Control_P0_1(INPUT, STRONG); //SET UART PIN to TRISTATE: necessary with LARIX_V3
#ifdef DPS310
SensorError err = setupDPS310();
if(err)
ortime=0;
while(ortime<20){
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_0;
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_2;
ortime++;
}
#endif
// /* Initialization of the USBD_VCOM App */
if(USBD_VCOM_Init() != DAVEApp_SUCCESS)
{
while(1){
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_0;
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_1;
ortime++;
}
return -1;
}
#ifdef SERIAL_DEBUG
uint32_t power = 0;
#endif
#ifdef DEBUG_SPECIFIC
#undef DEBUG_CONTINOUS
uint32_t p = 0;
uint32_t t = 0;
#endif
#ifdef DEBUG_CONTINOUS
uint32_t counterccc = 0;
uint32_t helper = 0;
#endif
ortime=0;
while(ortime<20){
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_0;
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_1;
ortime++;
}
while(1)
{
updateValues(&p,&t);
#ifndef SERIAL_DEBUG
if(newvalue){
CalculateActuatorSpeed_Percent(&u_roll, &u_pitch, &u_yaw_dot, &powerD, actuator_speed_percent, &YPR[1], &YPR[2]);
#ifdef PWM_OUTPUT
//Scale percent Output
PWM_width[0]=0.45*actuator_speed_percent[3]+45;
PWM_width[1]=0.45*actuator_speed_percent[2]+45;
PWM_width[2]=0.45*actuator_speed_percent[0]+45;
PWM_width[3]=0.45*actuator_speed_percent[1]+45;
//set actors //normal//LARIX with PWMoutput
#ifdef LARIX_with_PWM_used
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[3]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[2]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[0]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[1]);
#endif
#ifdef WIDEFIELD_used
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[0]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[1]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[2]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[3]);
// #else
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[0]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[1]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[2]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[3]);
#endif
#endif
#ifdef UART_SC_IF
#ifdef IRMCK
uint32_t data_TX;
//Motor 1
DaisyTransmit[0]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[0]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[1]=(uint8_t)(data_TX>>8);
DaisyTransmit[2]=(uint8_t) data_TX;
//Motor 2
DaisyTransmit[3]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[1]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[4]=(uint8_t)(data_TX>>8);
DaisyTransmit[5]=(uint8_t) data_TX;
//Motor 3
DaisyTransmit[6]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[2]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[7]=(uint8_t)(data_TX>>8);
DaisyTransmit[8]=(uint8_t) data_TX;
//Motor 4
DaisyTransmit[9]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[3]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[10]=(uint8_t)(data_TX>>8);
DaisyTransmit[11]=(uint8_t) data_TX;
UART001_WriteDataBytes(&ESC_UART_Handle, DaisyTransmit, 12);
#else
uint16_t data;
//Motor 1
DaisyTransmit[0]=SET_MOTOR_SPEED;
data=actuator_speed_percent[0]/100.0*0xFFFF;
DaisyTransmit[1]=(uint8_t)(data>>8);
DaisyTransmit[2]=(uint8_t) data;
//Motor 2
DaisyTransmit[3]=SET_MOTOR_SPEED;
data=actuator_speed_percent[1]/100.0*0xFFFF;
DaisyTransmit[4]=(uint8_t)(data>>8);
DaisyTransmit[5]=(uint8_t) data;
//Motor 3
DaisyTransmit[6]=SET_MOTOR_SPEED;
data=actuator_speed_percent[2]/100.0*0xFFFF;
DaisyTransmit[7]=(uint8_t)(data>>8);
DaisyTransmit[8]=(uint8_t) data;
//Motor 4
DaisyTransmit[9]=SET_MOTOR_SPEED;
data=actuator_speed_percent[3]/100.0*0xFFFF;
DaisyTransmit[10]=(uint8_t)(data>>8);
DaisyTransmit[11]=(uint8_t) data;
UART001_WriteDataBytes(&UART001_Handle2, DaisyTransmit, 12);
#endif
#endif
newvalue=0;
}
#else
#ifdef PWM_OUTPUT
//Scale percent Output
actuator_speed_percent[0]=powerD;
actuator_speed_percent[1]=powerD;
actuator_speed_percent[2]=powerD;
actuator_speed_percent[3]=powerD;
PWM_width[0]=0.45*actuator_speed_percent[0]+45;
PWM_width[1]=0.45*actuator_speed_percent[1]+45;
PWM_width[2]=0.45*actuator_speed_percent[2]+45;
PWM_width[3]=0.45*actuator_speed_percent[3]+45;
//set actors //normal//LARIX with PWMoutput
#ifdef LARIX_with_PWM_used
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[3]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[2]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[0]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[1]);
#endif
#ifdef WIDEFIELD_used
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[0]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[1]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[2]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[3]);
// #else
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[0]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[1]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[2]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[3]);
#endif
#endif
#ifdef UART_SC_IF
#ifdef IRMCK
uint32_t data_TX;
//Motor 1
DaisyTransmit[0]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[0]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[1]=(uint8_t)(data_TX>>8);
DaisyTransmit[2]=(uint8_t) data_TX;
//Motor 2
DaisyTransmit[3]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[1]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[4]=(uint8_t)(data_TX>>8);
DaisyTransmit[5]=(uint8_t) data_TX;
//Motor 3
DaisyTransmit[6]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[2]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[7]=(uint8_t)(data_TX>>8);
DaisyTransmit[8]=(uint8_t) data_TX;
//Motor 4
DaisyTransmit[9]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[3]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[10]=(uint8_t)(data_TX>>8);
DaisyTransmit[11]=(uint8_t) data_TX;
UART001_WriteDataBytes(&ESC_UART_Handle, DaisyTransmit, 12);
#else
uint16_t data;
//Motor 1
DaisyTransmit[0]=SET_MOTOR_SPEED;
data=actuator_speed_percent[0]/100.0*0xFFFF;
DaisyTransmit[1]=(uint8_t)(data>>8);
DaisyTransmit[2]=(uint8_t) data;
//Motor 2
DaisyTransmit[3]=SET_MOTOR_SPEED;
data=actuator_speed_percent[1]/100.0*0xFFFF;
DaisyTransmit[4]=(uint8_t)(data>>8);
DaisyTransmit[5]=(uint8_t) data;
//Motor 3
DaisyTransmit[6]=SET_MOTOR_SPEED;
data=actuator_speed_percent[2]/100.0*0xFFFF;
DaisyTransmit[7]=(uint8_t)(data>>8);
DaisyTransmit[8]=(uint8_t) data;
//Motor 4
DaisyTransmit[9]=SET_MOTOR_SPEED;
data=actuator_speed_percent[3]/100.0*0xFFFF;
DaisyTransmit[10]=(uint8_t)(data>>8);
DaisyTransmit[11]=(uint8_t) data;
UART001_WriteDataBytes(&UART001_Handle2, DaisyTransmit, 12);
#endif
#endif
#endif
#ifdef DEBUG_SPECIFIC
#undef DEBUG_CONTINOUS
// Check number of bytes received
Bytes = USBD_VCOM_BytesReceived();
if(Bytes != 0)
{
for(nByte = 0; nByte < Bytes; nByte++)
{
// Receive Byte
if(USBD_VCOM_ReceiveByte(&USB_Rx_Buffer[0]) != DAVEApp_SUCCESS)
{
//Error
}
}
switch(USB_Rx_Buffer[0])
{
case '1':
sprintf(USB_Tx_Buffer, "Throttle: %f Rudder: %f Elevator: %f Aileron: %f\n", powerD, yawD_dot, pitchD, rollD);
break;
case '2':
PWMSP001_Start(&MagCalib_Timer);
break;
case '3':
sprintf(USB_Tx_Buffer, "Y:%1.2f P:%1.2f R:%1.2f YOff:%1.2f\n", YPR[0], YPR[1], YPR[2], yoffset);
break;
case '4':
sprintf(USB_Tx_Buffer, "Y_dot:%1.2f\n", yaw_dot);
break;
case '5':
sprintf(USB_Tx_Buffer, "PWM1:%f PWM2:%f PWM3:%f PWM4:%f\n", PWM_width[0], PWM_width[1], PWM_width[2], PWM_width[3]);
break;
case '6':
sprintf(USB_Tx_Buffer, "PWM1:%f PWM2:%f PWM3:%f PWM4:%f\n", actuator_speed_percent[0], actuator_speed_percent[1], actuator_speed_percent[2], actuator_speed_percent[3]);
break;
case '7':
sprintf(USB_Tx_Buffer, "eY:%f eP:%f eR:%f\n", yawD_dot-yaw_dot, pitchD-YPR[1], rollD-YPR[2]);
break;
case '8':
sprintf(USB_Tx_Buffer, "TimerSensor:%d TimerMain:%d TimerRC:%d\n", (int)GetSensorCount(), (int)counter_main, (int)GetRCCount());
break;
case '9':
updateValues(&p,&t);
sprintf(USB_Tx_Buffer, "Pressure: %d Temperature: %d Counter: %d\n",(int)p,(int)t,(int)DPS310_INT_counter);
break;
case 'a':
#ifdef SERIAL_DEBUG
power += 10;
if (power > 100) power = 100;
powerD = power;
sprintf(USB_Tx_Buffer, "Set Speed to: %d\n",(int)power);
#endif
break;
case 'b':
#ifdef SERIAL_DEBUG
power = 0;
powerD = power;
sprintf(USB_Tx_Buffer, "Set Speed to: %d\n",(int)power);
#endif
break;
default:
sprintf(USB_Tx_Buffer, "Unknown Command\n");
break;
}
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
}
if (sendMag)
{
sendMag = FALSE;
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
}
//Call continuous to handle class specific control request
/* Main USB management task */
/* Check if data received */
CDC_Device_USBTask(&USBD_VCOM_CDCInterface);
#endif
#ifdef DEBUG_CONTINOUS
counterccc++;
if(counterccc % 500 == 0)
{
helper++;
// sprintf(USB_Tx_Buffer, "{PacketC,T,%d}\n",packets);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Loss,T,%d}\n",packet_loss);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{UndefE,T,%d}\n",undef_error);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Timeout,T,%d}\n",timeout_count);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Throttle,T,%.3f}\n",powerD);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Rudder,T,%.3f}\n",yawD_dot);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Elevator,T,%.3f}\n",pitchD);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Aileron,T,%.3f}\n",rollD);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
sprintf(USB_Tx_Buffer, "{Motor1,T,%u}\n",(uint16_t)((actuator_speed_percent[0]/100.0)*65535.0));
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
sprintf(USB_Tx_Buffer, "{Motor2,T,%u}\n",(uint16_t)((actuator_speed_percent[1]/100.0)*65535.0));
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
sprintf(USB_Tx_Buffer, "{Motor3,T,%u}\n",(uint16_t)((actuator_speed_percent[2]/100.0)*65535.0));
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
sprintf(USB_Tx_Buffer, "{Motor4,T,%u}\n",(uint16_t)((actuator_speed_percent[3]/100.0)*65535.0));
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
counterccc=0;
}
//Call continuous to handle class specific control request
/* Main USB management task */
/* Check if data received */
CDC_Device_USBTask(&USBD_VCOM_CDCInterface);
#endif
}
return 0;
}
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);
}
