Subversion Repositories FlightCtrl

Rev

Rev 438 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 438 Rev 466
Line 24... Line 24...
24 
//############################################################################
 24 
//############################################################################
25 
// Initialisierung
 25 
// Initialisierung
26 
void init_MM3(void)
 26 
void init_MM3(void)
27 
//############################################################################
 27 
//############################################################################
28 
{
 28 
{
29 
        SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1);  //Interrupt an, Master, 625 kHz Oszillator
 29 
        SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0);        //Interrupt an, Master, 156 kHz Oszillator
30 
        SPSR = (1<<SPI2X);
 30 
        //SPSR = (1<<SPI2X);
Line 31... Line 31...
31 
       
 31 
       
Line 32... Line 32...
32 
    DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang
 32 
    DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang
Line 56... Line 56...
56 
        case MM3_START_TRANSFER:
 56 
        case MM3_START_TRANSFER:
57 
                PORTB &= ~(1<<PB2);     // J8 auf Low (war ~125 µs auf High)           
57 
                PORTB &= ~(1<<PB2);     // J8 auf Low (war ~125 µs auf High)           
Line 58... Line 58...
58 
               
 58 
               
59 
                if (MM3.AXIS == MM3_X) SPDR = 0x31;                     // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
 59 
                if (MM3.AXIS == MM3_X) SPDR = 0x31;                     // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
60 
                else if (MM3.AXIS == MM3_Y) SPDR = 0x32;                // Micromag Period Select ist auf 256 (0x30)
 60 
                else if (MM3.AXIS == MM3_Y) SPDR = 0x32;                // Micromag Period Select ist auf 256 (0x30)
Line 61... Line 61...
61 
                else if (MM3.AXIS == MM3_Z) SPDR = 0x33;                // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
 61 
                else SPDR = 0x33;       //if (MM3.AXIS == MM3_Z)        // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
62 
               
 62 
               
63 
                MM3.DRDY = SetDelay(5);         // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms)
 63 
                MM3.DRDY = SetDelay(8);         // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms)
Line 64... Line 64...
64 
                MM3.STATE = MM3_WAIT_DRDY;
 64 
                MM3.STATE = MM3_WAIT_DRDY;
65 
                return;
 65 
                return;
Line 174... Line 174...
174 
signed int heading_MM3(void)
 174 
signed int heading_MM3(void)
175 
//############################################################################
 175 
//############################################################################
176 
{
 176 
{
177 
        float sin_nick, cos_nick, sin_roll, cos_roll;
 177 
        float sin_nick, cos_nick, sin_roll, cos_roll;
178 
        float x_corr, y_corr;
 178 
        float x_corr, y_corr;
179 
        signed int x_axis,y_axis,z_axis, heading;
 179 
        signed int x_axis,y_axis,z_axis, heading, nicktilt, rolltilt;
180 
        unsigned int div_faktor;
 180 
        unsigned int div_faktor;
Line 181... Line 181...
181 
               
 181 
               
Line 182... Line 182...
182 
        div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8;
 182 
        div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8;
183 
       
 183 
       
184 
        // Berechung von sinus und cosinus
 - 
 
185 
        MM3.NickGrad = (IntegralNick/div_faktor);
 184 
        // Berechung von sinus und cosinus
186 
        //MM3.NickGrad = asin_i(MM3.NickGrad);
 185 
        nicktilt = (IntegralNick/div_faktor);
187 
        sin_nick = sin_f(MM3.NickGrad);
 186 
        sin_nick = sin_f(nicktilt);
188 
        cos_nick = cos_f(MM3.NickGrad);
 187 
        cos_nick = cos_f(nicktilt);
189 
       
 - 
 
190 
        MM3.RollGrad = (IntegralRoll/div_faktor);
 188 
       
191 
        //MM3.RollGrad = asin_i(MM3.RollGrad);
 189 
        rolltilt = (IntegralRoll/div_faktor);
Line 192... Line 190...
192 
        sin_roll = sin_f(MM3.RollGrad);
 190 
        sin_roll = sin_f(rolltilt);
193 
        cos_roll = cos_f(MM3.RollGrad);
 191 
        cos_roll = cos_f(rolltilt);
194 
       
 192 
       
195 
        // Offset
 193 
        // Offset