Subversion Repositories FlightCtrl

Rev

Rev 276 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 276 Rev 278
Line 48... Line 48...
48 
                return;
 48 
                return;
Line 49... Line 49...
49 
               
 49 
               
50 
        case MM3_START_TRANSFER:
 50 
        case MM3_START_TRANSFER:
Line 51... Line 51...
51 
                PORTB &= ~(1<<PB2);     // J8 auf Low (war ~125 µs auf High)           
51 
                PORTB &= ~(1<<PB2);     // J8 auf Low (war ~125 µs auf High)           
52 
               
 52 
               
53 
                if (MM3.AXIS == MM3_X) SPDR = 0x51;                     // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
 53 
                if (MM3.AXIS == MM3_X) SPDR = 0x41;                     // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
Line 54... Line 54...
54 
                else if (MM3.AXIS == MM3_Y) SPDR = 0x52;                // Micromag Period Select ist auf 1024 (0x50)
 54 
                else if (MM3.AXIS == MM3_Y) SPDR = 0x42;                // Micromag Period Select ist auf 1024 (0x50)
55 
                else SPDR = 0x53;                                                               // if (MM3.AXIS == MM3_Z)
55 
                else if (MM3.AXIS == MM3_Z) SPDR = 0x43;                // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
56 
               
 56 
               
Line 57... Line 57...
57 
                MM3.DRDY = SetDelay(15);                // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024)
 57 
                MM3.DRDY = SetDelay(15);                // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024)
58 
                MM3.STATE = MM3_WAIT_DRDY;
 58 
                MM3.STATE = MM3_WAIT_DRDY;
59 
                return;
 59 
                return;
Line 60... Line 60...
60 
       
 60 
       
61 
        case MM3_WAIT_DRDY:
 61 
        case MM3_WAIT_DRDY:
62 
                if (CheckDelay(MM3.DRDY)) {SPDR = 0x00; MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
 62 
                if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
Line 78... Line 78...
78 
//############################################################################
 78 
//############################################################################
79 
{      
 79 
{      
80 
        switch (MM3.STATE)
 80 
        switch (MM3.STATE)
81 
        {      
 81 
        {      
82 
        case MM3_DRDY:
 82 
        case MM3_DRDY:
83 
                        // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken
 83 
                        // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken und Übertragung von 2. Byte auslösen
84 
                if (MM3.AXIS == MM3_X)
 84 
                if (MM3.AXIS == MM3_X) {MM3.x_axis=SPDR; MM3.x_axis<<=8; SPDR=0x00; MM3.STATE=MM3_X_BYTE2; return;}
85 
                        {
 - 
 
86 
                        MM3.x_axis=SPDR;
 85 
                if (MM3.AXIS == MM3_Y) {MM3.y_axis=SPDR; MM3.y_axis<<=8; SPDR=0x00; MM3.STATE=MM3_Y_BYTE2; return;}
87 
                        MM3.x_axis<<=8;
 86 
                if (MM3.AXIS == MM3_Z) {MM3.z_axis=SPDR; MM3.z_axis<<=8; SPDR=0x00; MM3.STATE=MM3_Z_BYTE2; return;}
88 
                        }
 87 
       
89 
                else if (MM3.AXIS == MM3_Y)
 88 
        case MM3_X_BYTE2:       // 2. Byte der entsprechenden Achse ist da.            
90 
                        {
 - 
 
91 
                        MM3.y_axis=SPDR;
 89 
                MM3.x_axis |= SPDR;
92 
                        MM3.y_axis<<=8;
 - 
 
93 
                        }
 - 
 
94 
                else    // if (MM3.AXIS == MM3_Z)
 90 
                MM3.x_axis -= OFF_X;    // Sofort Offset aus der Kalibrierung berücksichtigen
95 
                        {
 - 
 
96 
                        MM3.z_axis=SPDR;
 91 
                MM3.AXIS = MM3_Y;
97 
                        MM3.z_axis<<=8;
 - 
 
98 
                        }
 - 
 
99 
               
 - 
 
100 
                SPDR=0x00;                      // Übertragung von 2. Byte auslösen
 - 
 
101 
                MM3.STATE=MM3_BYTE2;
 92 
                MM3.STATE = MM3_RESET;
102 
                return;
 93 
                return;
103 
               
 94 
       
104 
        case MM3_BYTE2:                         // 2. Byte der entsprechenden Achse ist da.
 - 
 
105 
                if (MM3.AXIS == MM3_X)
 95 
        case MM3_Y_BYTE2:
106 
                        {
 - 
 
107 
                        MM3.x_axis |= SPDR;
 96 
                MM3.y_axis |= SPDR;            
108 
                        MM3.x_axis -= OFF_X;
 97 
                MM3.y_axis -= OFF_Y;
109 
                        MM3.AXIS = MM3_Y;
 98 
                MM3.AXIS = MM3_Z;
110 
                        MM3.STATE = MM3_RESET;
 99 
                MM3.STATE = MM3_RESET;
111 
                        }
 100 
                return;
112 
                else if (MM3.AXIS == MM3_Y)
 - 
 
113 
                        {
 101 
       
114 
                        MM3.y_axis |= SPDR;
 - 
 
115 
                        MM3.y_axis -= OFF_Y;
 - 
 
116 
                        MM3.AXIS = MM3_Z;
 - 
 
117 
                        MM3.STATE = MM3_RESET;
 102 
        case MM3_Z_BYTE2:
118 
                        }
 - 
 
119 
                else    // if (MM3.AXIS == MM3_Z)
 - 
 
120 
                        {
 - 
 
121 
                        MM3.z_axis |= SPDR;
 103 
                MM3.z_axis |= SPDR;
122 
                        MM3.z_axis -= OFF_Z;
 104 
                MM3.z_axis -= OFF_Z;
123 
                        MM3.STATE = MM3_TILT;
 105 
                MM3.STATE = MM3_TILT;
124 
                        }
 - 
 
125 
               
 - 
 
126 
                return;
 106 
                return;
127 
        }
 107 
        }
128 
}
 108 
}
Line 129... Line 109...
129 
 
 109 
 
Line 142... Line 122...
142 
    // Neigungskompensation
 122 
    // Neigungskompensation
143 
        x_corr = (cos_nick * MM3.x_axis) + (((sin_roll *  MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick);
 123 
        x_corr = (cos_nick * MM3.x_axis) + (((sin_roll *  MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick);
144 
        y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
 124 
        y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
Line 145... Line 125...
145 
       
 125 
       
146 
        // Winkelberechnung
 126 
        // Winkelberechnung
Line 147... Line 127...
147 
        heading = atan2_i(x_corr, y_corr);
 127 
        heading = arctan_i(x_corr, y_corr);
148 
 
 128