Rev 1941 | Rev 2011 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1941 | Rev 2010 | ||
---|---|---|---|
Line 200... | Line 200... | ||
200 | if(CalculateServoSignals == 1) |
200 | if(CalculateServoSignals == 1) |
201 | { |
201 | { |
202 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
202 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
203 | nick -= POI_KameraNick * 7; |
203 | nick -= POI_KameraNick * 7; |
204 | nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
204 | nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
- | 205 | // offset (Range from 0 to 255 * 3 = 765) |
|
205 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
206 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
206 | ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
207 | if(EE_Parameter.ServoCompInvert & 0x01) // inverting movement of servo |
207 | 208 | { |
|
208 | if(EE_Parameter.ServoCompInvert & 0x01) |
- | |
209 | { // inverting movement of servo |
209 | nick = ServoNickOffset / 16 + nick; |
210 | ServoNickValue += nick;//(int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * nick) / (256L) ); |
- | |
211 | } |
210 | } |
212 | else |
211 | else |
213 | { // non inverting movement of servo |
212 | { // inverting movement of servo |
214 | ServoNickValue -= nick; |
213 | nick = ServoNickOffset / 16 - nick; |
215 | } |
214 | } |
- | 215 | if(Parameter_UserParam1) ServoNickValue = ((ServoNickValue * Parameter_UserParam1) + nick) / (Parameter_UserParam1 + 1); |
|
- | 216 | else ServoNickValue = nick; |
|
- | 217 | DebugOut.Analog[16] = nick; |
|
- | 218 | DebugOut.Analog[17] = ServoNickValue; |
|
216 | // limit servo value to its parameter range definition |
219 | // limit servo value to its parameter range definition |
217 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
220 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
218 | { |
221 | { |
219 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
222 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
220 | } |
223 | } |
Line 228... | Line 231... | ||
228 | else |
231 | else |
229 | { |
232 | { |
230 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
233 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
231 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
234 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
232 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
235 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
233 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
- | |
234 | if(EE_Parameter.ServoCompInvert & 0x02) |
236 | if(EE_Parameter.ServoCompInvert & 0x02) |
235 | { // inverting movement of servo |
237 | { // inverting movement of servo |
236 | ServoRollValue += roll; |
238 | roll = ServoRollOffset / 16 + roll; |
237 | } |
239 | } |
238 | else |
240 | else |
239 | { // non inverting movement of servo |
241 | { // inverting movement of servo |
240 | ServoRollValue -= roll; |
242 | roll = ServoRollOffset / 16 - roll; |
241 | } |
243 | } |
- | 244 | if(Parameter_UserParam2) ServoRollValue = ((ServoRollValue * Parameter_UserParam2) + roll) / (Parameter_UserParam2 + 1); |
|
- | 245 | else ServoRollValue = roll; |
|
- | 246 | DebugOut.Analog[18] = roll; |
|
- | 247 | DebugOut.Analog[19] = ServoRollValue; |
|
242 | // limit servo value to its parameter range definition |
248 | // limit servo value to its parameter range definition |
243 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
249 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
244 | { |
250 | { |
245 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
251 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
246 | } |
252 | } |