/** * \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); }