311,7 → 311,7 |
if(AdValueGyrPitch > 2034) Reading_GyroPitch = +2000; |
} |
|
// start ADC |
// start ADC again to capture measurement values for the next loop |
ADC_Enable(); |
|
IntegralYaw = Reading_IntegralGyroYaw; |
813,9 → 813,9 |
{ |
int32_t tmp_long, tmp_long2; |
// determine the deviation of gyro integral from averaged acceleration sensor |
tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch); |
tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFactor - (int32_t)Mean_AccPitch); |
tmp_long /= 16; |
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll); |
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll); |
tmp_long2 /= 16; |
|
if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands |
823,7 → 823,7 |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further is yaw stick is active |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
856,8 → 856,8 |
MeanIntegralRoll /= BALANCE_NUMBER; |
|
// Calculate mean of the acceleration values |
IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER; |
IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER; |
IntegralAccPitch = (ParamSet.GyroAccFactor * IntegralAccPitch) / BALANCE_NUMBER; |
IntegralAccRoll = (ParamSet.GyroAccFactor * IntegralAccRoll ) / BALANCE_NUMBER; |
|
// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
982,7 → 982,7 |
AttitudeCorrectionPitch = 0; |
} |
|
// if Gyro_I_Faktor == 0 , for example at Heading Hold, ignore attitude correction |
// if Gyro_I_Factor == 0 , for example at Heading Hold, ignore attitude correction |
if(!Gyro_I_Factor) |
{ |
AttitudeCorrectionRoll = 0; |
1073,8 → 1073,8 |
if(!TimerDebugOut--) |
{ |
TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |
DebugOut.Analog[0] = IntegralPitch / ParamSet.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFaktor; |
DebugOut.Analog[0] = IntegralPitch / ParamSet.GyroAccFactor; |
DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFactor; |
DebugOut.Analog[2] = Mean_AccPitch; |
DebugOut.Analog[3] = Mean_AccRoll; |
DebugOut.Analog[4] = Reading_GyroYaw; |