/**
* \brief This function initializes the hardware
*
*
* \details Sensors are set up, polynomials are calculated and other general preparations are made
*/
void setup(void)
{
initBluetoothStorage();//initialize space for variables used for Bluetooth Communication
//-------------------Dave Setup---------------------
DAVE_STATUS_t status;
status = DAVE_Init();//Initialization of DAVE APPs
if (status == DAVE_STATUS_FAILURE)
{
/* Placeholder for error handler code. The while loop below can be replaced with an user error handler. */
XMC_DEBUG("DAVE APPs initialization failed\n");
while (1U);
}
disableIRQ();//disables all Interrupts
delay(2000);//waits 2000ms
enableIRQ();//enables configurated Interrupts
setup_STATE_LEDs();//setup Status-LED's
WatchRC_Init();//initialize RC-Watchdog
setup_UART_Trigger_limits();//setup Trigger-Limits of UART-FIFO
PressureFIR = Initialize_FIR_Filter(PressureFIR, MOVING_AVERAGE);//initialize FIR Filter
setupDPS310I2C();//initialize DPS310
getCoefficients();//get Coefficients of DPS310
setupDPS310();//setup DPS Hardware
MPU9150_Setup();//configures the IMU
delay(3000);//wait 3000ms to wait for ESC's to startup
// initialize controller polynomials
polynoms.b_roll[0]=parameter.P_roll-parameter.I_roll*parameter.T-parameter.P_roll*parameter.N_roll*parameter.T+parameter.N_roll*parameter.I_roll*parameter.T*parameter.T+parameter.D_roll*parameter.N_roll;
polynoms.b_roll[1]=parameter.I_roll*parameter.T-2*parameter.P_roll+parameter.P_roll*parameter.N_roll*parameter.T-2*parameter.D_roll*parameter.N_roll;
polynoms.b_roll[2]=parameter.P_roll+parameter.D_roll*parameter.N_roll;
polynoms.a_roll[0]=1-parameter.N_roll*parameter.T;
polynoms.a_roll[1]=parameter.N_roll*parameter.T-2;
polynoms.b_pitch[0]=parameter.P_pitch-parameter.I_pitch*parameter.T-parameter.P_pitch*parameter.N_pitch*parameter.T+parameter.N_pitch*parameter.I_pitch*parameter.T*parameter.T+parameter.D_pitch*parameter.N_pitch;
polynoms.b_pitch[1]=parameter.I_pitch*parameter.T-2*parameter.P_pitch+parameter.P_pitch*parameter.N_pitch*parameter.T-2*parameter.D_pitch*parameter.N_pitch;
polynoms.b_pitch[2]=parameter.P_pitch+parameter.D_pitch*parameter.N_pitch;
polynoms.a_pitch[0]=1-parameter.N_pitch*parameter.T;
polynoms.a_pitch[1]=parameter.N_pitch*parameter.T-2;
TIMER_Start(&Control_Timer);//start Timer for Controller
}
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);
}
