Subversion Repositories FlightCtrl

Rev

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

Rev 343 Rev 349
Line 66... Line 66...
66
                return;
66
                return;
Line 67... Line 67...
67
       
67
       
68
        case MM3_WAIT_DRDY:
68
        case MM3_WAIT_DRDY:
69
                if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
69
                if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
70
                return;                                                 // Jetzt gehts weiter in SIGNAL (SIG_SPI)
70
                return;                                                 // Jetzt gehts weiter in SIGNAL (SIG_SPI)
71
       
71
        /*
72
        case MM3_TILT:                                          // Zeitnahe Speicherung der aktuellen Neigung in °
72
        case MM3_TILT:                                          // Zeitnahe Speicherung der aktuellen Neigung in °
73
                MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8);
73
                MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8);
Line 74... Line 74...
74
                MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8);
74
                MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8);
75
               
75
               
76
                MM3.AXIS = MM3_X;
76
                MM3.AXIS = MM3_X;
-
 
77
                MM3.STATE = MM3_RESET;
77
                MM3.STATE = MM3_RESET;
78
                return;
78
                return;
79
        */
Line 79... Line 80...
79
        }
80
        }
Line 129... Line 130...
129
                else    // if (MM3.AXIS == MM3_Z) 
130
                else    // if (MM3.AXIS == MM3_Z) 
130
                {
131
                {
131
                        MM3.z_axis |= SPDR;
132
                        MM3.z_axis |= SPDR;
132
                        if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis;
133
                        if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis;
133
                        else MM3.z_axis = MM3.z_axis_old;
134
                        else MM3.z_axis = MM3.z_axis_old;
-
 
135
                        MM3.AXIS = MM3_X;
134
                        MM3.STATE = MM3_TILT;
136
                        MM3.STATE = MM3_RESET;
135
                }
137
                }
Line 136... Line 138...
136
               
138
               
137
                return;
139
                return;
138
        }
140
        }
Line 194... Line 196...
194
//############################################################################
196
//############################################################################
195
{
197
{
196
        float sin_nick, cos_nick, sin_roll, cos_roll;
198
        float sin_nick, cos_nick, sin_roll, cos_roll;
197
        signed int x_corr, y_corr, heading;
199
        signed int x_corr, y_corr, heading;
198
        signed int x_axis,y_axis,z_axis;
200
        signed int x_axis,y_axis,z_axis;
199
               
201
       
-
 
202
        MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8);
-
 
203
        MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8);
-
 
204
       
200
        // Berechung von sinus und cosinus
205
        // Berechung von sinus und cosinus
201
        sin_nick = sin_f(MM3.NickGrad);
206
        sin_nick = sin_f(MM3.NickGrad);
202
        cos_nick = cos_f(MM3.NickGrad);
207
        cos_nick = cos_f(MM3.NickGrad);
203
        sin_roll = sin_f(MM3.RollGrad);
208
        sin_roll = sin_f(MM3.RollGrad);
204
        cos_roll = cos_f(MM3.RollGrad);
209
        cos_roll = cos_f(MM3.RollGrad);