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