Rev 816 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 816 | Rev 826 | ||
|---|---|---|---|
| Line 309... | Line 309... | ||
| 309 | { |
309 | { |
| 310 | if(AdValueGyrPitch > 2020) Reading_GyroPitch = +1000; |
310 | if(AdValueGyrPitch > 2020) Reading_GyroPitch = +1000; |
| 311 | if(AdValueGyrPitch > 2034) Reading_GyroPitch = +2000; |
311 | if(AdValueGyrPitch > 2034) Reading_GyroPitch = +2000; |
| 312 | } |
312 | } |
| Line 313... | Line 313... | ||
| 313 | 313 | ||
| 314 | // start ADC |
314 | // start ADC again to capture measurement values for the next loop |
| Line 315... | Line 315... | ||
| 315 | ADC_Enable(); |
315 | ADC_Enable(); |
| 316 | 316 | ||
| 317 | IntegralYaw = Reading_IntegralGyroYaw; |
317 | IntegralYaw = Reading_IntegralGyroYaw; |
| Line 811... | Line 811... | ||
| 811 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
811 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 812 | if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction |
812 | if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction |
| 813 | { |
813 | { |
| 814 | int32_t tmp_long, tmp_long2; |
814 | int32_t tmp_long, tmp_long2; |
| 815 | // determine the deviation of gyro integral from averaged acceleration sensor |
815 | // determine the deviation of gyro integral from averaged acceleration sensor |
| 816 | tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch); |
816 | tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFactor - (int32_t)Mean_AccPitch); |
| 817 | tmp_long /= 16; |
817 | tmp_long /= 16; |
| 818 | tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll); |
818 | tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll); |
| 819 | tmp_long2 /= 16; |
819 | tmp_long2 /= 16; |
| Line 820... | Line 820... | ||
| 820 | 820 | ||
| 821 | if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands |
821 | if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands |
| 822 | { |
822 | { |
| 823 | tmp_long /= 3; |
823 | tmp_long /= 3; |
| 824 | tmp_long2 /= 3; |
824 | tmp_long2 /= 3; |
| 825 | } |
825 | } |
| 826 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further is yaw stick is active |
826 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active |
| 827 | { |
827 | { |
| 828 | tmp_long /= 3; |
828 | tmp_long /= 3; |
| 829 | tmp_long2 /= 3; |
829 | tmp_long2 /= 3; |
| Line 854... | Line 854... | ||
| 854 | // Calculate mean value of the gyro integrals |
854 | // Calculate mean value of the gyro integrals |
| 855 | MeanIntegralPitch /= BALANCE_NUMBER; |
855 | MeanIntegralPitch /= BALANCE_NUMBER; |
| 856 | MeanIntegralRoll /= BALANCE_NUMBER; |
856 | MeanIntegralRoll /= BALANCE_NUMBER; |
| Line 857... | Line 857... | ||
| 857 | 857 | ||
| 858 | // Calculate mean of the acceleration values |
858 | // Calculate mean of the acceleration values |
| 859 | IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER; |
859 | IntegralAccPitch = (ParamSet.GyroAccFactor * IntegralAccPitch) / BALANCE_NUMBER; |
| Line 860... | Line 860... | ||
| 860 | IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER; |
860 | IntegralAccRoll = (ParamSet.GyroAccFactor * IntegralAccRoll ) / BALANCE_NUMBER; |
| 861 | 861 | ||
| 862 | // Pitch ++++++++++++++++++++++++++++++++++++++++++++++++ |
862 | // Pitch ++++++++++++++++++++++++++++++++++++++++++++++++ |
| 863 | // Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
863 | // Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
| Line 980... | Line 980... | ||
| 980 | { |
980 | { |
| 981 | AttitudeCorrectionRoll = 0; |
981 | AttitudeCorrectionRoll = 0; |
| 982 | AttitudeCorrectionPitch = 0; |
982 | AttitudeCorrectionPitch = 0; |
| 983 | } |
983 | } |
| Line 984... | Line 984... | ||
| 984 | 984 | ||
| 985 | // if Gyro_I_Faktor == 0 , for example at Heading Hold, ignore attitude correction |
985 | // if Gyro_I_Factor == 0 , for example at Heading Hold, ignore attitude correction |
| 986 | if(!Gyro_I_Factor) |
986 | if(!Gyro_I_Factor) |
| 987 | { |
987 | { |
| 988 | AttitudeCorrectionRoll = 0; |
988 | AttitudeCorrectionRoll = 0; |
| 989 | AttitudeCorrectionPitch = 0; |
989 | AttitudeCorrectionPitch = 0; |
| Line 1071... | Line 1071... | ||
| 1071 | // Debugwerte zuordnen |
1071 | // Debugwerte zuordnen |
| 1072 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1072 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1073 | if(!TimerDebugOut--) |
1073 | if(!TimerDebugOut--) |
| 1074 | { |
1074 | { |
| 1075 | TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |
1075 | TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |
| 1076 | DebugOut.Analog[0] = IntegralPitch / ParamSet.GyroAccFaktor; |
1076 | DebugOut.Analog[0] = IntegralPitch / ParamSet.GyroAccFactor; |
| 1077 | DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFaktor; |
1077 | DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFactor; |
| 1078 | DebugOut.Analog[2] = Mean_AccPitch; |
1078 | DebugOut.Analog[2] = Mean_AccPitch; |
| 1079 | DebugOut.Analog[3] = Mean_AccRoll; |
1079 | DebugOut.Analog[3] = Mean_AccRoll; |
| 1080 | DebugOut.Analog[4] = Reading_GyroYaw; |
1080 | DebugOut.Analog[4] = Reading_GyroYaw; |
| 1081 | DebugOut.Analog[5] = ReadingHeight; |
1081 | DebugOut.Analog[5] = ReadingHeight; |
| 1082 | DebugOut.Analog[6] = (Reading_Integral_Top / 512); |
1082 | DebugOut.Analog[6] = (Reading_Integral_Top / 512); |