void UART4_IRQHandler()//UART4 Interrupt handler implementation
{
int eeRreg;
uint8_t data;
ConfigMode=1;
while ( USART_GetFlagStatus(UART4, USART_FLAG_RXNE) == RESET);
UART4_DATA=USART_ReceiveData(UART4);
LEDon;
if(UART4_DATA==103)
{ //if "g"
Delay_ms(100);
sprintf (buff, "x");
USART_PutString(buff);
for(eeRreg=0; eeRreg<configDataSize;eeRreg++)
{
data = ReadFromEEPROM(eeRreg);
Delay_ms(1);
sprintf (buff, "%c", data);
USART_PutString(buff);
}
}
if(enable_writing==1)
{
configData[w]=(int)UART4_DATA;
w++;
if(w>=configDataSize)
{
w=0;
enable_writing=0;
//saveData();
configSave();
}
}
if(UART4_DATA==104)
{ // if h (write to eeprom)
enable_writing=1;
}
if(UART4_DATA==105)
{
ConfigMode=1;
}
if(UART4_DATA==106)
{
ConfigMode=0;
}
}
void configLoad(void)
{
//reads configuration from eeprom
for (int i = 0; i < CONFIGDATASIZE; i++)
{
int data = ReadFromEEPROM(i);
if (data >= 0 && data <= LARGEST_CONFIGDATA)
{
configData[i] = data;
}
Delay_ms(5);
}
}
void configSave(void)
{
uint8_t i;
LEDon();
for (i = 0; i < CONFIGDATASIZE; i++)
{
//read data from eeprom,
//check, if it has changed, only then rewrite;
int data = ReadFromEEPROM(i);
if (data != -1 && data != configData[i])
{
WriteToEEPROM(i, configData[i]);
}
Delay_ms(5);
}
LEDoff();
}
int main(void)
{
Delay_ms(100);
Periph_clock_enable();
GPIO_Config();
Usart4Init();
I2C_Config();
ADC_Config();
MPU6050_Init();
Timer1_Config();
Timer8_Config();
Timer2_Config();
Timer5_Config();
Timer4_Config();
Timer3_Config();//RC control timer
NVIC_Configuration();
EXTI_Config();
TIM_Cmd(TIM5, ENABLE);
TIM_CtrlPWMOutputs(TIM5, ENABLE);
for (i = 1 ; i < 1 ; i++) ; //small delay before starting Timer4
TIM_Cmd(TIM4, ENABLE);
TIM_CtrlPWMOutputs(TIM4, ENABLE);
Delay_ms(100);
for (i = 0; i < configDataSize; i++) //reads configuration from eeprom
{
ReadFromEEPROM(i);
configData[i] = EepromData;
Delay_ms(5);
}
I2C_AcknowledgeConfig(I2C2, ENABLE);
Delay_ms(100);
while (1)
{
LEDon;
DEBUG_LEDon;
while (ConfigMode == 1)
{
TimerOff(); //Configuration loop
}
MPU6050_ACC_get();//Getting Accelerometer data
acc_roll_angle = -(atan2(accADC_x, accADC_z)) + (configData[11] - 50.00) * 0.0035; //Calculating pitch ACC angle+callibration
acc_pitch_angle = +(atan2(accADC_y, accADC_z)); //Calculating roll ACC angle
MPU6050_Gyro_get();//Getting Gyroscope data
acc_roll_angle_vid = ((acc_roll_angle_vid * 99.00) + acc_roll_angle) / 100.00; //Averaging pitch ACC values
acc_pitch_angle_vid = ((acc_pitch_angle_vid * 99.00) + acc_pitch_angle) / 100.00; //Averaging roll ACC values
sinus = sinusas[(int)(rc4)]; //Calculating sinus
cosinus = sinusas[90 - (int)(rc4)]; //Calculating cosinus
ROLL = -gyroADC_z * sinus + gyroADC_y * cosinus;
roll_angle = (roll_angle + ROLL * dt) + 0.0002 * (acc_roll_angle_vid - roll_angle); //Roll Horizon
//ROLL=-gyroADC_z*sinus+gyroADC_y*cosinus;
yaw_angle = (yaw_angle + gyroADC_z * dt); //Yaw
pitch_angle_true = ((pitch_angle_true + gyroADC_x * dt) + 0.0002 * (acc_pitch_angle_vid - pitch_angle_true)); //Pitch Horizon
ADC1Ch1_vid = ((ADC1Ch1_vid * 99.00) + (readADC1(1) / 4000.00)) / 100.00; //Averaging ADC values
ADC1Ch1_vid = 0.00;
rc4_avg = ((rc4_avg * 499.00) + (rc4)) / 500.00; //Averaging RC4 values
pitch_angle = pitch_angle_true - rc4_avg / 57.3; //Adding angle
pitch_angle_correction = pitch_angle * 150.0;
if (pitch_angle_correction > 2.0)
{
pitch_angle_correction = 2.0;
}
if (pitch_angle_correction < -2.0)
{
pitch_angle_correction = -2.0;
}
pitch_setpoint = pitch_setpoint + pitch_angle_correction; //Pitch return to zero after collision
roll_angle_correction = roll_angle * 200.0;
if (roll_angle_correction > 2.0)
{
roll_angle_correction = 2.0;
}
if (roll_angle_correction < -2.0)
{
roll_angle_correction = -2.0;
}
roll_setpoint = roll_setpoint + roll_angle_correction; //Roll return to zero after collision
ADC1Ch13_vid = ((ADC1Ch13_vid * 99.00) + ((readADC1(13) - 2000) / 4000.00)) / 100.00; //Averaging ADC values
if (configData[10] == '0')
{
yaw_angle = (yaw_angle + gyroADC_z * dt) + 0.01 * (ADC1Ch13_vid - yaw_angle); //Yaw AutoPan
}
if (configData[10] == '1')
{
yaw_angle = (yaw_angle + gyroADC_z * dt); //Yaw RCPan
}
yaw_angle_correction = yaw_angle * 50.0;
if (yaw_angle_correction > 1.0)
{
yaw_angle_correction = 1.0;
}
if (yaw_angle_correction < -1.0)
{
yaw_angle_correction = -1.0;
}
yaw_setpoint = yaw_setpoint + yaw_angle_correction; //Yaw return to zero after collision
pitch_PID();//Pitch axis pid
roll_PID(); //Roll axis pid
yaw_PID(); //Yaw axis pid
printcounter++; //Print data to UART
if (printcounter >= 100)
{
//sprintf (buff, " %d %d %c Labas\n\r", ACCread[0], ACCread[1], ACCread[2]);
//sprintf (buff, " %x %x %x %x %x %x Labas\n\r", ACCread[0], ACCread[1], ACCread[2], ACCread[3], ACCread[4], ACCread[5]);
//sprintf (buff, "Labas %d %d\n\r", ACCread[0], ACCread[1]);
//sprintf (buff, "%3.1f %f\n\r", ADC1Ch1_vid*57.3, sinus);
//sprintf (buff, "Labas %f %f %f \n\r", accADC_x, accADC_y, accADC_z);
//sprintf (buff, "%3.1f %3.1f \n\r", acc_roll_angle_vid*57.3, acc_pitch_angle_vid *57.3);
//sprintf (buff, "%3.1f %3.1f \n\r", pitch_angle*57.3, roll_angle*57.3);
//sprintf (buff, "%d\n\r", rc4);
//USART_PutString(buff);
printcounter = 0;
}
stop = 0;
LEDoff;
watchcounter = 0;
while (stop == 0) {} //Closed loop waits for interrupt
}
}
void UART4_IRQHandler()//UART4 Interrupt handler implementation
{
int eeRreg;
ConfigMode = 1;
while (USART_GetFlagStatus(UART4, USART_FLAG_RXNE) == RESET);
UART4_DATA = USART_ReceiveData(UART4);
LEDon;
if (UART4_DATA == 103) //if "g"
{
Delay_ms(100);
sprintf(buff, "x");
USART_PutString(buff);
for (eeRreg = 0; eeRreg < configDataSize; eeRreg++)
{
ReadFromEEPROM(eeRreg);
Delay_ms(5);
sprintf(buff, "%c", EepromData);
USART_PutString(buff);
}
// Enable Acknowledgement to be ready for another reception
I2C_AcknowledgeConfig(I2C2, ENABLE);
}
if (enable_writing == 1)
{
configData[w] = (int)UART4_DATA;
w++;
if (w >= configDataSize)
{
w = 0;
enable_writing = 0;
saveData();
}
}
if (UART4_DATA == 104) // if h (write to eeprom)
{
enable_writing = 1;
}
if (UART4_DATA == 105)
{
ConfigMode = 1;
}
if (UART4_DATA == 106)
{
ConfigMode = 0;
}
}
