Subversion Repositories FlightCtrl

/*

This program (files spi.c and spi.h) is free software; you can redistribute it and/or modify

it under the terms of the GNU General Public License as published by the Free Software Foundation;

either version 3 of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;

without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

GNU General Public License for more details. You should have received a copy of the GNU General Public License

along with this program. If not, see <http://www.gnu.org/licenses/>.

Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de

*/

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Patrick Kögel

Micromag3 Auswertung beta

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

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

//

// File Name    : 'spi.c'

// Title                : SPI interface driver

// Author               : Pascal Stang - Copyright (C) 2000-2002

// Created              : 11/22/2000

// Revised              : 06/06/2002

// Version              : 0.6

// Target MCU   : Atmel AVR series

// Editor Tabs  : 4

//

// NOTE: This code is currently below version 1.0, and therefore is considered

// to be lacking in some functionality or documentation, or may not be fully

// tested.  Nonetheless, you can expect most functions to work.

//

// This code is distributed under the GNU Public License

//              which can be found at http://www.gnu.org/licenses/gpl.txt

//

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

#include "main.h"

// Define the SPI_USEINT key if you want SPI bus operation to be

// interrupt-driven.  The primary reason for not using SPI in

// interrupt-driven mode is if the SPI send/transfer commands

// will be used from within some other interrupt service routine

// or if interrupts might be globally turned off due to of other

// aspects of your program

//

// Comment-out or uncomment this line as necessary

//#define SPI_USEINT

// global variables

volatile unsigned char spiTransferComplete,MessungFertig=0,KompassInUse =0,KompassInUseCal=0,NewX= 0,NewY= 0,NewZ= 0,EEPROMWrite=0;

volatile signed char nickgrad=0,rollgrad=0,nickgradkomp=0,rollgradkomp=0;

volatile double XE= 0,YE=0,nick=0,roll=0;

volatile double richtungorgd=0,richtungorgr=0;

volatile int iXsf=0,iYsf=0,iZsf=0;

volatile int Xoffset=0, Yoffset=0, Zoffset=0;

volatile signed int Xmax=-32768 , Xmin=32767, Ymax=-32768, Ymin=32767, Zmin=32767, Zmax=-32768; // where you store max and min X and Ys

volatile int XrawOld=0, YrawOld=0, ZrawOld=0; // save values you get from the ASIC

volatile int Xraw=0, Yraw=0, Zraw=0; // values you get from the ASIC

volatile double XrawCal=0, YrawCal=0, ZrawCal=0; // values you get from the ASIC

double XEx=0,XEy=0,XEz=0,YEy=0,YEz=0;

int MagnetG=0;

typedef struct

{

        int Xoffsetm;

        int Yoffsetm;

        int Zoffsetm;

        int iXsfm;

        int iYsfm;

        int iZsfm;

        int dummy;

} CALPARAKOMPASS ;

CALPARAKOMPASS CalParaKompass;

volatile int azimuthgrad= 0,azimuthold = 0;

// SPI interrupt service handler

#ifdef SPI_USEINT

SIGNAL(SIG_SPI)

{

        spiTransferComplete = 1;

}

#endif

// access routines

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

// Name:

// Function:

//

// Parameter:

// Return:

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

void spiInit(void)

{

        // setup SPI I/O pins

        sbi(PORTB, 7);  // set SCK hi

        sbi(DDRB, 7);   // set SCK as output

        cbi(DDRB, 6);   // set MISO as input

        sbi(DDRB, 5);   // set MOSI as output

        sbi(DDRD, 5);   // SS must be output for Master mode to work

        sbi(DDRD, 3);   // Reset output

        cbi(DDRD, 4);   // DataReady as input

        // setup SPI interface :

        // master mode

        sbi(SPCR, MSTR);

        // clock = f/32  -> 20 MHZ /32

        sbi(SPCR, SPR0);

        sbi(SPCR, SPR1);

        cbi(SPSR,SPI2X);

        // select clock phase positive-going in middle of data

        cbi(SPCR, CPOL);

        // Data order MSB first

        cbi(SPCR,DORD);

        // enable SPI

        sbi(SPCR, SPE);

        // clear status

        inb(SPSR);

        sbi(SPSR,SPI2X);

        spiTransferComplete = 1;

        // enable SPI interrupt

        #ifdef SPI_USEINT

        sbi(SPCR, SPIE);

        #endif

}

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

// Name:

// Function:

//

// Parameter:

// Return:

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

void spiSendByte(unsigned char data)

{

        // send a byte over SPI and ignore reply

        #ifdef SPI_USEINT

                while(!spiTransferComplete);

                spiTransferComplete = FALSE;

        #else

                while(!(inb(SPSR) & (1<<SPIF)));

        #endif

        outb(SPDR, data);

}

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

// Name:

// Function:

//

// Parameter:

// Return:

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

unsigned char spiTransferByte(unsigned char data)

{

        #ifdef SPI_USEINT

        // send the given data

        spiTransferComplete = FALSE;

        outb(SPDR, data);

        // wait for transfer to complete

        while(!spiTransferComplete);

        #else

        // send the given data

        outb(SPDR, data);

        // wait for transfer to complete

        while(!(inb(SPSR) & (1<<SPIF)));

        #endif

        // return the received data

        return inb(SPDR);

}

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

// Name:

// Function:

//

// Parameter:

// Return:

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

unsigned int spiTransferWord(unsigned int data)

{

        unsigned int rxData = 0;

        // send MS byte of given data

        rxData = (spiTransferByte((data>>8) & 0x00FF))<<8;

        // send LS byte of given data

        rxData |= (spiTransferByte(data & 0x00FF));

        // return the received data

        return rxData;

}

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

 //---[ compassFilter ]----------------------------------------------------------

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

 /** Digital filter for the compass readings

  *  \n Filter by Vadym Grygorenko

  *  \n fn = (1/4)*(cn + 2*c_(n-1) + c_(n-2)) + (f_(n-1)) - (1/4)*f_(n-2)

  *

  *  \param   axiss     The axis this measurement is for

  *  \param   reading   The latest reading from the compass

  *  \return            The newest output from the filter

  *

  */

 int compassFilter(unsigned char axiss, int reading)

 {

    static int input[3][3]={{0,0,0},{0,0,0},{0,0,0}};       //[n, n-1, n-2][axis]

    static int output[3][3]={{0,0,0},{0,0,0},{0,0,0}};

    input[0][axiss]  = reading;

    output[0][axiss] = ((input[0][axiss] + 2*input[1][axiss] + input[2][axiss]) / 4);

    input[2][axiss]  = input[1][axiss];

    input[1][axiss]  = input[0][axiss];

    output[2][axiss] = output[1][axiss];

    output[1][axiss] = output[0][axiss];

   return output[0][axiss];

}

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

// Name:

// Function:

//

// Parameter:

// Return:

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

int MAG_READ(char axis)

{

        int t;

        int Data;

        char dontCare;              // For a reading we don't care about

        if (MessungFertig == 0)

        {

                PORTD &= ~(1<<PB5);                     //SS auf low

                PORTD |= (1<<PB3);                      //Reset auf high

                for(t=0;t<5;t++);                       //5 Zyklen warten

                PORTD &= ~(1<<PB3);                     //Reset auf low

                dontCare = (int)spiTransferByte(0x40+axis);     // Achse + Mess. Kommando /512

                PORTD |= (1<<PB5);                      //SS auf high

                MessungFertig = 1;

        }

        if (bit_is_set(PIND,PINB4) && MessungFertig == 1) // Warten bis Messung fertig

        {

                PORTD &= ~(1<<PB5);                     //SS auf low

                Data=spiTransferWord(0x00);

                PORTD |= (1<<PB5);                      //SS auf high

                MessungFertig = 0;

                return (Data);

        }

        else return -32000;

}

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

// Name:

// Function:

//

// Parameter:

// Return:

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

int MAG_READ_CAL(char axis)

{

        int t;

        int Data=0;

        char dontCare;              // For a reading we don't care about

        PORTD &= ~(1<<PB5);                     //SS auf low

        PORTD |= (1<<PB3);                      //Reset auf high

        for(t=0;t<5;t++);                       //5 Zyklen warten

        PORTD &= ~(1<<PB3);                     //Reset auf low

        dontCare = (int)spiTransferByte(0x40+axis);     // Achse + Mess. Kommando /512

        PORTD |= (1<<PB5);                      //SS auf high

        while(!bit_is_set(PIND,PINB4)) // Warten bis Messung fertig

        PORTD &= ~(1<<PB5);                     //SS auf low

        Data=spiTransferWord(0x00);

        PORTD |= (1<<PB5);                      //SS auf high

        return (Data);

}

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

// Name:

// Function:

//

// Parameter:

// Return:

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

void ReadEepromKompassValues(void)

{

        eeprom_read_block(&CalParaKompass, &EEPromArray[4], sizeof(CALPARAKOMPASS));

        Xoffset = CalParaKompass.Xoffsetm;

        Yoffset = CalParaKompass.Yoffsetm;

        Zoffset = CalParaKompass.Zoffsetm;

        iXsf = CalParaKompass.iXsfm;

        iYsf = CalParaKompass.iYsfm;

        iZsf = CalParaKompass.iZsfm;

}

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

// Name:

// Function:

//

// Parameter:

// Return:

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

void KalibrierKompass(void)

{

        // The numbers here will be 16-bit signed values.

        // Positive = 0x0000 to 0x7FFF

        // Negative = 0x8000 to 0xFFFF

        int t = 0;

        int tempx=0,tempy=0,tempz=0;

        if(Poti4 > 70 && Poti4 < 100)

        {

                LED1_OFF;

                LED2_OFF;

                EEPROMWrite = 0;

/*

                Xmax = -32768;

                Ymax = -32768; // start with lowest possible value

                Zmax = -32768;

                Xmin = 32767;

                Ymin = 32767; // start with highest possible value

                Zmin = 32767;

*/

        }

        if(Poti4 > 70)  // wenn Schalter ausgeschaltet, dann werden die LED ausgeschaltet und das GPS Steuerkommando gelöscht // (199797Kö)

        {

                if(Poti4 > 100 && Poti4 < 150)  // X-Y Achse Kalibrieren

                {

                        LED1_ON;

                        Xraw=MAG_READ_CAL(1);  //Kompass x-Achse auslesen

                        Xraw = -Xraw;

                        if(((Xraw > (XrawOld + 200)) || (Xraw < (XrawOld - 200)))) Xraw = XrawOld; //Filter gegen Spikes

                        tempx = Xraw;

                        Xraw = compassFilter(0,tempx);

                        XrawOld = Xraw;

                        Yraw=MAG_READ_CAL(2);  //Kompass y-Achse auslesen

                        Yraw =-Yraw;

                        if(((Yraw > (YrawOld + 200)) || (Yraw < (YrawOld - 200)))) Yraw = YrawOld; //Filter gegen Spikes

                        tempy = Yraw;

                        Yraw = compassFilter(1,tempy);

                        YrawOld = Yraw;

                        if( Xraw > Xmax ) Xmax = Xraw;

                        if( Xraw < Xmin ) Xmin = Xraw;

                        if( Yraw > Ymax ) Ymax = Yraw;

                        if( Yraw < Ymin ) Ymin = Yraw;

                        Xoffset = ( abs(Xmax) - abs(Xmin) ) / 2; // calculate offsets

                        Yoffset = ( abs(Ymax) - abs(Ymin) ) / 2;

                        iXsf = ( abs(Xmax) + abs(Xmin)) / 2; // calculate the range

                        iYsf = ( abs(Ymax) + abs(Ymin)) / 2;

                        XrawCal = (double)(Xraw - Xoffset)/(double)iXsf;

                        YrawCal = (double)(Yraw - Yoffset) /(double)iYsf;

                }

                if(Poti4 > 150)

                {

                        LED1_OFF;

                        LED2_ON;

                        Zraw=MAG_READ_CAL(3);  //Kompass Z-Achse auslesen

                        Zraw = -Zraw;

                        if(((Zraw > (ZrawOld + 200)) || (Zraw < (ZrawOld - 200)))) Zraw = ZrawOld; //Filter gegen Spikes

                        tempz = Zraw;

                        Zraw = compassFilter(2,tempz);

                        ZrawOld = Zraw;

                        if( Zraw > Zmax ) Zmax = Zraw;

                        if( Zraw < Zmin ) Zmin = Zraw;

                        Zoffset = ( abs(Zmax) - abs(Zmin) ) / 2;

                        iZsf = ( abs(Zmax) + abs(Zmin)) / 2;

                        ZrawCal = (double)(Zraw- Zoffset) / (double)iZsf;

                }

        }

        if(Poti4 > 170)

        {

                if (EEPROMWrite == 0)

                {

                        LED2_ON;

                        LED1_ON;

                        CalParaKompass.Xoffsetm = Xoffset;

                        CalParaKompass.Yoffsetm = Yoffset;

                        CalParaKompass.Zoffsetm = Zoffset;

                        CalParaKompass.iXsfm = iXsf;

                        CalParaKompass.iYsfm = iYsf;

                        CalParaKompass.iZsfm = iZsf;

                        eeprom_write_block(&CalParaKompass, &EEPromArray[4], sizeof(CALPARAKOMPASS));

                        Xmax = -32768;

                        Ymax = -32768; // start with lowest possible value

                        Zmax = -32768;

                        Xmin = 32767;

                        Ymin = 32767; // start with highest possible value

                        Zmin = 32767;

                        EEPROMWrite = 1;

                }

        }

}

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

// Name:

// Function:

//

// Parameter:

// Return:

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

int KompassRead(void)

{

/*

Declination = 1,209° changing by 0,118 °/year

Inclination = 64,779° changing by 0,004 °/year

X component = 20.529,42 changing by 10,87 nT/year

Y component = 433,23 changing by 42,54 nT/year

Z component = 43.596,06 changing by 32,61 nT/year

Horizontal Intensity = 20.533,99 changing by 11,8 nT/year

Total Intensity = 48.189,85 changing by 34,53 nT/year

*/

/*

Messung Real Nick

14              10

30              20

40              30

52              40

61              50

30              20 roll

39              28

19              12

50              35

*/

        double  azimuthrad = 0;

        double magnet = 68 /180 * M_PI;

        //Look-Up Tabelle für Korrekturfaktoren

        //float winkelkorr[5]={0.5,0.6,0.65,0.65,0.7};

        int lookup_nick =0,lookup_roll=0;

        int tempx=0,tempy=0,tempz=0;

        nickgrad = -(IntegralNick / 1430); //Teiler ermittelt durch Test bestätigung durch Holger

        rollgrad = -(IntegralRoll / 1430); //Teiler ermittelt durch Test bestätigung durch Holger

/*      if(nickgrad > 45) nickgradkomp = 45;

        else nickgradkomp = nickgrad;

        if(nickgrad < -45) nickgradkomp = -45;

        else nickgradkomp = nickgrad;

        if(rollgrad > 45) rollgradkomp = 45;

        else rollgradkomp = rollgrad;

        if(rollgrad < -45) rollgradkomp = -45;

        else rollgradkomp = rollgrad;

        if(abs(nickgrad) > 5 && abs(nickgrad) <= 15) lookup_nick = 0;

        if(abs(nickgrad) > 15 && abs(nickgrad) <= 25) lookup_nick = 1;

        if(abs(nickgrad) > 25 && abs(nickgrad) <= 35) lookup_nick = 2;

        if(abs(nickgrad) > 35 && abs(nickgrad) <= 45) lookup_nick = 3;

        if(abs(nickgrad) > 45) lookup_nick = 4;

        if(abs(rollgrad) > 5 && abs(rollgrad) <= 15) lookup_roll = 0;

        if(abs(rollgrad) > 15 && abs(rollgrad) <= 25) lookup_roll = 1;

        if(abs(rollgrad) > 25 && abs(rollgrad) <= 35) lookup_roll = 2;

        if(abs(rollgrad) > 35 && abs(rollgrad) <= 45) lookup_roll = 3;

        if(abs(rollgrad) > 45) lookup_roll = 4;

*/

        nick = (float)nickgrad/180 * M_PI;// * (float)winkelkorr[lookup_nick]*(float);

        roll = (float)rollgrad/180 * M_PI;// * (float)winkelkorr[lookup_roll]*(float);

        if(Poti4 > 70)  // wenn Schalter ausgeschaltet, dann werden die LED ausgeschaltet und das GPS Steuerkommando gelöscht // (199797Kö)

        {

                KalibrierKompass();

        }

        else

        {

                switch(KompassInUse)

                {

                case 0:

                        Xraw=MAG_READ(1);  //Kompass x-Achse auslesen

                        Xraw = -Xraw;

                        if (Xraw == 32000) Xraw = XrawOld;

                        else

                        {

                                //if(((Xraw > (XrawOld + 200)) || (Xraw < (XrawOld - 200)))) Xraw = XrawOld; //Filter gegen Spikes

                                XrawOld = Xraw;

                                tempx = Xraw;

                                Xraw = compassFilter(0, tempx);

                                NewX = 1;

                                KompassInUse = 1;

                        }

                        break;

                case 1:

                        Yraw=MAG_READ(2);  //Kompass y-Achse auslesen

                        Yraw = -Yraw;

                        if (Yraw == 32000) Yraw = YrawOld;

                        else

                        {

                                //if(((Yraw > (YrawOld + 200)) || (Yraw < (YrawOld - 200)))) Yraw = YrawOld; //Filter gegen Spikes

                                YrawOld = Yraw;

                                tempy = Yraw;

                                Yraw = compassFilter(1, tempy);

                                NewY = 1;

                                KompassInUse = 2;

                        }

                        break;

                case 2:

                        Zraw=MAG_READ(3);  //Kompass z-Achse auslesen

                        Zraw = -Zraw;

                        if (Zraw == 32000) Zraw = ZrawOld;

                        else

                        {

                                //if(((Zraw > (ZrawOld + 200)) || (Zraw < (ZrawOld - 200)))) Zraw = ZrawOld; //Filter gegen Spikes

                                ZrawOld = Zraw;

                                tempz = Zraw;

                                Zraw = compassFilter(2, tempz);

                                NewZ = 1;

                                KompassInUse = 0;

                        }

                        break;

                }

        /*      Xoffset =-44;

                Yoffset =-36;

                Zoffset =35;

                iXsf =242;

                iYsf =256;

                iZsf =265;

*/

                if(NewX == 1 && NewY == 1 && NewZ == 1)

                {

                        // Kalibrierung

                        XrawCal = (double)(Xraw - Xoffset)/ (double)iXsf;

                        YrawCal = (double)(Yraw - Yoffset) / (double)iYsf;

                        ZrawCal = (double)(Zraw- Zoffset) / (double)iZsf;

                        // 1.2 Grad Missweisung durch Geographischer und Magnetischer Nordpol

                        //MagnetG = (int)sqrt((long)(XrawCal * XrawCal) + (long)(YrawCal * YrawCal) + (long)(ZrawCal * ZrawCal));

                        //Kompensationsroutine

                        XEx = XrawCal * (double)cos(nick);

                        XEy = YrawCal * (double)sin(nick) * (double)sin(roll);

                        XEz = ZrawCal * (double)sin(nick) * (double)cos(roll);

                        YEy = YrawCal * (double)cos(roll);

                        YEz = ZrawCal * (double)sin(roll);

                        XE = XEx + XEy - XEz;

                        YE = YEy +  YEz;

                        if(XE==0 && YE <0) azimuthgrad = 90;

                        if(XE==0 && YE >0) azimuthgrad = 270;

                        if(XE < 0) azimuthgrad = 180-(atan(YE/XE)*180/M_PI);

                        if(XE > 0 && YE < 0) azimuthgrad = -(atan(YE/XE)*180/M_PI);

                        if(XE > 0 && YE > 0) azimuthgrad = 360 - (atan(YE/XE)*180/M_PI);

                        azimuthold = azimuthgrad;

                        NewX = 0;

                        NewY = 0;

                        NewZ = 0;

                        return azimuthgrad;

                }

                else return azimuthold;

        }

}