Subversion Repositories FlightCtrl

Rev

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

Rev 308 Rev 309
Line 61... Line 61...
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
63
                return;         // Jetzt gehts weiter in SIGNAL (SIG_SPI)
63
                return;         // Jetzt gehts weiter in SIGNAL (SIG_SPI)
Line 64... Line 64...
64
       
64
       
65
        case MM3_TILT:          // Zeitnahe Speicherung der aktuellen Neigung in °
65
        case MM3_TILT:          // Zeitnahe Speicherung der aktuellen Neigung in °
66
                MM3.NickGrad = asin_i((float)Aktuell_ax/EE_Parameter.UserParam1*200);
66
                MM3.NickGrad = asin_i((float)Aktuell_ax/220*200);
Line 67... Line 67...
67
                MM3.RollGrad = asin_i((float)Aktuell_ay/EE_Parameter.UserParam2*200);
67
                MM3.RollGrad = asin_i((float)Aktuell_ay/220*200);
68
               
68
               
69
                MM3.AXIS = MM3_X;
69
                MM3.AXIS = MM3_X;
70
                MM3.STATE = MM3_RESET;
70
                MM3.STATE = MM3_RESET;
Line 138... Line 138...
138
        cos_nick = cos_f(MM3.NickGrad);
138
        cos_nick = cos_f(MM3.NickGrad);
139
        sin_roll = sin_f(MM3.RollGrad);
139
        sin_roll = sin_f(MM3.RollGrad);
140
        cos_roll = cos_f(MM3.RollGrad);      
140
        cos_roll = cos_f(MM3.RollGrad);      
Line 141... Line 141...
141
       
141
       
-
 
142
    // Neigungskompensation
-
 
143
        //x_corr = (cos_nick * MM3.x_axis) + (((sin_roll *  MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick);
142
    // Neigungskompensation
144
       
143
        x_corr = (cos_nick * MM3.x_axis) + (((sin_roll *  MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick);
145
        x_corr = (cos_nick * MM3.x_axis) - (((sin_roll *  MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick);
Line 144... Line 146...
144
        y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
146
        y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
145
       
147