void MPU6050_Gyro_calibration(void)
{
uint8_t i;
int loops = 150;
float InitGyroData[3];
for (i = 0; i < loops; i++)
{
MPU6050_Gyro_get(InitGyroData);
gyroADC_ROLL_offset += gyroADC_ROLL;
gyroADC_PITCH_offset += gyroADC_PITCH;
gyroADC_YAW_offset += gyroADC_YAW;
Delay_ms(2);
}
gyroADC_ROLL_offset /= loops;
gyroADC_PITCH_offset /= loops;
gyroADC_YAW_offset /= loops;
Delay_ms(5);
}
//--------------------Engine Process-----------------------------//
void engineProcess(float dt)
{
static int loopCounter;
tStopWatch sw;
loopCounter++;
LEDon();
DEBUG_LEDoff();
StopWatchInit(&sw);
MPU6050_ACC_get(AccData); // Getting Accelerometer data
unsigned long tAccGet = StopWatchLap(&sw);
MPU6050_Gyro_get(GyroData); // Getting Gyroscope data
unsigned long tGyroGet = StopWatchLap(&sw);
Get_Orientation(AccAngleSmooth, CameraOrient, AccData, GyroData, dt);
unsigned long tAccAngle = StopWatchLap(&sw);
// if we enable RC control
if (configData[9] == '1')
{
// Get the RX values and Averages
Get_RC_Step(Step, RCSmooth); // Get RC movement on all three AXIS
Step[PITCH] = Limit_Pitch(Step[PITCH], CameraOrient[PITCH]); // limit pitch to defined limits in header
}
// Pitch adjustments
//pitch_setpoint += Step[PITCH];
pitchRCOffset += Step[PITCH] / 1000.0;
pitch_angle_correction = constrain((CameraOrient[PITCH] + pitchRCOffset) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
pitch_setpoint += pitch_angle_correction; // Pitch return to zero after collision
// Roll Adjustments
//roll_setpoint += Step[ROLL];
rollRCOffset += Step[ROLL] / 1000.0;
// include the config roll offset which is scaled to 0 = -10.0 degrees, 100 = 0.0 degrees, and 200 = 10.0 degrees
roll_angle_correction = constrain((CameraOrient[ROLL] + rollRCOffset + Deg2Rad((configData[11] - 100) / 10.0)) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
roll_setpoint += roll_angle_correction; //Roll return to zero after collision
// if we enabled AutoPan on Yaw
if (configData[10] == '0')
{
//ADC1Ch13_yaw = ((ADC1Ch13_yaw * 99.0) + ((float)(readADC1(13) - 2000) / 4000.0)) / 100.0; // Average ADC value
//CameraOrient[YAW] = CameraOrient[YAW] + 0.01 * (ADC1Ch13_yaw - CameraOrient[YAW]);
yaw_setpoint = autoPan(Output[YAW], yaw_setpoint);
}
else
{
// Yaw Adjustments
yaw_setpoint += Step[YAW];
yawRCOffset += Step[YAW] / 1000.0;
}
#if 0
yaw_angle_correction = constrain((CameraOrient[YAW] + yawRCOffset) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
yaw_setpoint += yaw_angle_correction; // Yaw return to zero after collision
#endif
unsigned long tCalc = StopWatchLap(&sw);
pitch_PID();
roll_PID();
yaw_PID();
unsigned long tPID = StopWatchLap(&sw);
unsigned long tAll = StopWatchTotal(&sw);
printcounter++;
//if (printcounter >= 500 || dt > 0.0021)
if (printcounter >= 500)
{
if (debugPrint)
{
print("Loop: %7d, I2CErrors: %d, angles: roll %7.2f, pitch %7.2f, yaw %7.2f\r\n",
loopCounter, I2Cerrorcount, Rad2Deg(CameraOrient[ROLL]),
Rad2Deg(CameraOrient[PITCH]), Rad2Deg(CameraOrient[YAW]));
}
if (debugSense)
{
print(" dt %f, AccData: %8.3f | %8.3f | %8.3f, GyroData %7.3f | %7.3f | %7.3f \r\n",
dt, AccData[X_AXIS], AccData[Y_AXIS], AccData[Z_AXIS], GyroData[X_AXIS], GyroData[Y_AXIS], GyroData[Z_AXIS]);
}
if (debugPerf)
{
print("idle: %5.2f%%, time[µs]: attitude est. %4d, IMU acc %4d, gyro %4d, angle %4d, calc %4d, PID %4d\r\n",
GetIdlePerf(), tAll, tAccGet, tGyroGet, tAccAngle, tCalc, tPID);
}
if (debugRC)
{
print(" RC2avg: %7.2f | RC3avg: %7.2f | RC4avg: %7.2f | RStep:%7.3f PStep: %7.3f YStep: %7.3f\r\n",
RCSmooth[ROLL], RCSmooth[PITCH], RCSmooth[YAW], Step[ROLL], Step[PITCH], Step[YAW]);
}
if (debugOrient)
{
print("Roll_setpoint:%12.4f | Pitch_setpoint:%12.4f | Yaw_setpoint:%12.4f\r\n",
roll_setpoint, pitch_setpoint, yaw_setpoint);
}
if (debugCnt)
{
print("Counter min %3d, %3d, %3d, max %4d, %4d, %4d, count %3d, %3d, %3d, usbOverrun %4d\r\n",
MinCnt[ROLL], MinCnt[PITCH], MinCnt[YAW],
MaxCnt[ROLL], MaxCnt[PITCH], MaxCnt[YAW],
IrqCnt[ROLL], IrqCnt[PITCH], IrqCnt[YAW],
usbOverrun());
}
if (debugAutoPan)
{
print("Pitch_output:%3.2f | Roll_output:%3.2f | Yaw_output:%3.2f | centerpoint:%4.4f\n\r",
Output[PITCH],
Output[ROLL],
Output[YAW],
centerPoint);
}
printcounter = 0;
}
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
}
}
