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