60,14 → 60,16 |
volatile unsigned int cntKompass = 0; |
volatile unsigned int beeptime = 0; |
volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1; |
uint16_t RemainingPulse = 0; |
uint16_t RemainingPulse = 255; // MartinR zur Sicherheit bei Interrupts |
volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
volatile int16_t ServoPanOffset = (255 / 2) * MULTIPLYER * 16; // MartinR: für Pan-Funktion |
|
unsigned int BeepMuster = 0xffff; |
|
volatile int16_t ServoNickValue = 0; |
volatile int16_t ServoRollValue = 0; |
volatile int16_t ServoPanValue = 0; // MartinR : für PAN-Funktion |
|
|
enum { |
244,16 → 246,36 |
|
void CalculateServo(void) |
{ |
signed char cosinus, sinus; |
//signed char cosinus, sinus; // MartinR : so war es |
extern signed char cosinus, sinus; // MartinR : extern für PAN-Funktion |
signed long nick, roll; |
|
cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
sinus = sintab[EE_Parameter.CamOrientation]; |
nick = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
roll = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
int tmp; // MartinR : für PAN-Funktion // Wert : 0-24 -> 0-360 -> 15° steps |
/* // MartinR: bisher |
tmp = EE_Parameter.CamOrientation + ((Parameter_Servo4 - 125) * (Parameter_UserParam8 - 125)) / 400 ; //MartinR : für PAN-Funktion |
if (tmp < 0) tmp = 24- (abs(tmp)) % 24 ; // MartinR :Modulo 24 |
else tmp = tmp % 24 ; // MartinR :Modulo 24 |
*/ |
tmp = EE_Parameter.CamOrientation + ((Parameter_Servo4 - 125) * (Parameter_UserParam8 - 125)) / 200 ; //MartinR : für PAN-Funktion |
if (tmp < 0) tmp = 48- (abs(tmp)) % 48 ; // MartinR :Modulo 48 |
else tmp = tmp % 48 ; // MartinR :Modulo 48 |
|
// cosinus = sintab[EE_Parameter.CamOrientation + 6]; // MartinR : so war es |
// sinus = sintab[EE_Parameter.CamOrientation]; // MartinR : so war es |
//cosinus = sintab[tmp + 6]; // MartinR : für PAN-Funktion |
cosinus += (2*sintab[tmp + 12]- cosinus + 1) / 2; // MartinR : für PAN-Funktion |
sinus += (2*sintab[tmp] - sinus + 1) / 2; // MartinR : für PAN-Funktion |
|
if(CalculateServoSignals == 1) |
{ |
nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
if (Parameter_UserParam7 < 50) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
{ |
//nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; // MartinR: so war es |
nick = (cosinus * IntegralNick) / 512L - (sinus * IntegralRoll) / 512L; // MartinR: bessere Auflösung |
nick -= POI_KameraNick * 7; |
} |
nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
// offset (Range from 0 to 255 * 3 = 765) |
ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
281,7 → 303,11 |
} |
else |
{ |
roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
if (Parameter_UserParam7 < 100) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
{ |
//roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; // MartinR: so war es |
roll = (cosinus * IntegralRoll) / 512L + (sinus * IntegralNick) / 512L; // MartinR: bessere Auflösung |
} |
roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
if(EE_Parameter.ServoCompInvert & 0x02) |
304,6 → 330,11 |
{ |
ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
} |
|
// MartinR: Filterung der Pan- Funktion |
ServoPanOffset += ((int16_t)Parameter_Servo4 * (MULTIPLYER*16) - ServoPanOffset) / EE_Parameter.ServoManualControlSpeed; |
ServoPanValue = (int16_t)ServoPanOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
|
CalculateServoSignals = 0; |
} |
} |
392,7 → 423,8 |
RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
break; |
case 4: |
RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
//RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; // MartinR: so war es |
RemainingPulse += ServoPanValue - (256 / 2) * MULTIPLYER; // MartinR: zur Filterung der Pan-Funktion |
break; |
case 5: |
RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |