Rev 1760 | Rev 1763 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1760 | Rev 1762 | ||
---|---|---|---|
Line 189... | Line 189... | ||
189 | /*****************************************************/ |
189 | /*****************************************************/ |
Line 190... | Line 190... | ||
190 | 190 | ||
191 | 191 | ||
192 | void CalculateServo(void) |
192 | void CalculateServo(void) |
193 | { |
193 | { |
Line 194... | Line 194... | ||
194 | long cosinus, sinus; |
194 | char cosinus, sinus; |
195 | long nick, roll; |
195 | long nick, roll; |
Line 196... | Line 196... | ||
196 | 196 | ||
197 | cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
197 | cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
198 | sinus = sintab[EE_Parameter.CamOrientation]; |
198 | sinus = sintab[EE_Parameter.CamOrientation]; |
199 | 199 | ||
200 | if(CalculateServoSignals == 1) |
- | |
201 | { |
200 | if(CalculateServoSignals == 1) |
202 | nick = ((cosinus * (IntegralNick / (8 * 128L))) - (sinus * (IntegralRoll / (8 * 128L)))); |
201 | { |
203 | //nick = (IntegralNick / 128L); |
202 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
204 | 203 | nick = (EE_Parameter.ServoNickComp * nick) / 512L; |
|
205 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
204 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
206 | ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
205 | ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
207 | if(EE_Parameter.ServoCompInvert & 0x01) |
206 | if(EE_Parameter.ServoCompInvert & 0x01) |
208 | { // inverting movement of servo |
207 | { // inverting movement of servo |
209 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * nick) / (256L) ); |
208 | ServoNickValue += nick;//(int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * nick) / (256L) ); |
210 | } |
209 | } |
211 | else |
210 | else |
212 | { // non inverting movement of servo |
211 | { // non inverting movement of servo |
213 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * nick) / (256L) ); |
212 | ServoNickValue -= nick; |
214 | } |
213 | } |
Line 224... | Line 223... | ||
224 | } |
223 | } |
225 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
224 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
226 | } |
225 | } |
227 | else |
226 | else |
228 | { |
227 | { |
229 | roll = ((cosinus * (IntegralRoll / (8 * 128L))) + (sinus * (IntegralNick / (8 * 128L)))); |
228 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
230 | - | ||
- | 229 | roll = (EE_Parameter.ServoRollComp * roll) / 512L; |
|
231 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
230 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
232 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
231 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
233 | if(EE_Parameter.ServoCompInvert & 0x02) |
232 | if(EE_Parameter.ServoCompInvert & 0x02) |
234 | { // inverting movement of servo |
233 | { // inverting movement of servo |
235 | ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * roll ) / (256L) ); |
234 | ServoRollValue += roll; |
236 | } |
235 | } |
237 | else |
236 | else |
238 | { // non inverting movement of servo |
237 | { // non inverting movement of servo |
239 | ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * roll ) / (256L) ); |
238 | ServoRollValue -= roll; |
240 | } |
239 | } |
241 | // limit servo value to its parameter range definition |
240 | // limit servo value to its parameter range definition |
242 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
241 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
243 | { |
242 | { |
244 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
243 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |