Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 195 → Rev 1379

/branches/Micromag3 auslesen beta pakoxda/spi.c
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;
}
}
/branches/Micromag3 auslesen beta pakoxda/spi.h
0,0 → 1,66
/*! \file spi.h \brief SPI interface driver. */
//*****************************************************************************
//
// File Name : 'spi.h'
// 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.
//
/// \ingroup driver_avr
/// \defgroup spi SPI (Serial Peripheral Interface) Function Library (spi.c)
/// \code #include "spi.h" \endcode
/// \par Overview
/// Provides basic byte and word transmitting and receiving via the AVR
/// SPI interface.  Due to the nature of SPI, every SPI communication operation
/// is both a transmit and simultaneous receive.
///
/// \note Currently, only MASTER mode is supported.
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************
 
// function prototypes
 
 
extern volatile double XE,YE;
extern volatile signed char nickgrad,rollgrad,nickgradkomp,rollgradkomp;
extern volatile double richtungorgr,richtungorgd,nick,roll;
extern volatile int iXsf,iYsf,iZsf;
extern volatile signed int Xmax, Xmin, Ymax, Ymin, Zmin, Zmax;
extern volatile double XrawCal, YrawCal, ZrawCal;
extern volatile int Xraw, Yraw, Zraw; // save values you get from the ASIC
extern volatile int Xoffset, Yoffset, Zoffset;
extern volatile int azimuthgrad;
extern double XEx,XEy,XEz,YEy,YEz;
int MagnetG;
// SPI interface initializer
void spiInit(void);
 
// spiSendByte(u08 data) waits until the SPI interface is ready
// and then sends a single byte over the SPI port. This command
// does not receive anything.
void spiSendByte(unsigned char data);
 
// spiTransferByte(u08 data) waits until the SPI interface is ready
// and then sends a single byte over the SPI port. The function also
// returns the byte that was received during transmission.
unsigned char spiTransferByte(unsigned char data);
 
// spiTransferWord(u08 data) works just like spiTransferByte but
// operates on a whole word (16-bits of data).
unsigned int spiTransferWord(unsigned int data);
 
 
extern int KompassRead(void);
extern void ReadEepromKompassValues(void);