Subversion Repositories MK3Mag

Rev

Rev 30 | 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;
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
 
163
        // blink code for normal operation
164
        // blink code for normal operation
164
        if(CheckDelay(Led_Timer))
165
        if(CheckDelay(Led_Timer))
165
        {
166
        {
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
        {
189
                case ATTITUDE_SOURCE_I2C:
190
                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;
191
                        roll_rad = ((double)I2C_WriteAttitude.Roll) * M_PI / (double)(1800.0);
193
                        roll = I2C_WriteAttitude.Roll;
-
 
194
           sei(); // start interrupts
192
                        break;
195
                        break;
193
 
-
 
194
                case ATTITUDE_SOURCE_UART:
196
                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];
196
                        roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0);
199
            roll = ExternData.Attitude[ROLL];
-
 
200
           sei(); // start interrupts
197
                        break;
201
                        break;
198
 
-
 
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;
205
                        break;
203
 
-
 
204
                default:
206
                default:
205
                        nick_rad = 0;
207
                        nick_rad = 0;
206
                        roll_rad = 0;
208
                        roll_rad = 0;
207
                break;
209
                break;
208
        }
210
        }
-
 
211
    nick_rad = ((double)nick) * M_PI / (double)(1800.0);
-
 
212
    roll_rad = ((double)roll) * M_PI / (double)(1800.0);
209
 
213
 
210
        // calculate attitude correction
214
        // calculate attitude correction
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);
213
 
217
 
214
        DebugOut.Analog[27] = (int16_t)Hx;
218
        DebugOut.Analog[27] = (int16_t)Hx;
215
        DebugOut.Analog[28] = (int16_t)Hy;
219
        DebugOut.Analog[28] = (int16_t)Hy;
216
 
220
 
217
 
-
 
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);
220
        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
223
        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
221
        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
224
        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
222
        if (heading < 0) heading = -heading;
225
        if (heading < 0) heading = -heading;
223
        else heading = 360 - heading;
226
        else heading = 360 - heading;
224
 
227
 
-
 
228
   cli(); // stop interrupts
225
        if(abs(heading) < 361) Heading = heading;
229
        if(abs(heading) < 361) Heading = heading;
226
        else (Heading = -1);
230
        else (Heading = -1);
-
 
231
   sei(); // start interrupts
-
 
232
   
227
}
233
}
228
 
234
 
229
 
235
 
230
void Calibrate(void)
236
void Calibrate(void)
231
{
237
{
232
        uint8_t cal;
238
        uint8_t cal;
233
        static uint8_t calold = 0;
239
        static uint8_t calold = 0;
234
        static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
240
        static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
235
        static uint8_t blinkcount = 0;
241
        static uint8_t blinkcount = 0;
-
 
242
        static uint8_t invert_blinking = 0;
236
 
243
   
237
        // check both sources of communication for calibration request
244
        // check both sources of communication for calibration request
238
        if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
245
        if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
239
        else                     cal = ExternData.CalState;
246
        else                     cal = ExternData.CalState;
240
 
247
 
241
 
248
 
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;
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
                        }
257
                        else
264
                        else
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
        }
268
 
275
 
269
        // calibration state machine
276
        // calibration state machine
270
        switch(cal)
277
        switch(cal)
271
        {
278
        {
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;
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;
291
                        if(UncalMagY > Ymax) Ymax = UncalMagY;
299
                        if(UncalMagY > Ymax) Ymax = UncalMagY;
-
 
300
            invert_blinking = 1;
292
                        break;
301
                        break;
293
 
302
 
294
                case 3: // 3rd step of calibration
303
                case 3: // 3rd step of calibration
295
                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
304
                        // 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:
299
                        // find Min and Max of the Z-Sensor
309
                        // find Min and Max of the Z-Sensor
300
                        if(UncalMagZ < Zmin) Zmin = UncalMagZ;
310
                        if(UncalMagZ < Zmin) Zmin = UncalMagZ;
301
                        if(UncalMagZ > Zmax) Zmax = UncalMagZ;
311
                        if(UncalMagZ > Zmax) Zmax = UncalMagZ;
-
 
312
            invert_blinking = 1;
302
                        break;
313
                        break;
303
 
314
 
304
                case 5:
315
                case 5:
305
                        // Save values
316
                        // Save values
306
                        if(cal != calold) // avoid continously writing of eeprom!
317
                        if(cal != calold) // avoid continously writing of eeprom!
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;
325
 
337
 
326
                default:
338
                default:
327
                        break;
339
                        break;
328
        }
340
        }