Subversion Repositories FlightCtrl

/******************************************************************************************************************

V0.80g-Arthur-P1 20100922

------------------------------------------------------------------------------------------------------------------

Version includes only support for external HEF4017 for FC1.x hardware, NOT for Twi2Ppm converters for ESCs.

20100917: Transferred changes to v0.80g-Arthur-P.

Arthur P. Modified to use several parameters for servo control:

User_Parameter4:

User_Parameter5:

User_Parameter6:

User_Parameter7:

User_Parameter8: Use external HEF4017 if bit 8 is set (>127). The remaining 7 bits are used

                 for the shutter cycle counter: the value is multiplied by 5 programmatically,

                                 resulting in steps of approx. 0.1sec. Minimum value to start using the

                                 interval timer is 10 (approx. 1 sec, or countervalue of 50). Note that this

                                 was originally done through user para 6.

******************************************************************************************************************/

#include "main.h"

volatile unsigned int CountMilliseconds = 0;

volatile static unsigned int tim_main;

volatile unsigned char UpdateMotor = 0;

volatile unsigned int cntKompass = 0;

volatile unsigned int beeptime = 0;

volatile unsigned char SendSPI = 0, ServoActive = 0;

unsigned int BeepMuster = 0xffff;

volatile int16_t        ServoNickValue = 0;

volatile int16_t        ServoRollValue = 0;

/******************************************************************************************************************

Arthur P: Added two variables for control of the shutter servo cycle.

091114 Inserted same changes into v.0.76g code.

091114 Inactivated the following two lines as the shutter interval funtion is not

used at the moment.

20100804 Reactivated to be able to choose slower shutter rate than normal for

Panasonic FX150 in continuous mode.

******************************************************************************************************************/

volatile static unsigned int CameraShutterCycleCounter = 0;

volatile static unsigned int CameraShutterCycle = 0;

volatile static unsigned int CameraShutterCycleOnCount = 20;  // Leave the shutter on for at least

                                                                                                                          // 20 cycles or approx. 0.2 seconds.

enum {

  STOP             = 0,

  CK               = 1,

  CK8              = 2,

  CK64             = 3,

  CK256            = 4,

  CK1024           = 5,

  T0_FALLING_EDGE  = 6,

  T0_RISING_EDGE   = 7

};

ISR(TIMER0_OVF_vect)    // 9,7kHz

{

   static unsigned char cnt_1ms = 1,cnt = 0, compass_active = 0;

   unsigned char pieper_ein = 0;

   if(SendSPI) SendSPI--;

   if(SpektrumTimer) SpektrumTimer--;

   if(!cnt--)

    {

     cnt = 9;

     CountMilliseconds++;

     cnt_1ms++;

     cnt_1ms %= 2;

     if(!cnt_1ms) UpdateMotor = 1;

         if(!(PINC & 0x10)) compass_active = 1;

     if(beeptime)

        {

        if(beeptime > 10) beeptime -= 10; else beeptime = 0;

        if(beeptime & BeepMuster)

         {

          pieper_ein = 1;

         }

         else pieper_ein = 0;

        }

     else

      {

       pieper_ein = 0;

       BeepMuster = 0xffff;

      }

     if(pieper_ein)

        {

          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2

          else                      PORTC |= (1<<7); // Speaker an PORTC.7

        }

     else

        {

         if(PlatinenVersion == 10) PORTD &= ~(1<<2);

         else                      PORTC &= ~(1<<7);

        }

        }

 if(compass_active && !NaviDataOkay && EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)

 {

  if(PINC & 0x10)

   {

        if(++cntKompass > 1000) compass_active = 0;

   }

  else

   {

    if((cntKompass) && (cntKompass < 362))

    {

     cntKompass += cntKompass / 41;

     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;

     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;

    }

    cntKompass = 0;

   }

 }

}

// -----------------------------------------------------------------------

unsigned int SetDelay (unsigned int t)

{

//  TIMSK0 &= ~_BV(TOIE0);

  return(CountMilliseconds + t + 1);

//  TIMSK0 |= _BV(TOIE0);

}

// -----------------------------------------------------------------------

char CheckDelay(unsigned int t)

{

//  TIMSK0 &= ~_BV(TOIE0);

  return(((t - CountMilliseconds) & 0x8000) >> 9);

//  TIMSK0 |= _BV(TOIE0);

}

// -----------------------------------------------------------------------

void Delay_ms(unsigned int w)

{

 unsigned int akt;

 akt = SetDelay(w);

 while (!CheckDelay(akt));

}

void Delay_ms_Mess(unsigned int w)

{

 unsigned int akt;

 akt = SetDelay(w);

 while (!CheckDelay(akt)) if(AdReady) {AdReady = 0; ANALOG_ON;}

}

/*****************************************************/

/*              Initialize Timer 2                   */

/*****************************************************/

// The timer 2 is used to generate the PWM at PD7 (J7)

// to control a camera servo for nick compensation.

void TIMER2_Init(void)

{

        uint8_t sreg = SREG;

/******************************************************************************************************************

Arthur P: Added initialization of the CameraShutterCycle value here as this routine is only

called once. This retains all code changes in timer0.c. If (parameter 8 & 127) > 0 then the user

has set a value for the cycle. CameraShuytterCycle == 5x (Para8 & 127) to get approx 0.1sec increments.

090807: Arthur P.: Removed the shutter cycle parts as they may be impacting timing loops.

20100804 Arthur P.: Reactivate shutter cycle counters. Modified to use the lower 7 bits of

user parameter 8 (bit 8 is used for enabling the external HEF4017).

CameraShutterCycle = Parameter_UserParam6;

******************************************************************************************************************/

        CameraShutterCycle = 5 *  (Parameter_UserParam8 & 127);

        // disable all interrupts before reconfiguration

        cli();

        PORTD &= ~(1<<PORTD7);  // set PD7 to low

        DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)

    HEF4017R_ON;

        // Timer/Counter 2 Control Register A

        // Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)

    // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)

    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)

        TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0));

    TCCR2A |= (1<<WGM21)|(1<<WGM20);

    // Timer/Counter 2 Control Register B

        // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz

        // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us

        // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms

    // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)

        TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22));

    TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22);

        // Initialize the Timer/Counter 2 Register

    TCNT2 = 0;

        // Initialize the Output Compare Register A used for PWM generation on port PD7.

        OCR2A = 255;

        TCCR2A |= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0

        // Timer/Counter 2 Interrupt Mask Register

        // Enable timer output compare match A Interrupt only

        TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));

        TIMSK2 |= (1<<OCIE2A);

    SREG = sreg;

}

//----------------------------

void Timer_Init(void)

{

    tim_main = SetDelay(10);

    TCCR0B = CK8;

    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM

    OCR0A =  0;

    OCR0B = 180;

    TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE;  // reload

    //OCR1  = 0x00;

    TIMSK0 |= _BV(TOIE0);

}

/*****************************************************/

/*              Control Servo Position               */

/*****************************************************/

ISR(TIMER2_COMPA_vect)

{

        // frame len 22.5 ms = 14063 * 1.6 us

        // stop pulse: 0.3 ms = 188 * 1.6 us

        // min servo pulse: 0.6 ms =  375 * 1.6 us

        // max servo pulse: 2.4 ms = 1500 * 1.6 us

        // resolution: 1500 - 375 = 1125 steps

        #define IRS_RUNTIME 127

        #define PPM_STOPPULSE 188

//      #define PPM_FRAMELEN (14063

    #define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)

        #define MINSERVOPULSE 375

        #define MAXSERVOPULSE 1500

        #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)

        static uint8_t  PulseOutput = 0;

        static uint16_t RemainingPulse = 0;

        static uint16_t ServoFrameTime = 0;

        static uint8_t  ServoIndex = 0;

        #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

/******************************************************************************************************************

Arthur P: Modified the code to scheck the value of parameter 8. If 128 or higher then a HEF4017 is

expected and will be used. Else J7 and J9 are seen as separate normal outputs.

if((PlatinenVersion < 20)

091114. Inserted same changes into v.0.76g code.

20100802 Inserted same changes into v.0.80d code.

******************************************************************************************************************/    

//      if(PlatinenVersion < 20)

        if((PlatinenVersion < 20) && (Parameter_UserParam8 < 128 ))

        {

                //---------------------------

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

/******************************************************************************************************************

Arthur P: Shutter Servo including interval control over parameter 5 and 6.

091114 Inserted same modification into v.0.76g code, removing previously REM-ed out modified parts.

20100802 Inserted same modification into v.0.76g code, removing previously REM-ed out modified parts.

Modified to use lower 7 bits of user parameter 7.

******************************************************************************************************************/

                                         case 3:

//                                                      RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER;

//                                                      break;

                                                        if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -32)

                                                        {

                                                        // Set servo to null position, turning camera off.

                                                                RemainingPulse  = MINSERVOPULSE;

                                                        }

                                                        else

                                                        {

                                                                // 090807: Arthur P.: Removed the shutter cycle parts as they may be impacting timing loops.

                                                                // 20100804 Reactived shutter interval timer capability.

                                                                if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 32)

                                                                // Top position on a 3 position switch which runs from -127 to +127

                                                                {

                                                                        RemainingPulse  = MINSERVOPULSE + SERVORANGE;

                                                                }

                                                                else

                                                                {

                                                                        // Cycle shutter servo between 50% on and off depending upon CameraShutterCycleCounter

                                                                        // If CameraShutterCylce < 50 then default to continuous shoot.

                                                                        if(CameraShutterCycle < 50 )    // == 5x the minimum value of userpara8 lower 7 bits

                                                                        {

                                                                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2;

                                                                        }

                                                                        else

                                                                        {      

                                                                                if(CameraShutterCycleCounter == CameraShutterCycle)

                                                                                {

                                                                                        // Shutter on

                                                                                        CameraShutterCycleCounter = 0;

                                                                                        RemainingPulse  = MINSERVOPULSE + SERVORANGE/2;

                                                                                }

                                                                                else

                                                                                {

                                                                                        // Leave on for at least 20 cycles or 0.2 seconds to allow 

                                                                                        // the camera to properly trigger, turn off if past 0.2 sec.

                                                                                        // For now this is actually set via para5 to allow for a long enough

                                                                                        // shutter pulse for different cameras. Once it is clear what value

                                                                                        // works, this can be changed to a hardcoded value.

                                                                                        CameraShutterCycleCounter++;

                                                                                        if(CameraShutterCycleCounter == CameraShutterCycleOnCount)

                                                                                        {

                                                                                                // Shutter off

                                                                                                RemainingPulse = MINSERVOPULSE;

                                                                                        }

                                                                                }

                                                                        }

                                                                }      

                                                        }

                                                        break;

                                         case 4:

                                                        RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER;

                                                        break;

                                         case 5:

                                                        RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER;

                                                        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) || ServoActive == 2) 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

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

}