WebSVN - FlightCtrl - Rev 1379 - /branches/Micromag3 auslesen beta pakoxda/spi.c

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

Subversion Repositories FlightCtrl

(root)/branches/Micromag3 auslesen beta pakoxda/spi.c - Rev 1379