📄 timer0.c
字号:
#define MULTIPLYER 4
static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
if(PlatinenVersion < 20)
{
//---------------------------
// Nick servo state machine
//---------------------------
if(!PulseOutput) // pulse output complete
{
if(TCCR2A & (1<<COM2A0)) // we had a low pulse
{
TCCR2A &= ~(1<<COM2A0);// make a high pulse
RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
if(EE_Parameter.ServoNickCompInvert & 0x01)
{ // inverting movement of servo
ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
}
else
{ // non inverting movement of servo
ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
}
// limit servo value to its parameter range definition
if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
{
ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
}
else
if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
{
ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
}
RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
ServoNickValue /= MULTIPLYER;
DebugOut.Analog[20] = ServoNickValue;
// range servo pulse width
if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
// accumulate time for correct update rate
ServoFrameTime = RemainingPulse;
}
else // we had a high pulse
{
TCCR2A |= (1<<COM2A0); // make a low pulse
RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
}
// set pulse output active
PulseOutput = 1;
}
} // EOF Nick servo state machine
else
{
//-----------------------------------------------------
// PPM state machine, onboard demultiplexed by HEF4017
//-----------------------------------------------------
if(!PulseOutput) // pulse output complete
{
if(TCCR2A & (1<<COM2A0)) // we had a low pulse
{
TCCR2A &= ~(1<<COM2A0);// make a high pulse
if(ServoIndex == 0) // if we are at the sync gap
{
RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
ServoFrameTime = 0; // reset servo frame time
HEF4017R_ON; // enable HEF4017 reset
}
else // servo channels
{
RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
switch(ServoIndex) // map servo channels
{
case 1: // Nick Compensation Servo
ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
if(EE_Parameter.ServoNickCompInvert & 0x01)
{ // inverting movement of servo
ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
}
else
{ // non inverting movement of servo
ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
}
// limit servo value to its parameter range definition
if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
{
ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
}
else
if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
{
ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
}
RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
ServoNickValue /= MULTIPLYER;
DebugOut.Analog[20] = ServoNickValue;
break;
case 2: // Roll Compensation Servo
ServoRollOffset = (ServoRollOffset * 3 + (int16_t) 80 * MULTIPLYER) / 4; // lowpass offset
ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
//if(EE_Parameter.ServoRollCompInvert & 0x01)
{ // inverting movement of servo
ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
}
/* else
{ // non inverting movement of servo
ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
}
*/ // limit servo value to its parameter range definition
if(ServoRollValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
{
ServoRollValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
}
else
if(ServoRollValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
{
ServoRollValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
}
RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
ServoRollValue /= MULTIPLYER;
//DebugOut.Analog[20] = ServoRollValue;
/* ServoRollOffset = (ServoRollOffset * 3 + (int16_t)Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset
ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
if(EE_Parameter.ServoRollCompInvert & 0x01)
{ // inverting movement of servo
ServoRollValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
}
else
{ // non inverting movement of servo
ServoRollValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
}
// limit servo value to its parameter range definition
if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
{
ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
}
else
if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
{
ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
}
RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
ServoRollValue /= MULTIPLYER;
//DebugOut.Analog[20] = ServoRollValue;
*/ break;
default: // other servo channels
RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2?s
break;
}
// range servo pulse width
if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
// substract stop pulse width
RemainingPulse -= PPM_STOPPULSE;
// accumulate time for correct sync gap
ServoFrameTime += RemainingPulse;
}
}
else // we had a high pulse
{
TCCR2A |= (1<<COM2A0); // make a low pulse
// set pulsewidth to stop pulse width
RemainingPulse = PPM_STOPPULSE;
// accumulate time for correct sync gap
ServoFrameTime += RemainingPulse;
if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset
ServoIndex++; // change to next servo channel
if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
}
// set pulse output active
PulseOutput = 1;
}
} // EOF PPM state machine
// General pulse output generator
if(RemainingPulse > (255 + IRS_RUNTIME))
{
OCR2A = 255;
RemainingPulse -= 255;
}
else
{
if(RemainingPulse > 255) // this is the 2nd last part
{
if((RemainingPulse - 255) < IRS_RUNTIME)
{
OCR2A = 255 - IRS_RUNTIME;
RemainingPulse -= 255 - IRS_RUNTIME;
}
else // last part > ISR_RUNTIME
{
OCR2A = 255;
RemainingPulse -= 255;
}
}
else // this is the last part
{
OCR2A = RemainingPulse;
RemainingPulse = 0;
PulseOutput = 0; // trigger to stop pulse
}
} // EOF general pulse output generator
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -