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;
}
/**
* \brief Function for setup of the pins for the Battery-LEDs on the LARIX-Board
*/
void setup_STATE_LEDs(void)
{
Control_P3_0(OUTPUT_PP_GP, VERYSTRONG); //Configure Pin 3.0 -->BatteryState1 (See: _Quadrocopter/BatterySafety/BatterySafety.h)
Control_P3_1(OUTPUT_PP_GP, VERYSTRONG); //Configure Pin 3.1 -->BatteryState2
Control_P3_2(OUTPUT_PP_GP, VERYSTRONG); //Configure Pin 3.2 -->BatteryState3
}
