Subversion Repositories FlightCtrl

Rev

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

Rev 229 Rev 232
Line 48... Line 48...
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
50
                else if (MM3.AXIS == MM3_Z) SPDR = 0x53;                // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
51
                else {MM3.STATE == MM3_IDLE;break;}
51
                else {MM3.STATE == MM3_IDLE;break;}
Line 52... Line 52...
52
               
52
               
53
                MM3.DRDY = SetDelay(10);                // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024)
53
                MM3.DRDY = SetDelay(15);                // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024)
54
                MM3.STATE = MM3_WAIT_DRDY;
54
                MM3.STATE = MM3_WAIT_DRDY;
Line 55... Line 55...
55
                break;
55
                break;
56
       
56
       
Line 98... Line 98...
98
                MM3.z_axis -= OFF_Z;
98
                MM3.z_axis -= OFF_Z;
99
                //MM3.z_axis /= GAIN_Z;                 
99
                //MM3.z_axis /= GAIN_Z;                 
100
                MM3.AXIS = MM3_X;
100
                MM3.AXIS = MM3_X;
101
                MM3.STATE = MM3_RESET;         
101
                MM3.STATE = MM3_RESET;         
102
                // Zeitnahe Speicherung der aktuellen Nick-/Rollneigung in °
102
                // Zeitnahe Speicherung der aktuellen Nick-/Rollneigung in °
103
                MM3.NickGrad = IntegralNick/Int2Grad_Faktor;
103
                MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8);
104
                MM3.RollGrad = IntegralRoll/Int2Grad_Faktor;
104
                MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8);
105
                break; 
105
                break; 
106
        }
106
        }
107
}
107
}
Line 108... Line 108...
108
 
108
 
Line 119... Line 119...
119
        cos_roll = (float)cos_f(MM3.RollGrad);      
119
        cos_roll = (float)cos_f(MM3.RollGrad);      
Line 120... Line 120...
120
       
120
       
121
    //Neigungskompensation
121
    //Neigungskompensation
122
        y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
122
        y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
123
    x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis);
-
 
124
 
123
    x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis);
125
        //Winkelberechnung
124
        //Winkelberechnung
Line 126... Line 125...
126
        heading = arctan_f(x_corr, y_corr);
125
        heading = arctan_i(x_corr, y_corr);
127
 
126