Subversion Repositories FlightCtrl

#include <avr/io.h>
#include <stdlib.h>
#include "fc.h"
#include "timer0.h"
#include "uart.h"

int32_t PWMHeading = -1;
uint8_t PWMTimeout = 0;

/*********************************************/
/*  Initialize Interface to CMPS02 Compass   */
/*********************************************/
void CMPS03_Init(void)
{
        // Port PC4 connected to PWM output from compass module
        DDRC &= ~(1<<DDC4); // set as input
        PORTC |= (1<<PORTC4); // pull up  to increase PWM counter also 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++;
                // pwm overflow?
                if (PWMCount > 400)
                {
                        if(PWMTimeout ) PWMTimeout--; // decrement timeout
                        PWMCount = 0; // reset PWM Counter
                }

        }
        else // PWM is low
        {
                if((PWMCount) && (PWMCount < 400))
                {
                        if(PWMCount <9) PWMHeading = 0;
                        else PWMHeading = ((uint32_t)(PWMCount - 9) * 1049L)/1024; // 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
                }
                PWMCount = 0; // reset pwm counter
        }
}


/*********************************************/
/*  Calculate north direction (heading)      */
/*********************************************/
int16_t CMPS03_Heading(void)
{
        int16_t heading, w, v;

        if(PWMTimeout)
        {
                w = abs(IntegralPitch / 512);
                v = abs(IntegralRoll  / 512);
            if(v > w) w = v; // get maximum declination
                // if declination is small enough to have valid compass heading
                if(w < 35)
                {
                        // range from 0 to 359
                        heading = (int16_t)PWMHeading;
                        if (heading < 0) heading += 360;
                        heading = heading%360;
                }
                else // compass to much tilted
                {
                        heading = -1;
                }
        }
        else // no data from compass
        {
                if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem
                heading = -1;
        }
        return heading;
}