Subversion Repositories FlightCtrl

Rev

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

Rev 333 Rev 338
Line 62... Line 62...
62 
        case MM3_WAIT_DRDY:
 62 
        case MM3_WAIT_DRDY:
63 
                if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
 63 
                if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
64 
                return;         // Jetzt gehts weiter in SIGNAL (SIG_SPI)
 64 
                return;         // Jetzt gehts weiter in SIGNAL (SIG_SPI)
Line 65... Line 65...
65 
       
 65 
       
66 
        case MM3_TILT:          // Zeitnahe Speicherung der aktuellen Neigung in °
 66 
        case MM3_TILT:          // Zeitnahe Speicherung der aktuellen Neigung in °
67 
                MM3.NickGrad = asin_i((float)Aktuell_ax/215*200);
 67 
                MM3.NickGrad = asin_i((float)Aktuell_ax/208*200);
Line 68... Line 68...
68 
                MM3.RollGrad = asin_i((float)Aktuell_ay/205*200);
 68 
                MM3.RollGrad = asin_i((float)Aktuell_ay/208*200);
69 
               
 69 
               
70 
                MM3.AXIS = MM3_X;
 70 
                MM3.AXIS = MM3_X;
71 
                MM3.STATE = MM3_RESET;
 71 
                MM3.STATE = MM3_RESET;
Line 101... Line 101...
101 
                SPDR=0x00;              // Übertragung von 2. Byte auslösen
 101 
                SPDR=0x00;              // Übertragung von 2. Byte auslösen
102 
                MM3.STATE=MM3_BYTE2;
 102 
                MM3.STATE=MM3_BYTE2;
103 
                return;
 103 
                return;
Line 104... Line 104...
104 
               
 104 
               
- 
 
 105 
        case MM3_BYTE2:         // 2. Byte der entsprechenden Achse ist da             
- 
 
 106 
       
- 
 
 107 
        // versuch die werte nicht über min und max steigen zu lassen um so die spikes zu verhindern
- 
 
 108 
                // verhindert hats nicht aber deutlich minimiert. Evtl kommen die restlichen
- 
 
 109 
               
105 
        case MM3_BYTE2:         // 2. Byte der entsprechenden Achse ist da             
110 
       
106 
                if (MM3.AXIS == MM3_X)
 111 
                if (MM3.AXIS == MM3_X)
107 
                {
 112 
                {
108 
                        MM3.x_axis |= SPDR;
 113 
                        MM3.x_axis |= SPDR;
- 
 
 114 
                        MM3.x_axis -= OFF_X;    // Sofort Offset aus der Kalibrierung berücksichtigen
- 
 
 115 
                                if (MM3.x_axis > MM3_Xmax || MM3.x_axis < MM3_Xmin)
- 
 
 116 
                                {
- 
 
 117 
                                        MM3.x_axis = MM3.x_axis_last_valid;
- 
 
 118 
                                }              
- 
 
 119 
                                else
- 
 
 120 
                                {
- 
 
 121 
                                 //m = (MM3.x_axis + MM3.x_axis_last_valid) /2;
- 
 
 122 
                                 MM3.x_axis_last_valid = MM3.x_axis;
- 
 
 123 
                                 //MM3.x_axis = m;
- 
 
 124 
                                }
109 
                        MM3.x_axis -= OFF_X;    // Sofort Offset aus der Kalibrierung berücksichtigen
 125 
 
110 
                        MM3.AXIS = MM3_Y;
 126 
                        MM3.AXIS = MM3_Y;
111 
                        MM3.STATE = MM3_RESET;
 127 
                        MM3.STATE = MM3_RESET;
112 
                }
 128 
                }
113 
                else if (MM3.AXIS == MM3_Y)
 129 
                else if (MM3.AXIS == MM3_Y)
114 
                {
 130 
                {
115 
                        MM3.y_axis |= SPDR;            
 131 
                        MM3.y_axis |= SPDR;            
116 
                        MM3.y_axis -= OFF_Y;
 - 
 
117 
                        MM3.AXIS = MM3_Z;
 - 
 
118 
                        MM3.STATE = MM3_RESET;
 - 
 
119 
                }
 - 
 
120 
                else            // if (MM3.AXIS == MM3_Z)
- 
 
121 
                {
 - 
 
122 
                        MM3.z_axis |= SPDR;
 - 
 
123 
                        MM3.z_axis -= OFF_Z;
 - 
 
124 
                        MM3.STATE = MM3_TILT;
 - 
 
125 
                }
 - 
 
126 
                               
 - 
 
127 
                // versuch die werte nicht über min und max steigen zu lassen um so die spikes zu verhindern
 - 
 
128 
                // verhindert hats nicht aber deutlich minimiert. Evtl kommen die restlichen
- 
 
129 
               
 - 
 
130 
                        if (MM3.x_axis > MM3_Xmax || MM3.x_axis < MM3_Xmin)
 - 
 
131 
                        {
 - 
 
132 
                                MM3.x_axis = MM3.x_axis_last_valid;
 - 
 
133 
                        }              
 - 
 
134 
                        else
 - 
 
135 
                        if (MM3.x_axis_last_valid *1,10 < MM3.x_axis || MM3.x_axis_last_valid *0,90 > MM3.x_axis)
 - 
 
136 
                        {
 - 
 
137 
                                MM3.x_axis = MM3.x_axis_last_valid;
 - 
 
138 
                        }
 - 
 
139 
                        else
 - 
 
140 
                        {
 - 
 
141 
                         //m = (MM3.x_axis + MM3.x_axis_last_valid) /2;
 - 
 
142 
                         MM3.x_axis_last_valid = MM3.x_axis;
 - 
 
143 
                         //MM3.x_axis = m;
 - 
 
144 
                        }
 - 
 
145 
 
 - 
 
146 
 
 - 
 
Line 147... Line 132...
147 
 
 132 
                        MM3.y_axis -= OFF_Y;
148 
 
 133 
 
149 
                        if (MM3.y_axis > MM3_Ymax || MM3.y_axis < MM3_Ymin)
 134 
                        if (MM3.y_axis > MM3_Ymax || MM3.y_axis < MM3_Ymin)
150 
                        {
 135 
                        {
151 
                                MM3.y_axis = MM3.y_axis_last_valid;
 136 
                                MM3.y_axis = MM3.y_axis_last_valid;
152 
                        }              
 - 
 
153 
                        else
 - 
 
154 
                        if (MM3.y_axis_last_valid *1,10 < MM3.y_axis || MM3.y_axis_last_valid *0,90 > MM3.y_axis)
 - 
 
155 
                        {
 - 
 
156 
                                MM3.y_axis = MM3.y_axis_last_valid;
 - 
 
157 
                        }
 137 
                        }              
158 
                        else
 138 
                        else
159 
                        {
 139 
                        {
160 
                         //m = (MM3.y_axis + MM3.y_axis_last_valid) /2;
 140 
                         //m = (MM3.y_axis + MM3.y_axis_last_valid) /2;
Line 161... Line 141...
161 
                         MM3.y_axis_last_valid = MM3.y_axis;
 141 
                         MM3.y_axis_last_valid = MM3.y_axis;
Line -... Line 142...
- 
 
 142 
                         //MM3.y_axis = m;
- 
 
 143 
                         
- 
 
 144 
                        }
- 
 
 145 
 
162 
                         //MM3.y_axis = m;
 146 
                        MM3.AXIS = MM3_Z;
- 
 
 147 
                        MM3.STATE = MM3_RESET;
- 
 
 148 
                }
Line 163... Line 149...
163 
                         
 149 
                else            // if (MM3.AXIS == MM3_Z)
164 
                        }
 150 
                {
165 
 
 151 
                        MM3.z_axis |= SPDR;
166 
 
 152 
                        MM3.z_axis -= OFF_Z;
167 
 
 153 
 
168 
                        if (MM3.z_axis > MM3_Zmax || MM3.z_axis < MM3_Zmin)
 - 
 
169 
                        {
 - 
 
170 
                                MM3.z_axis = MM3.z_axis_last_valid;
 - 
 
171 
                        }              
 - 
 
172 
                        else
 - 
 
173 
                        if (MM3.z_axis_last_valid *1,10 < MM3.z_axis || MM3.z_axis_last_valid *0,90 > MM3.z_axis)
 154 
                        if (MM3.z_axis > MM3_Zmax || MM3.z_axis < MM3_Zmin)
174 
                        {
 155 
                        {
175 
                                MM3.z_axis = MM3.z_axis_last_valid;
 156 
                                MM3.z_axis = MM3.z_axis_last_valid;
176 
                        }      
 157 
                        }              
177 
                        else
 158 
                        else
178 
                        {
 - 
 
179 
                                //m = (MM3.z_axis + MM3.z_axis_last_valid) /2;
 - 
 
Line 180... Line -...
180 
                                MM3.z_axis_last_valid = MM3.z_axis;
 - 
 
181 
                                //MM3.z_axis = m;
 - 
 
- 
 
 159 
                        {
182 
                        }
 160 
                                //m = (MM3.z_axis + MM3.z_axis_last_valid) /2;
183 
                       
 161 
                                MM3.z_axis_last_valid = MM3.z_axis;
184 
                       
 162 
                                //MM3.z_axis = m;
185 
 
 163 
                        }
Line 186... Line 164...
186 
 
 164