Subversion Repositories NaviCtrl

Rev

Rev 339 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
242 killagreg 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Copyright (c) 2010 Ingo Busker, Holger Buss
6
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
7
// + FOR NON COMMERCIAL USE ONLY
8
// + www.MikroKopter.com
9
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13
// + bzgl. der Nutzungsbedingungen aufzunehmen.
14
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15
// + Verkauf von Luftbildaufnahmen, usw.
16
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22
// + eindeutig als Ursprung verlinkt werden
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25
// + Benutzung auf eigene Gefahr
26
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
29
// + mit unserer Zustimmung zulässig
30
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
32
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
34
// + this list of conditions and the following disclaimer.
35
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
36
// +     from this software without specific prior written permission.
37
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
38
// +     for non-commercial use (directly or indirectly)
39
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
40
// +     with our written permission
41
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
42
// +     clearly linked as origin
43
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
44
//
45
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
53
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
54
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
55
// +  POSSIBILITY OF SUCH DAMAGE.
56
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
254 killagreg 57
#include <math.h>
292 killagreg 58
#include <stdio.h>
242 killagreg 59
#include <string.h>
60
#include "91x_lib.h"
253 killagreg 61
#include "ncmag.h"
242 killagreg 62
#include "i2c.h"
63
#include "timer1.h"
64
#include "led.h"
65
#include "uart1.h"
254 killagreg 66
#include "eeprom.h"
256 killagreg 67
#include "mymath.h"
292 killagreg 68
#include "main.h"
242 killagreg 69
 
253 killagreg 70
u8 NCMAG_Present = 0;
254 killagreg 71
u8 NCMAG_IsCalibrated = 0;
242 killagreg 72
 
253 killagreg 73
#define MAG_TYPE_NONE           0
74
#define MAG_TYPE_HMC5843        1
75
#define MAG_TYPE_LSM303DLH      2
254 killagreg 76
u8 NCMAG_MagType = MAG_TYPE_NONE;
242 killagreg 77
 
338 holgerb 78
#define CALIBRATION_VERSION                     1
79
#define EEPROM_ADR_MAG_CALIBRATION              50
339 holgerb 80
#define MAG_CALIBRATION_COMPATIBEL              0xA2
254 killagreg 81
 
256 killagreg 82
#define NCMAG_MIN_RAWVALUE -2047
83
#define NCMAG_MAX_RAWVALUE  2047
84
#define NCMAG_INVALID_DATA -4096
85
 
254 killagreg 86
typedef struct
87
{
88
        s16 Range;
89
        s16 Offset;
256 killagreg 90
} __attribute__((packed)) Scaling_t;
254 killagreg 91
 
92
typedef struct
93
{
94
        Scaling_t MagX;
95
        Scaling_t MagY;
96
        Scaling_t MagZ;
97
        u8 Version;
98
        u8 crc;
256 killagreg 99
} __attribute__((packed)) Calibration_t;
254 killagreg 100
 
101
Calibration_t Calibration;              // calibration data in RAM 
339 holgerb 102
volatile s16vec_t AccRawVector;
103
volatile s16vec_t MagRawVector;
254 killagreg 104
 
253 killagreg 105
// i2c MAG interface
106
#define MAG_SLAVE_ADDRESS       0x3C    // i2C slave address mag. sensor registers
242 killagreg 107
 
253 killagreg 108
// register mapping
109
#define REG_MAG_CRA                     0x00
110
#define REG_MAG_CRB                     0x01
111
#define REG_MAG_MODE            0x02
112
#define REG_MAG_DATAX_MSB       0x03
113
#define REG_MAG_DATAX_LSB       0x04
114
#define REG_MAG_DATAY_MSB       0x05
115
#define REG_MAG_DATAY_LSB       0x06
116
#define REG_MAG_DATAZ_MSB       0x07
117
#define REG_MAG_DATAZ_LSB       0x08
118
#define REG_MAG_STATUS          0x09
329 holgerb 119
 
253 killagreg 120
#define REG_MAG_IDA                     0x0A
121
#define REG_MAG_IDB                     0x0B
122
#define REG_MAG_IDC                     0x0C
329 holgerb 123
#define REG_MAG_IDF                     0x0F
242 killagreg 124
 
253 killagreg 125
// bit mask for configuration mode
126
#define CRA_MODE_MASK           0x03
127
#define CRA_MODE_NORMAL         0x00    //default
128
#define CRA_MODE_POSBIAS        0x01
129
#define CRA_MODE_NEGBIAS        0x02
130
#define CRA_MODE_SELFTEST       0x03
242 killagreg 131
 
253 killagreg 132
// bit mask for measurement mode
133
#define MODE_MASK                       0xFF
134
#define MODE_CONTINUOUS         0x00
135
#define MODE_SINGLE                     0x01    // default
136
#define MODE_IDLE                       0x02
137
#define MODE_SLEEP                      0x03
138
 
242 killagreg 139
// bit mask for rate
253 killagreg 140
#define CRA_RATE_MASK           0x1C
141
 
142
// bit mask for gain
143
#define CRB_GAIN_MASK           0xE0
144
 
145
// ids
146
#define MAG_IDA         0x48
147
#define MAG_IDB         0x34
148
#define MAG_IDC         0x33
149
 
150
// the special HMC5843 interface
151
// bit mask for rate
242 killagreg 152
#define HMC5843_CRA_RATE_0_5HZ          0x00
153
#define HMC5843_CRA_RATE_1HZ            0x04
154
#define HMC5843_CRA_RATE_2HZ            0x08
155
#define HMC5843_CRA_RATE_5HZ            0x0C
156
#define HMC5843_CRA_RATE_10HZ           0x10    //default
157
#define HMC5843_CRA_RATE_20HZ           0x14
158
#define HMC5843_CRA_RATE_50HZ           0x18
159
// bit mask for gain
160
#define HMC5843_CRB_GAIN_07GA           0x00
161
#define HMC5843_CRB_GAIN_10GA           0x20    //default
339 holgerb 162
#define HMC5843_CRB_GAIN_15GA           0x40    // <--- we use this     
242 killagreg 163
#define HMC5843_CRB_GAIN_20GA           0x60
164
#define HMC5843_CRB_GAIN_32GA           0x80
165
#define HMC5843_CRB_GAIN_38GA           0xA0
166
#define HMC5843_CRB_GAIN_45GA           0xC0
167
#define HMC5843_CRB_GAIN_65GA           0xE0
253 killagreg 168
// self test value
339 holgerb 169
#define HMC5843_TEST_XSCALE             555
170
#define HMC5843_TEST_YSCALE             555
171
#define HMC5843_TEST_ZSCALE             555
172
// clibration range
173
#define HMC5843_CALIBRATION_RANGE   550
242 killagreg 174
 
253 killagreg 175
// the special LSM302DLH interface
176
// bit mask for rate
177
#define LSM303DLH_CRA_RATE_0_75HZ       0x00
178
#define LSM303DLH_CRA_RATE_1_5HZ        0x04
179
#define LSM303DLH_CRA_RATE_3_0HZ        0x08
180
#define LSM303DLH_CRA_RATE_7_5HZ        0x0C
181
#define LSM303DLH_CRA_RATE_15HZ         0x10    //default
182
#define LSM303DLH_CRA_RATE_30HZ         0x14
183
#define LSM303DLH_CRA_RATE_75HZ         0x18
338 holgerb 184
 
253 killagreg 185
// bit mask for gain
186
#define LSM303DLH_CRB_GAIN_XXGA         0x00
187
#define LSM303DLH_CRB_GAIN_13GA         0x20    //default
339 holgerb 188
#define LSM303DLH_CRB_GAIN_19GA         0x40    // <--- we use this
253 killagreg 189
#define LSM303DLH_CRB_GAIN_25GA         0x60
190
#define LSM303DLH_CRB_GAIN_40GA         0x80
191
#define LSM303DLH_CRB_GAIN_47GA         0xA0
192
#define LSM303DLH_CRB_GAIN_56GA         0xC0
193
#define LSM303DLH_CRB_GAIN_81GA         0xE0
194
// self test value
338 holgerb 195
#define LSM303DLH_TEST_XSCALE   495
196
#define LSM303DLH_TEST_YSCALE   495
197
#define LSM303DLH_TEST_ZSCALE   470
339 holgerb 198
// clibration range
199
#define LSM303_CALIBRATION_RANGE   500
253 killagreg 200
 
201
// the i2c ACC interface
202
#define ACC_SLAVE_ADDRESS               0x30    // i2c slave for acc. sensor registers
203
// register mapping
204
#define REG_ACC_CTRL1                   0x20
205
#define REG_ACC_CTRL2                   0x21
206
#define REG_ACC_CTRL3                   0x22
207
#define REG_ACC_CTRL4                   0x23
208
#define REG_ACC_CTRL5                   0x24
209
#define REG_ACC_HP_FILTER_RESET 0x25
210
#define REG_ACC_REFERENCE               0x26
211
#define REG_ACC_STATUS                  0x27
212
#define REG_ACC_X_LSB                   0x28
213
#define REG_ACC_X_MSB                   0x29
214
#define REG_ACC_Y_LSB                   0x2A
215
#define REG_ACC_Y_MSB                   0x2B
216
#define REG_ACC_Z_LSB                   0x2C
217
#define REG_ACC_Z_MSB                   0x2D
218
 
219
 
220
 
242 killagreg 221
typedef struct
222
{
253 killagreg 223
        u8 A;
224
        u8 B;
225
        u8 C;
226
} __attribute__((packed)) Identification_t;
227
volatile Identification_t NCMAG_Identification;
242 killagreg 228
 
253 killagreg 229
typedef struct
230
{
329 holgerb 231
        u8 Sub;
232
} __attribute__((packed)) Identification2_t;
233
volatile Identification2_t NCMAG_Identification2;
234
 
235
typedef struct
236
{
253 killagreg 237
        u8 cra;
238
        u8 crb;
239
        u8 mode;
240
} __attribute__((packed)) MagConfig_t;
242 killagreg 241
 
253 killagreg 242
volatile MagConfig_t MagConfig;
242 killagreg 243
 
253 killagreg 244
typedef struct
245
{
246
        u8 ctrl_1;
247
        u8 ctrl_2;
248
        u8 ctrl_3;
249
        u8 ctrl_4;
250
        u8 ctrl_5;
251
} __attribute__((packed)) AccConfig_t;
252
 
253
volatile AccConfig_t AccConfig;
254
 
254 killagreg 255
u8 NCMag_CalibrationWrite(void)
256
{
338 holgerb 257
        u8 i, crc = MAG_CALIBRATION_COMPATIBEL;
254 killagreg 258
        EEPROM_Result_t eres;
259
        u8 *pBuff = (u8*)&Calibration;
260
 
261
        Calibration.Version = CALIBRATION_VERSION;
256 killagreg 262
        for(i = 0; i<(sizeof(Calibration)-1); i++)
254 killagreg 263
        {
264
                crc += pBuff[i];        
265
        }
266
        Calibration.crc = ~crc;
267
        eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration));
268
        if(EEPROM_SUCCESS == eres) i = 1;
269
        else i = 0;
270
        return(i);     
271
}
272
 
273
u8 NCMag_CalibrationRead(void)
274
{
338 holgerb 275
        u8 i, crc = MAG_CALIBRATION_COMPATIBEL;
254 killagreg 276
        u8 *pBuff = (u8*)&Calibration;
277
 
278
        if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)))
279
        {
256 killagreg 280
                for(i = 0; i<(sizeof(Calibration)-1); i++)
254 killagreg 281
                {
282
                        crc += pBuff[i];        
283
                }
284
                crc = ~crc;
285
                if(Calibration.crc != crc) return(0); // crc mismatch
257 killagreg 286
                if(Calibration.Version == CALIBRATION_VERSION) return(1);
254 killagreg 287
        }
288
        return(0);
289
}
290
 
291
 
292
void NCMAG_Calibrate(void)
293
{
330 holgerb 294
        u8 msg[64];
254 killagreg 295
        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
256 killagreg 296
        static s16 X = 0, Y = 0, Z = 0;
254 killagreg 297
        static u8 OldCalState = 0;     
339 holgerb 298
        s16 MinCaclibration = 450;
254 killagreg 299
 
256 killagreg 300
        X = (4*X + MagRawVector.X + 3)/5;
301
        Y = (4*Y + MagRawVector.Y + 3)/5;
302
        Z = (4*Z + MagRawVector.Z + 3)/5;
303
 
254 killagreg 304
        switch(Compass_CalState)
305
        {
306
                case 1:
307
                        // 1st step of calibration
308
                        // initialize ranges
309
                        // used to change the orientation of the NC in the horizontal plane
310
                        Xmin =  10000;
311
                        Xmax = -10000;
312
                        Ymin =  10000;
313
                        Ymax = -10000;
314
                        Zmin =  10000;
315
                        Zmax = -10000;
316
                        break;
317
 
318
                case 2: // 2nd step of calibration
319
                        // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
275 killagreg 320
                        if(X < Xmin)            { Xmin = X; BeepTime = 20;}
321
                        else if(X > Xmax)       { Xmax = X; BeepTime = 20;}
322
                        if(Y < Ymin)            { Ymin = Y; BeepTime = 60;}
323
                        else if(Y > Ymax)       { Ymax = Y; BeepTime = 60;}
254 killagreg 324
                        break;
325
 
326
                case 3: // 3rd step of calibration
327
                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
328
                        break;
329
 
330
                case 4:
331
                        // find Min and Max of the Z-Sensor
275 killagreg 332
                        if(Z < Zmin)      { Zmin = Z; BeepTime = 80;}
333
                        else if(Z > Zmax) { Zmax = Z; BeepTime = 80;}
254 killagreg 334
                        break;
335
 
336
                case 5:
337
                        // Save values
338
                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
339
                        {
338 holgerb 340
//                              #define MIN_CALIBRATION    256
339 holgerb 341
                                if(NCMAG_MagType == MAG_TYPE_HMC5843)
342
                                 {
343
                                  UART1_PutString("\r\nHMC5843 calibration\n\r");
344
                                  MinCaclibration = HMC5843_CALIBRATION_RANGE;
345
                                 }
346
                                if(NCMAG_MagType == MAG_TYPE_LSM303DLH)
347
                                 {
348
                                  UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
349
                                  MinCaclibration =LSM303_CALIBRATION_RANGE;
350
                                 }
351
 
254 killagreg 352
                                Calibration.MagX.Range = Xmax - Xmin;
353
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
354
                                Calibration.MagY.Range = Ymax - Ymin;
355
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
356
                                Calibration.MagZ.Range = Zmax - Zmin;
357
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
339 holgerb 358
                                if((Calibration.MagX.Range > MinCaclibration) && (Calibration.MagY.Range > MinCaclibration) && (Calibration.MagZ.Range > MinCaclibration))
254 killagreg 359
                                {
360
                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite();
270 killagreg 361
                                        BeepTime = 2500;
330 holgerb 362
                                        UART1_PutString("\r\n Calibration okay\n\r");
254 killagreg 363
                                }
364
                                else
365
                                {
339 holgerb 366
                                        UART1_PutString("\r\nCalibration FAILED - Values too low: ");
367
                                    if(Calibration.MagX.Range < MinCaclibration) UART1_PutString("X! ");
368
                                    if(Calibration.MagY.Range < MinCaclibration) UART1_PutString("y! ");
369
                                    if(Calibration.MagZ.Range < MinCaclibration) UART1_PutString("Z! ");
330 holgerb 370
                                        UART1_PutString("\r\n");
339 holgerb 371
                                        sprintf(msg, "Minimum is: %i \r\n",MinCaclibration);
372
                                        UART1_PutString(msg);
373
 
254 killagreg 374
                                        // restore old calibration data from eeprom
375
                                        NCMAG_IsCalibrated = NCMag_CalibrationRead();
376
                                }
330 holgerb 377
                                        sprintf(msg, "X: (%i - %i = %i)\r\n",Xmax,Xmin,Xmax - Xmin);
378
                                        UART1_PutString(msg);
379
                                        sprintf(msg, "Y: (%i - %i = %i)\r\n",Ymax,Ymin,Ymax - Ymin);
380
                                        UART1_PutString(msg);
381
                                        sprintf(msg, "Z: (%i - %i = %i)\r\n",Zmax,Zmin,Zmax - Zmin);
382
                                        UART1_PutString(msg);
254 killagreg 383
                        }
384
                        break;
385
 
386
                default:
387
                        break; 
388
        }
389
        OldCalState = Compass_CalState;
390
}
391
 
242 killagreg 392
// ---------- call back handlers -----------------------------------------
393
 
394
// rx data handler for id info request
253 killagreg 395
void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
254 killagreg 396
{       // if number of bytes are matching
253 killagreg 397
        if(RxBufferSize == sizeof(NCMAG_Identification) )
242 killagreg 398
        {
253 killagreg 399
                memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
400
        }
242 killagreg 401
}
329 holgerb 402
 
403
void NCMAG_UpdateIdentification_Sub(u8* pRxBuffer, u8 RxBufferSize)
404
{       // if number of bytes are matching
405
        if(RxBufferSize == sizeof(NCMAG_Identification2))
406
        {
407
                memcpy((u8 *)&NCMAG_Identification2, pRxBuffer, sizeof(NCMAG_Identification2));
408
        }
409
}
410
 
254 killagreg 411
// rx data handler for magnetic sensor raw data
253 killagreg 412
void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
254 killagreg 413
{       // if number of bytes are matching
414
        if(RxBufferSize == sizeof(MagRawVector) )
243 killagreg 415
        {       // byte order from big to little endian
256 killagreg 416
                s16 raw;
417
                raw = pRxBuffer[0]<<8;
418
                raw+= pRxBuffer[1];
419
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw;
420
                raw = pRxBuffer[2]<<8;
421
                raw+= pRxBuffer[3];
330 holgerb 422
            if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
423
                {
424
                  if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Z = raw; // here Z and Y are exchanged
425
                  else MagRawVector.Y = raw;
426
                }
256 killagreg 427
                raw = pRxBuffer[4]<<8;
428
                raw+= pRxBuffer[5];
330 holgerb 429
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
430
                {
431
                  if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Y = raw; // here Z and Y are exchanged
432
                  else MagRawVector.Z = raw;
433
                }
434
 
242 killagreg 435
        }
254 killagreg 436
        if(Compass_CalState || !NCMAG_IsCalibrated)
284 killagreg 437
        {       // mark out data invalid
289 killagreg 438
                MagVector.X = MagRawVector.X;
439
                MagVector.Y = MagRawVector.Y;
440
                MagVector.Z = MagRawVector.Z;
254 killagreg 441
                Compass_Heading = -1;
442
        }
443
        else
444
        {
445
                // update MagVector from MagRaw Vector by Scaling
446
                MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range);
447
                MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range);
448
                MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range);
292 killagreg 449
                Compass_CalcHeading();
254 killagreg 450
        }
242 killagreg 451
}
254 killagreg 452
// rx data handler  for acceleration raw data
253 killagreg 453
void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
454
{       // if number of byte are matching
254 killagreg 455
        if(RxBufferSize == sizeof(AccRawVector) )
253 killagreg 456
        {
254 killagreg 457
                memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
253 killagreg 458
        }
459
}
254 killagreg 460
// rx data handler for reading magnetic sensor configuration
253 killagreg 461
void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
462
{       // if number of byte are matching
463
        if(RxBufferSize == sizeof(MagConfig) )
464
        {
465
                memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig));
466
        }
467
}
254 killagreg 468
// rx data handler for reading acceleration sensor configuration
253 killagreg 469
void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize)
470
{       // if number of byte are matching
471
        if(RxBufferSize == sizeof(AccConfig) )
472
        {
473
                memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
474
        }
475
}
254 killagreg 476
//----------------------------------------------------------------------
253 killagreg 477
 
254 killagreg 478
 
479
// ---------------------------------------------------------------------
253 killagreg 480
u8 NCMAG_SetMagConfig(void)
481
{
482
        u8 retval = 0;
483
        // try to catch the i2c buffer within 100 ms timeout
484
        if(I2C_LockBuffer(100))
485
        {
486
                u8 TxBytes = 0;
487
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;    
488
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&MagConfig, sizeof(MagConfig));
489
                TxBytes += sizeof(MagConfig);
490
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, 0, 0))
491
                {
492
                        if(I2C_WaitForEndOfTransmission(100))
493
                        {
494
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
495
                        }
496
                }
497
        }
498
        return(retval);        
499
}
242 killagreg 500
 
253 killagreg 501
// ----------------------------------------------------------------------------------------
502
u8 NCMAG_GetMagConfig(void)
242 killagreg 503
{
253 killagreg 504
        u8 retval = 0;
252 killagreg 505
        // try to catch the i2c buffer within 100 ms timeout
248 killagreg 506
        if(I2C_LockBuffer(100))
242 killagreg 507
        {
253 killagreg 508
                u8 TxBytes = 0;
509
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;
510
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
248 killagreg 511
                {
252 killagreg 512
                        if(I2C_WaitForEndOfTransmission(100))
513
                        {
514
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
515
                        }
248 killagreg 516
                }
242 killagreg 517
        }
253 killagreg 518
        return(retval);        
242 killagreg 519
}
520
 
521
// ----------------------------------------------------------------------------------------
253 killagreg 522
u8 NCMAG_SetAccConfig(void)
242 killagreg 523
{
252 killagreg 524
        u8 retval = 0;
253 killagreg 525
        // try to catch the i2c buffer within 100 ms timeout
248 killagreg 526
        if(I2C_LockBuffer(100))
242 killagreg 527
        {
253 killagreg 528
                u8 TxBytes = 0;
529
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1;  
530
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
531
                TxBytes += sizeof(AccConfig);
532
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
533
                {
534
                        if(I2C_WaitForEndOfTransmission(100))
535
                        {
536
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
537
                        }
538
                }
539
        }
540
        return(retval);        
541
}
542
 
543
// ----------------------------------------------------------------------------------------
544
u8 NCMAG_GetAccConfig(void)
545
{
546
        u8 retval = 0;
547
        // try to catch the i2c buffer within 100 ms timeout
548
        if(I2C_LockBuffer(100))
549
        {
550
                u8 TxBytes = 0;
551
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1;
552
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
553
                {
554
                        if(I2C_WaitForEndOfTransmission(100))
555
                        {
556
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
557
                        }
558
                }
559
        }
560
        return(retval);        
561
}
562
 
563
// ----------------------------------------------------------------------------------------
564
u8 NCMAG_GetIdentification(void)
565
{
566
        u8 retval = 0;
567
        // try to catch the i2c buffer within 100 ms timeout
568
        if(I2C_LockBuffer(100))
569
        {
570
                u16 TxBytes = 0;
571
                NCMAG_Identification.A = 0xFF;
572
                NCMAG_Identification.B = 0xFF;
573
                NCMAG_Identification.C = 0xFF;
574
                I2C_Buffer[TxBytes++] = REG_MAG_IDA;
248 killagreg 575
                // initiate transmission
253 killagreg 576
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
248 killagreg 577
                {
253 killagreg 578
                        if(I2C_WaitForEndOfTransmission(100))
252 killagreg 579
                        {
580
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
581
                        }
248 killagreg 582
                }
242 killagreg 583
        }
253 killagreg 584
        return(retval);
242 killagreg 585
}
586
 
329 holgerb 587
u8 NCMAG_GetIdentification_Sub(void)
588
{
589
        u8 retval = 0;
590
        // try to catch the i2c buffer within 100 ms timeout
591
        if(I2C_LockBuffer(100))
592
        {
593
                u16 TxBytes = 0;
594
                NCMAG_Identification2.Sub = 0xFF;
595
                I2C_Buffer[TxBytes++] = REG_MAG_IDF;
596
                // initiate transmission
597
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
598
                {
599
                        if(I2C_WaitForEndOfTransmission(100))
600
                        {
601
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
602
                        }
603
                }
604
        }
605
        return(retval);
606
}
607
 
608
 
253 killagreg 609
// ----------------------------------------------------------------------------------------
610
void NCMAG_GetMagVector(void)
611
{
612
        // try to catch the I2C buffer within 0 ms
613
        if(I2C_LockBuffer(0))
614
        {
330 holgerb 615
//       s16 tmp;
253 killagreg 616
                u16 TxBytes = 0;
617
                // set register pointer
618
                I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB;
619
                // initiate transmission
620
                I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
621
        }
622
}
623
 
242 killagreg 624
//----------------------------------------------------------------
253 killagreg 625
void NCMAG_GetAccVector(void)
243 killagreg 626
{
252 killagreg 627
        // try to catch the I2C buffer within 0 ms
628
        if(I2C_LockBuffer(0))
243 killagreg 629
        {
248 killagreg 630
                u16 TxBytes = 0;
243 killagreg 631
                // set register pointer
253 killagreg 632
                I2C_Buffer[TxBytes++] = REG_ACC_X_LSB;
243 killagreg 633
                // initiate transmission
254 killagreg 634
                I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
243 killagreg 635
        }
636
}
637
 
330 holgerb 638
//----------------------------------------------------------------
639
void InitNC_MagnetSensor(void)
640
{
641
        s16 xscale, yscale, zscale;
642
        u8 crb_gain, cra_rate;
338 holgerb 643
//      u8  retval = 1;
330 holgerb 644
 
645
        switch(NCMAG_MagType)
646
        {
647
                case MAG_TYPE_HMC5843:
339 holgerb 648
                        crb_gain = HMC5843_CRB_GAIN_15GA;
330 holgerb 649
                        cra_rate = HMC5843_CRA_RATE_50HZ;
650
                        xscale = HMC5843_TEST_XSCALE;
651
                        yscale = HMC5843_TEST_YSCALE;
652
                        zscale = HMC5843_TEST_ZSCALE;
653
                        break;
654
 
655
                case MAG_TYPE_LSM303DLH:
338 holgerb 656
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
330 holgerb 657
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
658
                        xscale = LSM303DLH_TEST_XSCALE;
659
                        yscale = LSM303DLH_TEST_YSCALE;
660
                        zscale = LSM303DLH_TEST_ZSCALE;
661
                        break;
662
 
663
                default:
338 holgerb 664
                return;
330 holgerb 665
        }
666
 
667
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
668
        MagConfig.crb = crb_gain;
669
        MagConfig.mode = MODE_CONTINUOUS;
670
        NCMAG_SetMagConfig();
671
}
672
 
673
 
253 killagreg 674
// --------------------------------------------------------
292 killagreg 675
void NCMAG_Update(void)
243 killagreg 676
{
292 killagreg 677
        static u32 TimerUpdate = 0;
321 holgerb 678
        static u8 send_config = 0;
243 killagreg 679
 
254 killagreg 680
        if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
681
        {
682
                Compass_Heading = -1;
326 holgerb 683
                DebugOut.Analog[14]++; // count I2C error
254 killagreg 684
                return;
685
        }
292 killagreg 686
        if(CheckDelay(TimerUpdate))
243 killagreg 687
        {
326 holgerb 688
           if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
689
       if(++send_config == 25)   // 500ms
321 holgerb 690
            {
691
                 send_config = 0;
330 holgerb 692
             InitNC_MagnetSensor();
321 holgerb 693
                 TimerUpdate = SetDelay(15);    // back into the old time-slot
694
            }
695
                else
696
                {
254 killagreg 697
                // check for new calibration state
698
                Compass_UpdateCalState();
699
                if(Compass_CalState) NCMAG_Calibrate();
700
                NCMAG_GetMagVector(); //Get new data;
326 holgerb 701
                if(send_config == 24) TimerUpdate = SetDelay(5);    // next event is the re-configuration
321 holgerb 702
                else TimerUpdate = SetDelay(20);    // every 20 ms are 50 Hz
703
                }
243 killagreg 704
        }
705
}
706
 
330 holgerb 707
 
254 killagreg 708
// --------------------------------------------------------
253 killagreg 709
u8 NCMAG_SelfTest(void)
243 killagreg 710
{
266 holgerb 711
        u8 msg[64];
275 killagreg 712
        static u8 done = 0;
266 holgerb 713
 
287 holgerb 714
        if(done) return(1);        // just make it once
275 killagreg 715
 
271 holgerb 716
        #define LIMITS(value, min, max) {min = (80 * value)/100; max = (120 * value)/100;}
243 killagreg 717
        u32 time;
253 killagreg 718
        s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
719
        s16 xscale, yscale, zscale, scale_min, scale_max;
720
        u8 crb_gain, cra_rate;
721
        u8 i = 0, retval = 1;
243 killagreg 722
 
253 killagreg 723
        switch(NCMAG_MagType)
724
        {
725
                case MAG_TYPE_HMC5843:
339 holgerb 726
                        crb_gain = HMC5843_CRB_GAIN_15GA;
253 killagreg 727
                        cra_rate = HMC5843_CRA_RATE_50HZ;
728
                        xscale = HMC5843_TEST_XSCALE;
729
                        yscale = HMC5843_TEST_YSCALE;
730
                        zscale = HMC5843_TEST_ZSCALE;
731
                        break;
732
 
733
                case MAG_TYPE_LSM303DLH:
338 holgerb 734
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
253 killagreg 735
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
736
                        xscale = LSM303DLH_TEST_XSCALE;
737
                        yscale = LSM303DLH_TEST_YSCALE;
738
                        zscale = LSM303DLH_TEST_ZSCALE;
739
                        break;
740
 
741
                default:
742
                return(0);
743
        }
744
 
745
        MagConfig.cra = cra_rate|CRA_MODE_POSBIAS;
746
        MagConfig.crb = crb_gain;
747
        MagConfig.mode = MODE_CONTINUOUS;
748
        // activate positive bias field
749
        NCMAG_SetMagConfig();
251 killagreg 750
        // wait for stable readings
751
        time = SetDelay(50);
752
        while(!CheckDelay(time));
243 killagreg 753
        // averaging
253 killagreg 754
        #define AVERAGE 20
755
        for(i = 0; i<AVERAGE; i++)
243 killagreg 756
        {
253 killagreg 757
                NCMAG_GetMagVector();
243 killagreg 758
                time = SetDelay(20);
759
        while(!CheckDelay(time));
254 killagreg 760
                XMax += MagRawVector.X;
761
                YMax += MagRawVector.Y;
762
                ZMax += MagRawVector.Z;
243 killagreg 763
        }
253 killagreg 764
        MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS;
765
        // activate positive bias field
766
        NCMAG_SetMagConfig();
251 killagreg 767
    // wait for stable readings
768
        time = SetDelay(50);
769
        while(!CheckDelay(time));
243 killagreg 770
        // averaging
253 killagreg 771
        for(i = 0; i < AVERAGE; i++)
243 killagreg 772
        {
253 killagreg 773
                NCMAG_GetMagVector();
243 killagreg 774
                time = SetDelay(20);
775
        while(!CheckDelay(time));
254 killagreg 776
                XMin += MagRawVector.X;
777
                YMin += MagRawVector.Y;
778
                ZMin += MagRawVector.Z;
243 killagreg 779
        }
780
        // setup final configuration
253 killagreg 781
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
782
        // activate positive bias field
783
        NCMAG_SetMagConfig();
266 holgerb 784
        // check scale for all axes
243 killagreg 785
        // prepare scale limits
253 killagreg 786
        LIMITS(xscale, scale_min, scale_max);
267 holgerb 787
        xscale = (XMax - XMin)/(2*AVERAGE);
266 holgerb 788
        if((xscale > scale_max) || (xscale < scale_min))
789
     {
790
          retval = 0;
791
      sprintf(msg, "\r\n Value X: %d not %d-%d !", xscale, scale_min,scale_max);
792
          UART1_PutString(msg);
793
     }
267 holgerb 794
        LIMITS(yscale, scale_min, scale_max);
266 holgerb 795
        yscale = (YMax - YMin)/(2*AVERAGE);
796
        if((yscale > scale_max) || (yscale < scale_min))
797
     {
798
          retval = 0;
799
      sprintf(msg, "\r\n Value Y: %d not %d-%d !", yscale, scale_min,scale_max);
800
          UART1_PutString(msg);
801
     }
267 holgerb 802
        LIMITS(zscale, scale_min, scale_max);
266 holgerb 803
        zscale = (ZMax - ZMin)/(2*AVERAGE);
804
        if((zscale > scale_max) || (zscale < scale_min))      
805
         {
806
          retval = 0;
807
      sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
808
          UART1_PutString(msg);
809
     }
275 killagreg 810
        done = retval;
253 killagreg 811
        return(retval);
243 killagreg 812
}
813
 
814
 
815
//----------------------------------------------------------------
253 killagreg 816
u8 NCMAG_Init(void)
242 killagreg 817
{
818
        u8 msg[64];
252 killagreg 819
        u8 retval = 0;
242 killagreg 820
        u8 repeat;
821
 
253 killagreg 822
        NCMAG_Present = 0;
823
        NCMAG_MagType = MAG_TYPE_HMC5843;       // assuming having an HMC5843
824
        // polling for LSM302DLH option
825
        repeat = 0;
826
        do
827
        {
828
                retval = NCMAG_GetAccConfig();
829
                if(retval) break; // break loop on success
830
                UART1_PutString(".");
831
                repeat++;
832
        }while(repeat < 3);
833
        if(retval) NCMAG_MagType = MAG_TYPE_LSM303DLH; // must be a LSM303DLH
242 killagreg 834
        // polling of identification
835
        repeat = 0;
836
        do
837
        {
329 holgerb 838
                retval = NCMAG_GetIdentification_Sub();
839
                if(retval) break; // break loop on success
840
                UART1_PutString(".");
841
                repeat++;
842
        }while(repeat < 12);
843
        retval = 0;
844
        do
845
        {
253 killagreg 846
                retval = NCMAG_GetIdentification();
252 killagreg 847
                if(retval) break; // break loop on success
242 killagreg 848
                UART1_PutString(".");
849
                repeat++;
252 killagreg 850
        }while(repeat < 12);
329 holgerb 851
 
253 killagreg 852
        // if we got an answer to id request
252 killagreg 853
        if(retval)
242 killagreg 854
        {
329 holgerb 855
                u8 n1[] = "\n\r HMC5843";
856
                u8 n2[] = "\n\r LSM303DLH";
857
                u8 n3[] = "\n\r LSM303DLM";
253 killagreg 858
                u8* pn;
329 holgerb 859
 
860
                pn = n1;
861
                if(NCMAG_MagType == MAG_TYPE_LSM303DLH)
862
                {
863
                 if(NCMAG_Identification2.Sub == 0x3c) pn = n3;
864
                 else pn = n2;
865
                }
866
 
867
                sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
242 killagreg 868
                UART1_PutString(msg);
253 killagreg 869
                if (    (NCMAG_Identification.A == MAG_IDA)
870
                     && (NCMAG_Identification.B == MAG_IDB)
871
                         && (NCMAG_Identification.C == MAG_IDC))
242 killagreg 872
                {
268 killagreg 873
                        NCMAG_Present = 1;
329 holgerb 874
 
875
                        if(EEPROM_Init())
264 killagreg 876
                                {
877
                                        NCMAG_IsCalibrated = NCMag_CalibrationRead();
878
                                        if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
879
                                }
329 holgerb 880
                        else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
881
 
882
                        if(NCMAG_Identification2.Sub == 0x00)
883
                        {
884
                         if(!NCMAG_SelfTest())
885
                         {
886
                                UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
887
                                LED_RED_ON;
888
                                NCMAG_IsCalibrated = 0;
889
                         }      else UART1_PutString("\r\n Selftest ok");
254 killagreg 890
                        }
330 holgerb 891
                        else InitNC_MagnetSensor();
242 killagreg 892
                }
893
                else
894
                {
254 killagreg 895
                        UART1_PutString("\n\r Not compatible!");
256 killagreg 896
                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
242 killagreg 897
                        LED_RED_ON;
898
                }
899
        }
253 killagreg 900
        else // nothing found
901
        {
902
                NCMAG_MagType = MAG_TYPE_NONE;
903
                UART1_PutString("not found!");  
904
        }
905
        return(NCMAG_Present);
242 killagreg 906
}
907