/**********************************************/
/* Main Loop */
/**********************************************/
int count=0;
int _count= 200;
void loop()
{
:
:
:
if( _count ){
gyroRoll = gyroRoll + 100 * config.accelWeight * (rollAngleACC - rollSetpoint)* fabs(rollAngleACC - rollSetpoint)/sampleTimeACC;
gyroPitch = gyroPitch + 100 * config.accelWeight * (pitchAngleACC - pitchSetpoint)* fabs(pitchAngleACC - pitchSetpoint)/sampleTimeACC;
_count--;
}else{
gyroRoll = gyroRoll + config.accelWeight * (rollAngleACC - rollSetpoint)* fabs(rollAngleACC - rollSetpoint)/sampleTimeACC;
gyroPitch = gyroPitch + config.accelWeight * (pitchAngleACC - pitchSetpoint)* fabs(pitchAngleACC - pitchSetpoint)/sampleTimeACC;
}
:
}