Subversion Repositories FlightCtrl

#include <avr/io.h>



int32_t PWMHeading = -1;
uint8_t PWMTimeout = 0;

/*********************************************/
/*  Initialize Interface to CMPS02 Compass   */
/*********************************************/
void CMPS03_Init(void)
{
        // Port PC5 connected to PWM output from compass module
        DDRC &= ~(1<<DDC4); // set as input
        PORTC |= (1<<PORTC4); // pull up  to increase PWM counter allo if nothing is connected

        PWMTimeout = 0;
}


/*********************************************/
/*  Get Data from CMPS03                     */
/*********************************************/
void CMPS03_Update(void) // called every 102.4 us by timer 0 ISR
{
        static uint16_t PWMCount = 0;
        // The pulse width varies from 1ms (0°) to 36.99ms (359.9°)
        // in other words 100us/° with a +1ms offset.
        // The signal goes low for 65ms between pulses,
        // so the cycle time is 65mS + the pulse width.
        // The pulse is generated by a 16 bit timer in the processor
        // giving a 1uS resolution, however I would not recommend
        // measuring this to anything better than 0.1°  (10uS).

        if(PINC & (1<<PINC4))
        {       // If PWM signal is high increment PWM high counter
                // This counter is incremented by a periode of 102.4us,
                // i.e. the resoluton of pwm coded heading is approx. 1 deg.
                PWMCount++;
        }
        else // PWM is low
        {
                if((PWMCount) && (PWMCount < 400))
                {
                        PWMHeading = (((uint32_t)PWMCount * 1024L) / 1000L) - 10; // correct timebase and offset
                        PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid
                        // 12 * 400 counts * 102.4 us = 419 ms
                }
                else // PWM counter is over the pwm periode of 37 ms
                { // overflow at low edge detection
                        if(PWMTimeout ) PWMTimeout--;
                }
                PWMCount = 0; // reset pwm counter
        }
        // overflow without a low edge (nothing connected to PC4)
        if (PWMCount > 400)
        {
                if(PWMTimeout ) PWMTimeout--;
                PWMCount = 0;
        }


}


/*********************************************/
/*  Calculate north direction (heading)      */
/*********************************************/
int16_t CMPS03_Heading(void)
{
        int16_t heading;
        if(PWMTimeout)
        {       // range from 0 to 359
                heading = (int16_t)PWMHeading;
                if (heading < 0) heading += 360;
                heading = heading%360;
        }
        else // no data from compass
        {
                heading = -1;
        }
        return heading;
}