| 5,7 → 5,7 |
|---|
| #include "uart.h" |
| #include "main.h" |
| |
| volatile uint16_t ServoValue = 0; |
| volatile int16_t ServoValue = 0; |
| |
| #define HEF4017R_ON PORTC |= (1<<PORTC6) |
| #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
| 82,33 → 82,34 |
|---|
| case 4: |
| // recalculate new ServoValue |
| ServoValue = 0x0030; // Offset (part 1) |
| DebugOut.Analog[16] = ServoValue; |
| FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset |
| ServoValue += FilterServo; // add filtered static offset |
| |
| if(ParamSet.ServoNickCompInvert & 0x01) |
| { // inverting movement of servo |
| ServoValue += ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
| ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
| } |
| else |
| { // non inverting movement of servo |
| ServoValue -= ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER); |
| ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
| } |
| |
| // limit servo value to its parameter range definition |
| if(ServoValue < ((uint16_t)ParamSet.ServoNickMin * 3) ) |
| if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) ) |
| { |
| ServoValue = (uint16_t)ParamSet.ServoNickMin * 3; |
| ServoValue = (int16_t)ParamSet.ServoNickMin * 3; |
| } |
| else |
| if(ServoValue > ((uint16_t)ParamSet.ServoNickMax * 3) ) |
| if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) ) |
| { |
| ServoValue = (uint16_t)ParamSet.ServoNickMax * 3; |
| ServoValue = (int16_t)ParamSet.ServoNickMax * 3; |
| } |
| |
| DebugOut.Analog[20] = ServoValue; |
| // determine prepulse width (remaining part of ServoValue/Timer Cycle) |
| if ((ServoValue % 255) < 45) |
| { // if prepulse width is to short the execution time of thios isr is longer than the next compare match |
| { // if prepulse width is to short the execution time of this ISR is longer than the next compare match |
| // so balance with postpulse width |
| ServoValue += 77; |
| PostPulse = 0x60 - 77; |