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 | } |