| 0,0 → 1,623 |
|---|
| /* |
| 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; |
| } |
| } |