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