Subversion Repositories FlightCtrl

Rev

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

Rev 232 Rev 235
Line 46... Line 46...
46
                PORTB &= ~(1<<PB2);     // J8 auf Low (war ~125 µs auf High)            
46
                PORTB &= ~(1<<PB2);     // J8 auf Low (war ~125 µs auf High)            
Line 47... Line 47...
47
               
47
               
48
                if (MM3.AXIS == MM3_X) SPDR = 0x51;                     // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
48
                if (MM3.AXIS == MM3_X) SPDR = 0x51;                     // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
49
                else if (MM3.AXIS == MM3_Y) SPDR = 0x52;                // Micromag Period Select ist auf 1024 (0x50)
49
                else if (MM3.AXIS == MM3_Y) SPDR = 0x52;                // Micromag Period Select ist auf 1024 (0x50)
50
                else if (MM3.AXIS == MM3_Z) SPDR = 0x53;                // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
-
 
Line 51... Line 50...
51
                else {MM3.STATE == MM3_IDLE;break;}
50
                else if (MM3.AXIS == MM3_Z) SPDR = 0x53;                // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
52
               
51
               
53
                MM3.DRDY = SetDelay(15);                // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024)
52
                MM3.DRDY = SetDelay(15);                // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024)
Line 78... Line 77...
78
                if (MM3.AXIS == MM3_Z) {MM3.z_axis=SPDR; MM3.z_axis<<=8; SPDR=0x00; MM3.STATE=MM3_Z_BYTE2; break;}
77
                if (MM3.AXIS == MM3_Z) {MM3.z_axis=SPDR; MM3.z_axis<<=8; SPDR=0x00; MM3.STATE=MM3_Z_BYTE2; break;}
Line 79... Line 78...
79
       
78
       
80
        case MM3_X_BYTE2:       // 2. Byte der entsprechenden Achse ist da.             
79
        case MM3_X_BYTE2:       // 2. Byte der entsprechenden Achse ist da.             
81
                MM3.x_axis |= SPDR;
80
                MM3.x_axis |= SPDR;
82
                MM3.x_axis -= OFF_X;    // Sofort Offset aus der Kalibrierung berücksichtigen
-
 
83
                //MM3.x_axis /= GAIN_X;
81
                MM3.x_axis -= OFF_X;    // Sofort Offset aus der Kalibrierung berücksichtigen
84
                MM3.AXIS = MM3_Y;
82
                MM3.AXIS = MM3_Y;
85
                MM3.STATE = MM3_RESET;
83
                MM3.STATE = MM3_RESET;
Line 86... Line 84...
86
                break;
84
                break;
87
       
85
       
88
        case MM3_Y_BYTE2:
86
        case MM3_Y_BYTE2:
89
                MM3.y_axis |= SPDR;            
-
 
90
                MM3.y_axis -= OFF_Y;
87
                MM3.y_axis |= SPDR;            
91
                //MM3.y_axis /= GAIN_Y;
88
                MM3.y_axis -= OFF_Y;
92
                MM3.AXIS = MM3_Z;
89
                MM3.AXIS = MM3_Z;
Line 93... Line 90...
93
                MM3.STATE = MM3_RESET;
90
                MM3.STATE = MM3_RESET;
94
                break;
91
                break;
95
       
92
       
96
        case MM3_Z_BYTE2:
-
 
97
                MM3.z_axis |= SPDR;
93
        case MM3_Z_BYTE2:
98
                MM3.z_axis -= OFF_Z;
94
                MM3.z_axis |= SPDR;
99
                //MM3.z_axis /= GAIN_Z;                 
95
                MM3.z_axis -= OFF_Z;
100
                MM3.AXIS = MM3_X;
96
                MM3.AXIS = MM3_X;
101
                MM3.STATE = MM3_RESET;         
97
                MM3.STATE = MM3_RESET;         
Line 119... Line 115...
119
        cos_roll = (float)cos_f(MM3.RollGrad);      
115
        cos_roll = (float)cos_f(MM3.RollGrad);      
Line 120... Line 116...
120
       
116
       
121
    //Neigungskompensation
117
    //Neigungskompensation
122
        y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
118
        y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
-
 
119
    x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis);
123
    x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis);
120
       
124
        //Winkelberechnung
121
        //Winkelberechnung
Line 125... Line 122...
125
        heading = arctan_i(x_corr, y_corr);
122
        heading = arctan_i(x_corr, y_corr);
126
 
123