Rev 851 |
Blame |
Last modification |
View Log
| RSS feed
/*
Copyright 2007, Niklas Nold
This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser 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 Lesser General Public License for more details. You should have received a copy of the GNU Lesser 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
*/
#include "main.h"
struct MM3_calib_struct ee_calib EEMEM
; // Reservierung im EEPROM
struct MM3_working_struct MM3
;
struct MM3_calib_struct MM3_calib
;
//############################################################################
// Initialisierung
void init_MM3
(void)
//############################################################################
{
// SPI-Schnittstelle initialisieren
SPCR
= (1<<SPIE
)|(1<<SPE
)|(1<<MSTR
)|(1<<SPR1
)|(1<<SPR0
); // Interrupt an, Master, 156 kHz Oszillator
DDRB
|= (1<<PB7
)|(1<<PB5
); // MOSI, SCK Ausgang
DDRC
|= (1<<PC4
)|(1<<PC5
); // PC5 (RESET) und PC4 (SSNOT) als Ausgang
PORTC
|= (1<<PC4
); // PC4 (SSNOT) auf High -> MM3 passiv
PORTC
&= ~
(1<<PC5
); // PC5 (RESET) auf Low
// Init Statemachine
MM3.
AXIS = MM3_X
;
MM3.
STATE = MM3_RESET
;
// Kalibrierung aus dem EEprom lesen
eeprom_read_block
(&MM3_calib
,&ee_calib
,sizeof(struct MM3_calib_struct
));
}
//############################################################################
// Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen
void timer0_MM3
(void)
//############################################################################
{
switch (MM3.
STATE)
{
case MM3_RESET
:
PORTC
&= ~
(1<<PC4
); // MM3 aktiv
PORTC
|= (1<<PC5
); // MM3 Reset
MM3.
STATE = MM3_START_TRANSFER
;
return;
case MM3_START_TRANSFER
:
PORTC
&= ~
(1<<PC5
); // PC5 auf Low (war ~125 µs auf High)
if (MM3.
AXIS == MM3_X
) SPDR
= MM3_PERIOD_512
+ MM3_X_AXIS
; // Schreiben ins SPDR löst automatisch SPI-Übertragung (MOSI und MISO) aus
else if (MM3.
AXIS == MM3_Y
) SPDR
= MM3_PERIOD_512
+ MM3_Y_AXIS
; // Micromag Period Select ist 256 (0x30)
else SPDR
= MM3_PERIOD_512
+ MM3_Z_AXIS
; //if (MM3.AXIS == MM3_Z)
MM3.
DRDY = SetDelay
(10); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 512 eigentlich 8 ms)
MM3.
STATE = MM3_WAIT_DRDY
;
return;
case MM3_WAIT_DRDY
:
if (CheckDelay
(MM3.
DRDY)) {SPDR
= 0x00;MM3.
STATE = MM3_DRDY
;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
return; // Jetzt gehts weiter in SIGNAL (SIG_SPI)
}
}
//############################################################################
// SPI byte ready
SIGNAL
(SIG_SPI
)
//############################################################################
{
static char tmp
;
int value
;
switch (MM3.
STATE)
{
case MM3_DRDY
: // 1. Byte ist da, zwischenspeichern
tmp
= SPDR
;
SPDR
= 0x00; // Übertragung von 2. Byte auslösen
MM3.
STATE = MM3_BYTE2
;
return;
case MM3_BYTE2
: // 2. Byte der entsprechenden Achse ist da
value
= tmp
;
value
<<= 8; // 1. Byte an MSB-Stelle rücken
value
|= SPDR
; // 2. Byte dranpappen
if(abs(value
) < Max_Axis_Value
) // Spikes filtern. Zuweisung nur, wenn Max-Wert nicht überschritten
switch (MM3.
AXIS)
{
case MM3_X
:
MM3.
x_axis = value
;
MM3.
AXIS = MM3_Y
;
break;
case MM3_Y
:
MM3.
y_axis = value
;
MM3.
AXIS = MM3_Z
;
break;
default: //case MM3_Z:
MM3.
z_axis = value
;
MM3.
AXIS = MM3_X
;
}
PORTC
|= (1<<PC4
); // MM3 passiv
MM3.
STATE = MM3_RESET
;
}
}
//############################################################################
// Kompass kalibrieren
void calib_MM3
(void)
//############################################################################
{
int16_t x_min
=0,x_max
=0,y_min
=0,y_max
=0,z_min
=0,z_max
=0;
int16_t x_axis
, y_axis
, z_axis
;
uint8_t measurement
=50,beeper
=0;
GRN_ON
;
ROT_OFF
;
while (measurement
)
{
uint8_t tmp_sreg
= SREG
;
cli
();
x_axis
= MM3.
x_axis;
y_axis
= MM3.
y_axis;
z_axis
= MM3.
z_axis;
SREG
= tmp_sreg
;
if (x_axis
> x_max
) x_max
= x_axis
;
else if (x_axis
< x_min
) x_min
= x_axis
;
if (y_axis
> y_max
) y_max
= y_axis
;
else if (y_axis
< y_min
) y_min
= y_axis
;
if (z_axis
> z_max
) z_max
= z_axis
;
else if (z_axis
< z_min
) z_min
= z_axis
;
if (!beeper
)
{
ROT_FLASH
;
GRN_FLASH
;
beeptime
= 50;
beeper
= 50;
}
beeper
--;
// Schleife mit 100 Hz
Delay_ms
(10);
// Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden
if (PPM_in
[EE_Parameter.
Kanalbelegung[K_GAS
]] < 100) measurement
--;
}
// Wertebereich der Achsen
MM3_calib.
X_range = (x_max
- x_min
);
MM3_calib.
Y_range = (y_max
- y_min
);
MM3_calib.
Z_range = (z_max
- z_min
);
// Offset der Achsen
MM3_calib.
X_off = (x_max
+ x_min
) /2;
MM3_calib.
Y_off = (y_max
+ y_min
) /2;
MM3_calib.
Z_off = (z_max
+ z_min
) /2;
// und im EEProm abspeichern
eeprom_write_block
(&MM3_calib
,&ee_calib
,sizeof(struct MM3_calib_struct
));
}
//############################################################################
// Neigungskompensierung und Berechnung der Ausrichtung
int heading_MM3
(void)
//############################################################################
{
int16_t sin_nick
, cos_nick
, sin_roll
, cos_roll
;
int16_t mm3_x_axis
, mm3_y_axis
, mm3_z_axis
;
int32_t Hx
, Hy
, Hz
, x_corr
, y_corr
;
int16_t heading
;
int8_t tilt
;
// 16bit-Werte lesen
uint8_t tmp_sreg
= SREG
;
cli
();
mm3_x_axis
= MM3.
x_axis;
mm3_y_axis
= MM3.
y_axis;
mm3_z_axis
= MM3.
z_axis;
SREG
= tmp_sreg
;
int temp
= Aktuell_az
-550;
// Lage-Berechnung mittels Acc-Messwerte
tilt
= atan2_i
(temp
,AdWertAccNick
*64);
sin_nick
= sin_i
(tilt
);
cos_nick
= cos_i
(tilt
);
tilt
= atan2_i
(temp
,AdWertAccRoll
*64);
sin_roll
= sin_i
(tilt
);
cos_roll
= cos_i
(tilt
);
/*
// Lage-Berechnung mittels Gyro-Integral
uint16_t div_faktor;
div_faktor = (uint16_t)EE_Parameter.UserParam3 *8;
tilt = (IntegralNick /div_faktor);
sin_nick = sin_i(tilt);
cos_nick = cos_i(tilt);
tilt = (IntegralRoll /div_faktor);
sin_roll = sin_i(tilt);
cos_roll = cos_i(tilt);
*/
// Offset und Normalisierung
Hx
= (((int32_t)(mm3_x_axis
- MM3_calib.
X_off)) *512) /MM3_calib.
X_range;
Hy
= (((int32_t)(mm3_y_axis
- MM3_calib.
Y_off)) *512) /MM3_calib.
Y_range;
Hz
= (((int32_t)(mm3_z_axis
- MM3_calib.
Z_off)) *512) /MM3_calib.
Z_range;
// Neigungskompensierung
x_corr
= Hx
* cos_nick
;
x_corr
-= Hz
* sin_nick
;
x_corr
/= 1024;
y_corr
= Hy
* cos_roll
;
y_corr
+= Hz
* sin_roll
;
y_corr
/= 16; // atan2_i erwartet y_corr *64. Deshalb /16 und nicht /1024
// Winkelberechnung
heading
= atan2_i
(x_corr
, y_corr
);
// Skalieren von +-180° auf 0-360°
if (heading
< 0) heading
= -heading
;
else heading
= 360 - heading
;
return (heading
);
}