Subversion Repositories MK3Mag

Rev

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

Rev 30 Rev 31
Line 156... Line 156...
156
 
156
 
157
 
157
 
158
void CalcHeading(void)
158
void CalcHeading(void)
-
 
159
{
159
{
160
        double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
Line 160... Line 161...
160
        double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
161
    int16_t nick, roll;
161
        int16_t heading = -1;
162
        int16_t heading = -1;
162
 
163
 
Line 185... Line 186...
185
 
186
 
186
        // calculate nick and roll angle in rad
187
        // calculate nick and roll angle in rad
187
        switch(AttitudeSource)
188
        switch(AttitudeSource)
188
        {
189
        {
-
 
190
                case ATTITUDE_SOURCE_I2C:
189
                case ATTITUDE_SOURCE_I2C:
191
           cli(); // stop interrupts
190
                        nick_rad = ((double)I2C_WriteAttitude.Nick) * M_PI / (double)(1800.0);
192
            nick = I2C_WriteAttitude.Nick;
-
 
193
                        roll = I2C_WriteAttitude.Roll;
191
                        roll_rad = ((double)I2C_WriteAttitude.Roll) * M_PI / (double)(1800.0);
194
           sei(); // start interrupts
192
                        break;
-
 
193
 
195
                        break;
-
 
196
                case ATTITUDE_SOURCE_UART:
194
                case ATTITUDE_SOURCE_UART:
197
           cli(); // stop interrupts
195
                        nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0);
198
            nick = ExternData.Attitude[NICK];
-
 
199
            roll = ExternData.Attitude[ROLL];
196
                        roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0);
200
           sei(); // start interrupts
197
                        break;
-
 
198
 
201
                        break;
199
                case ATTITUDE_SOURCE_ACC:
202
                case ATTITUDE_SOURCE_ACC:
200
                        nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0);
203
            nick = AccAttitudeNick;
201
                        roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0);
204
            roll = AccAttitudeRoll;
202
                        break;
-
 
203
 
205
                        break;
204
                default:
206
                default:
205
                        nick_rad = 0;
207
                        nick_rad = 0;
206
                        roll_rad = 0;
208
                        roll_rad = 0;
207
                break;
209
                break;
-
 
210
        }
-
 
211
    nick_rad = ((double)nick) * M_PI / (double)(1800.0);
Line 208... Line 212...
208
        }
212
    roll_rad = ((double)roll) * M_PI / (double)(1800.0);
209
 
213
 
210
        // calculate attitude correction
214
        // calculate attitude correction
Line 211... Line 215...
211
        Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
215
        Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
212
        Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
216
        Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
Line 213... Line -...
213
 
-
 
214
        DebugOut.Analog[27] = (int16_t)Hx;
217
 
215
        DebugOut.Analog[28] = (int16_t)Hy;
218
        DebugOut.Analog[27] = (int16_t)Hx;
216
 
219
        DebugOut.Analog[28] = (int16_t)Hy;
217
 
220
 
218
        // calculate Heading
221
        // calculate Heading
219
        heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
222
        heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
Line -... Line 223...
-
 
223
        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
220
        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
224
        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
221
        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
225
        if (heading < 0) heading = -heading;
-
 
226
        else heading = 360 - heading;
-
 
227
 
222
        if (heading < 0) heading = -heading;
228
   cli(); // stop interrupts
Line 223... Line 229...
223
        else heading = 360 - heading;
229
        if(abs(heading) < 361) Heading = heading;
224
 
230
        else (Heading = -1);
225
        if(abs(heading) < 361) Heading = heading;
231
   sei(); // start interrupts
226
        else (Heading = -1);
232
   
227
}
233
}
228
 
234
 
-
 
235
 
229
 
236
void Calibrate(void)
230
void Calibrate(void)
237
{
231
{
238
        uint8_t cal;
232
        uint8_t cal;
239
        static uint8_t calold = 0;
Line 243... Line 250...
243
        // blink code for current calibration state
250
        // blink code for current calibration state
244
        if(cal)
251
        if(cal)
245
        {
252
        {
246
                if(CheckDelay(Led_Timer) || (cal != calold))
253
                if(CheckDelay(Led_Timer) || (cal != calold))
247
                {
254
                {
248
                        if(blinkcount & 0x01) LED_GRN_OFF;
255
                        if(blinkcount & 0x01) if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
249
                        else LED_GRN_ON;
256
                        else if(invert_blinking) LED_GRN_OFF; else LED_GRN_ON;
Line 250... Line 257...
250
 
257
 
251
                        // end of blinkcount sequence
258
                        // end of blinkcount sequence
252
                        if( (blinkcount + 1 ) >= (2 * cal) )
259
                        if((blinkcount + 1 ) >= (2 * cal))
253
                        {
260
                        {
254
                                blinkcount = 0;
261
                                blinkcount = 0;
255
                                Led_Timer = SetDelay(1000);
262
                                Led_Timer = SetDelay(1000);
256
                        }
263
                        }
Line 261... Line 268...
261
                        }
268
                        }
262
                }
269
                }
263
        }
270
        }
264
        else
271
        else
265
        {
272
        {
266
                LED_GRN_OFF;
273
                if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
267
        }
274
        }
Line 268... Line 275...
268
 
275
 
269
        // calibration state machine
276
        // calibration state machine
270
        switch(cal)
277
        switch(cal)
Line 279... Line 286...
279
                        Zmin =  10000;
286
                        Zmin =  10000;
280
                        Zmax = -10000;
287
                        Zmax = -10000;
281
                        Calibration.AccX.Offset = RawAccX;
288
                        Calibration.AccX.Offset = RawAccX;
282
                        Calibration.AccY.Offset = RawAccY;
289
                        Calibration.AccY.Offset = RawAccY;
283
                Calibration.AccZ.Offset = RawAccZ;
290
                Calibration.AccZ.Offset = RawAccZ;
-
 
291
            invert_blinking = 0;
284
                        break;
292
                        break;
Line 285... Line 293...
285
 
293
 
286
                case 2: // 2nd step of calibration
294
                case 2: // 2nd step of calibration
287
                        // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
295
                        // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
288
                        if(UncalMagX < Xmin) Xmin = UncalMagX;
296
                        if(UncalMagX < Xmin) Xmin = UncalMagX;
289
                        if(UncalMagX > Xmax) Xmax = UncalMagX;
297
                        if(UncalMagX > Xmax) Xmax = UncalMagX;
290
                        if(UncalMagY < Ymin) Ymin = UncalMagY;
298
                        if(UncalMagY < Ymin) Ymin = UncalMagY;
-
 
299
                        if(UncalMagY > Ymax) Ymax = UncalMagY;
291
                        if(UncalMagY > Ymax) Ymax = UncalMagY;
300
            invert_blinking = 1;
Line 292... Line 301...
292
                        break;
301
                        break;
293
 
302
 
-
 
303
                case 3: // 3rd step of calibration
294
                case 3: // 3rd step of calibration
304
                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
Line 295... Line 305...
295
                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
305
            invert_blinking = 0;
296
                        break;
306
                        break;
297
 
307
 
298
                case 4:
308
                case 4:
-
 
309
                        // find Min and Max of the Z-Sensor
299
                        // find Min and Max of the Z-Sensor
310
                        if(UncalMagZ < Zmin) Zmin = UncalMagZ;
Line 300... Line 311...
300
                        if(UncalMagZ < Zmin) Zmin = UncalMagZ;
311
                        if(UncalMagZ > Zmax) Zmax = UncalMagZ;
301
                        if(UncalMagZ > Zmax) Zmax = UncalMagZ;
312
            invert_blinking = 1;
302
                        break;
313
                        break;
Line 319... Line 330...
319
                                        Led_Timer = SetDelay(2000);
330
                                        Led_Timer = SetDelay(2000);
320
                                        // reset  blinkcode
331
                                        // reset  blinkcode
321
                                        blinkcount = 0;
332
                                        blinkcount = 0;
322
                                }
333
                                }
323
                        }
334
                        }
-
 
335
            invert_blinking = 0;
324
                        break;
336
                        break;
Line 325... Line 337...
325
 
337
 
326
                default:
338
                default:
327
                        break;
339
                        break;