Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1400 → Rev 1568

/branches/V0.76g-acid/timer0.c
1,5 → 1,7
 
#include "main.h"
#include "spectrum.h"
#include "heli.h"
 
volatile unsigned int CountMilliseconds = 0;
volatile static unsigned int tim_main;
6,7 → 8,7
volatile unsigned char UpdateMotor = 0;
volatile unsigned int cntKompass = 0;
volatile unsigned int beeptime = 0;
volatile unsigned char SendSPI = 0, ServoActive = 0;
volatile unsigned char SendSPI = 0;
 
unsigned int BeepMuster = 0xffff;
 
31,7 → 33,7
static unsigned char cnt_1ms = 1,cnt = 0;
unsigned char pieper_ein = 0;
if(SendSPI) SendSPI--;
if(SpektrumTimer) SpektrumTimer--;
if(SpektrumTimer) SpektrumTimer--;
 
if(!cnt--)
{
134,7 → 136,7
 
// disable all interrupts before reconfiguration
cli();
 
PORTD &= ~(1<<PORTD7); // set PD7 to low
 
DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017)
216,159 → 218,49
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)
//-----------------------------------------------------
// PPM state machine, onboard demultiplexed by HEF4017
//-----------------------------------------------------
if(!PulseOutput) // pulse output complete
{
//---------------------------
// Nick servo state machine
//---------------------------
if(!PulseOutput) // pulse output complete
if(TCCR2A & (1<<COM2A0)) // we had a low pulse
{
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
{
TCCR2A &= ~(1<<COM2A0);// make a high pulse
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
RemainingPulse += 2 * servoValues[ServoIndex - 1]; // add channel value, factor of 2 because timer 1 increments 3.2µs
 
ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
if(EE_Parameter.ServoCompInvert & 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;
 
// 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.ServoCompInvert & 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;
break;
case 2: // Roll Compensation Servo
ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset
ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
if(EE_Parameter.ServoCompInvert & 0x02)
{ // inverting movement of servo
ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
}
else
{ // non inverting movement of servo
ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 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;
// substract stop pulse width
RemainingPulse -= PPM_STOPPULSE;
// accumulate time for correct sync gap
ServoFrameTime += RemainingPulse;
if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset
else HEF4017R_ON;
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
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;
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;
}
 
// General pulse output generator
if(RemainingPulse > (255 + IRS_RUNTIME))