Rev 391 |
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)
//############################################################################
{
SPCR
= (1<<SPIE
)|(1<<SPE
)|(1<<MSTR
)|(1<<SPR1
); //Interrupt an, Master, 625 kHz Oszillator
SPSR
= (1<<SPI2X
);
DDRB
|= (1<<PB7
)|(1<<PB5
)|(1<<PB2
); // J8, MOSI, SCK Ausgang
PORTD
&= ~
(1<<PD3
); // J5 auf Low
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
:
PORTB
|= (1<<PB2
); // J8 auf High, MM3 Reset
MM3.
STATE = MM3_START_TRANSFER
;
return;
case MM3_START_TRANSFER
:
PORTB
&= ~
(1<<PB2
); // J8 auf Low (war ~125 µs auf High)
if (MM3.
AXIS == MM3_X
) SPDR
= 0x31; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
else if (MM3.
AXIS == MM3_Y
) SPDR
= 0x32; // Micromag Period Select ist auf 256 (0x30)
else if (MM3.
AXIS == MM3_Z
) SPDR
= 0x33; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
MM3.
DRDY = SetDelay
(5); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 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
)
//############################################################################
{
switch (MM3.
STATE)
{
case MM3_DRDY
: // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken
if (MM3.
AXIS == MM3_X
)
{
MM3.
x_axis = SPDR
;
MM3.
x_axis <<= 8;
}
else if (MM3.
AXIS == MM3_Y
)
{
MM3.
y_axis = SPDR
;
MM3.
y_axis <<= 8;
}
else // if (MM3.AXIS == MM3_Z)
{
MM3.
z_axis = SPDR
;
MM3.
z_axis <<= 8;
}
SPDR
=0x00; // Übertragung von 2. Byte auslösen
MM3.
STATE=MM3_BYTE2
;
return;
case MM3_BYTE2
: // 2. Byte der entsprechenden Achse ist da
if (MM3.
AXIS == MM3_X
)
{
MM3.
x_axis |= SPDR
;
// Spikes filtern
if (abs(MM3.
x_axis) < Max_Axis_Value
) MM3.
x_axis_old = MM3.
x_axis;
else MM3.
x_axis = MM3.
x_axis_old;
MM3.
AXIS = MM3_Y
;
MM3.
STATE = MM3_RESET
;
}
else if (MM3.
AXIS == MM3_Y
)
{
MM3.
y_axis |= SPDR
;
if (abs(MM3.
y_axis) < Max_Axis_Value
) MM3.
y_axis_old = MM3.
y_axis;
else MM3.
y_axis = MM3.
y_axis_old;
MM3.
AXIS = MM3_Z
;
MM3.
STATE = MM3_RESET
;
}
else // if (MM3.AXIS == MM3_Z)
{
MM3.
z_axis |= SPDR
;
if (abs(MM3.
z_axis) < Max_Axis_Value
) MM3.
z_axis_old = MM3.
z_axis;
else MM3.
z_axis = MM3.
z_axis_old;
MM3.
AXIS = MM3_X
;
MM3.
STATE = MM3_RESET
;
}
return;
}
}
//############################################################################
// Kompass kalibrieren
void calib_MM3
(void)
//############################################################################
{
signed int x_min
=0,x_max
=0,y_min
=0,y_max
=0,z_min
=0,z_max
=0;
uint8_t measurement
=50,beeper
=0;
unsigned int timer
;
GRN_ON
;
ROT_OFF
;
while (measurement
)
{
//H_earth = MM3.x_axis*MM3.x_axis + MM3.y_axis*MM3.y_axis + MM3.z_axis*MM3.z_axis;
if (MM3.
x_axis > x_max
) x_max
= MM3.
x_axis;
else if (MM3.
x_axis < x_min
) x_min
= MM3.
x_axis;
if (MM3.
y_axis > y_max
) y_max
= MM3.
y_axis;
else if (MM3.
y_axis < y_min
) y_min
= MM3.
y_axis;
if (MM3.
z_axis > z_max
) z_max
= MM3.
z_axis;
else if (MM3.
z_axis < z_min
) z_min
= MM3.
z_axis;
if (!beeper
)
{
ROT_FLASH
;
GRN_FLASH
;
beeptime
= 50;
beeper
= 50;
}
beeper
--;
// Schleife mit 100 Hz
timer
= SetDelay
(10);
while(!CheckDelay
(timer
));
// 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
signed int heading_MM3
(void)
//############################################################################
{
float sin_nick
, cos_nick
, sin_roll
, cos_roll
;
float x_corr
, y_corr
;
signed int x_axis
,y_axis
,z_axis
, heading
;
unsigned int div_faktor
;
div_faktor
= (uint16_t)EE_Parameter.
UserParam1 * 8;
// Berechung von sinus und cosinus
MM3.
NickGrad = (IntegralNick
/div_faktor
);
//MM3.NickGrad = asin_i(MM3.NickGrad);
sin_nick
= sin_f
(MM3.
NickGrad);
cos_nick
= cos_f
(MM3.
NickGrad);
MM3.
RollGrad = (IntegralRoll
/div_faktor
);
//MM3.RollGrad = asin_i(MM3.RollGrad);
sin_roll
= sin_f
(MM3.
RollGrad);
cos_roll
= cos_f
(MM3.
RollGrad);
// Offset
x_axis
= (MM3.
x_axis - MM3_calib.
X_off);
y_axis
= (MM3.
y_axis - MM3_calib.
Y_off);
z_axis
= (MM3.
z_axis - MM3_calib.
Z_off);
// Normierung Wertebereich
if ((MM3_calib.
X_range > MM3_calib.
Y_range) && (MM3_calib.
X_range > MM3_calib.
Z_range))
{
y_axis
= ((long)y_axis
* MM3_calib.
X_range) / MM3_calib.
Y_range;
z_axis
= ((long)z_axis
* MM3_calib.
X_range) / MM3_calib.
Z_range;
}
else if ((MM3_calib.
Y_range > MM3_calib.
X_range) && (MM3_calib.
Y_range > MM3_calib.
Z_range))
{
x_axis
= ((long)x_axis
* MM3_calib.
Y_range) / MM3_calib.
X_range;
z_axis
= ((long)z_axis
* MM3_calib.
Y_range) / MM3_calib.
Z_range;
}
else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range))
{
x_axis
= ((long)x_axis
* MM3_calib.
Z_range) / MM3_calib.
X_range;
y_axis
= ((long)y_axis
* MM3_calib.
Z_range) / MM3_calib.
Y_range;
}
DebugOut.
Analog[9] = x_axis
;
DebugOut.
Analog[10] = y_axis
;
DebugOut.
Analog[11] = z_axis
;
// Neigungskompensation
x_corr
= x_axis
* cos_nick
;
x_corr
+= y_axis
* sin_roll
* sin_nick
;
x_corr
-= z_axis
* cos_roll
* sin_nick
;
y_corr
= y_axis
* cos_roll
;
y_corr
+= z_axis
* sin_roll
;
// Winkelberechnung
heading
= atan2_i
(x_corr
, y_corr
);
if (heading
< 0) heading
= -heading
;
else heading
= 360 - heading
;
/*
if (!x_corr && y_corr <0) return (90);
if (!x_corr && y_corr >0) return (270);
heading = atan(y_corr/x_corr)*57.29578;
if (x_corr < 0) heading = 180-heading;
if (x_corr > 0 && y_corr < 0) heading = -heading;
if (x_corr > 0 && y_corr > 0) heading = 360 - heading;
*/
return (heading
);
}