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 |