Rev 2166 | Rev 2192 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2166 | Rev 2191 | ||
---|---|---|---|
Line 53... | Line 53... | ||
53 | 53 | ||
54 | #include "main.h" |
54 | #include "main.h" |
Line 55... | Line 55... | ||
55 | #define MULTIPLYER 4 |
55 | #define MULTIPLYER 4 |
56 | 56 | ||
57 | volatile unsigned int CountMilliseconds = 0; |
57 | volatile unsigned int CountMilliseconds = 0; |
58 | volatile static unsigned int tim_main; |
58 | volatile unsigned int tim_main; |
59 | volatile unsigned char UpdateMotor = 0; |
59 | volatile unsigned char UpdateMotor = 0; |
60 | volatile unsigned int cntKompass = 0; |
60 | volatile unsigned int cntKompass = 0; |
61 | volatile unsigned int beeptime = 0; |
61 | volatile unsigned int beeptime = 0; |
62 | volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1; |
62 | volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1; |
63 | uint16_t RemainingPulse = 0; |
63 | uint16_t RemainingPulse = 0; |
Line 64... | Line 64... | ||
64 | volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
64 | volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
- | 65 | volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
|
Line 65... | Line 66... | ||
65 | volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
66 | |
66 | 67 | unsigned int BeepMuster = 0xffff; |
|
Line -... | Line 68... | ||
- | 68 | signed int NickServoValue = 128 * MULTIPLYER * 16; |
|
- | 69 | ||
- | 70 | volatile int16_t ServoNickValue = 0; |
|
- | 71 | volatile int16_t ServoRollValue = 0; |
|
Line 67... | Line 72... | ||
67 | unsigned int BeepMuster = 0xffff; |
72 | |
68 | 73 | // bitcoding for EE_Parameter.ServoCompInvert |
|
69 | volatile int16_t ServoNickValue = 0; |
74 | #define SERVO_NICK_INV 0x01 |
70 | volatile int16_t ServoRollValue = 0; |
75 | #define SERVO_ROLL_INV 0x02 |
Line 238... | Line 243... | ||
238 | 243 | ||
239 | 244 | ||
240 | /*****************************************************/ |
245 | /*****************************************************/ |
- | 246 | /* Control Servo Position */ |
|
- | 247 | /*****************************************************/ |
|
- | 248 | void CalcNickServoValue(void) |
|
Line -... | Line 249... | ||
- | 249 | { |
|
- | 250 | unsigned int max, min; |
|
- | 251 | ||
- | 252 | if(EE_Parameter.ServoCompInvert & SERVO_RELATIVE) // relative moving of the servo value |
|
- | 253 | { |
|
- | 254 | max = ((unsigned int) EE_Parameter.ServoNickMax * MULTIPLYER * 15); |
|
- | 255 | min = ((unsigned int) EE_Parameter.ServoNickMin * MULTIPLYER * 20); |
|
- | 256 | NickServoValue -= ((signed char) (Parameter_ServoNickControl - 128) / 2) * 4; |
|
- | 257 | LIMIT_MIN_MAX(NickServoValue,min, max); |
|
Line 241... | Line 258... | ||
241 | /* Control Servo Position */ |
258 | } |
242 | /*****************************************************/ |
259 | else NickServoValue = (int16_t)Parameter_ServoNickControl * (MULTIPLYER*16); // direct poti control |
243 | 260 | } |
|
244 | 261 | ||
Line 254... | Line 271... | ||
254 | { |
271 | { |
255 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
272 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
256 | nick -= POI_KameraNick * 7; |
273 | nick -= POI_KameraNick * 7; |
257 | nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
274 | nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
258 | // offset (Range from 0 to 255 * 3 = 765) |
275 | // offset (Range from 0 to 255 * 3 = 765) |
- | 276 | if(EE_Parameter.ServoCompInvert & SERVO_RELATIVE) ServoNickOffset = NickServoValue; |
|
259 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
277 | else ServoNickOffset += (NickServoValue - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
- | 278 | ||
260 | if(EE_Parameter.ServoCompInvert & 0x01) // inverting movement of servo |
279 | if(EE_Parameter.ServoCompInvert & SERVO_NICK_INV) // inverting movement of servo |
261 | { |
280 | { |
262 | nick = ServoNickOffset / 16 + nick; |
281 | nick = ServoNickOffset / 16 + nick; |
263 | } |
282 | } |
264 | else |
283 | else |
265 | { // inverting movement of servo |
284 | { // inverting movement of servo |
Line 282... | Line 301... | ||
282 | else |
301 | else |
283 | { |
302 | { |
284 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
303 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
285 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
304 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
286 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
305 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
287 | if(EE_Parameter.ServoCompInvert & 0x02) |
306 | if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV) |
288 | { // inverting movement of servo |
307 | { // inverting movement of servo |
289 | roll = ServoRollOffset / 16 + roll; |
308 | roll = ServoRollOffset / 16 + roll; |
290 | } |
309 | } |
291 | else |
310 | else |
292 | { // inverting movement of servo |
311 | { // inverting movement of servo |