Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

/*
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;
        }
}