Subversion Repositories NaviCtrl

Rev

Rev 395 | Rev 398 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 395 Rev 397
1 
/*#######################################################################################*/
 1 
/*#######################################################################################*/
2 
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
 2 
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3 
/*#######################################################################################*/
 3 
/*#######################################################################################*/
4 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 4 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5 
// + www.MikroKopter.com
 5 
// + www.MikroKopter.com
6 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 6 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7 
// + Software Nutzungsbedingungen (english version: see below)
 7 
// + Software Nutzungsbedingungen (english version: see below)
8 
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
 8 
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
9 
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
 9 
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
10 
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
10 
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
11 
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
 11 
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
12 
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
 12 
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
13 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 13 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14 
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
 14 
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
15 
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
 15 
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
16 
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
 16 
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
17 
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
 17 
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
18 
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
 18 
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
19 
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
 19 
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
20 
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
 20 
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
21 
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
21 
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
22 
// + des Mitverschuldens offen.
 22 
// + des Mitverschuldens offen.
23 
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
 23 
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
24 
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
 24 
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
25 
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
 25 
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
26 
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
 26 
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
27 
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
 27 
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
28 
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
 28 
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
29 
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
 29 
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
30 
// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
 30 
// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
31 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 31 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32 
// + Software LICENSING TERMS
 32 
// + Software LICENSING TERMS
33 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 33 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34 
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
 34 
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
35 
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
35 
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
36 
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
 36 
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
37 
// + The Software may only be used with the Licensor's products.
 37 
// + The Software may only be used with the Licensor's products.
38 
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
 38 
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
39 
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
 39 
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
40 
// + agreement shall be the property of the Licensor.
 40 
// + agreement shall be the property of the Licensor.
41 
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
 41 
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
42 
// + features that can be used to identify the program may not be altered or defaced by the customer.
 42 
// + features that can be used to identify the program may not be altered or defaced by the customer.
43 
// + The customer shall be responsible for taking reasonable precautions
 43 
// + The customer shall be responsible for taking reasonable precautions
44 
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
 44 
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
45 
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
 45 
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
46 
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
 46 
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
47 
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
 47 
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
48 
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
 48 
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
49 
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
 49 
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
50 
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
 50 
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
51 
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
 51 
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
52 
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
 52 
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53 
// + #### END OF LICENSING TERMS ####
 53 
// + #### END OF LICENSING TERMS ####
54 
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
 54 
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
55 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 55 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56 
#include <math.h>
 56 
#include <math.h>
57 
#include <stdio.h>
 57 
#include <stdio.h>
58 
#include <string.h>
 58 
#include <string.h>
59 
#include "91x_lib.h"
 59 
#include "91x_lib.h"
60 
#include "ncmag.h"
 60 
#include "ncmag.h"
61 
#include "i2c.h"
 61 
#include "i2c.h"
62 
#include "timer1.h"
 62 
#include "timer1.h"
63 
#include "led.h"
 63 
#include "led.h"
64 
#include "uart1.h"
 64 
#include "uart1.h"
65 
#include "eeprom.h"
 65 
#include "eeprom.h"
66 
#include "mymath.h"
 66 
#include "mymath.h"
67 
#include "main.h"
 67 
#include "main.h"
68 
 
 68 
 
69 
u8 NCMAG_Present = 0;
 69 
u8 NCMAG_Present = 0;
70 
u8 NCMAG_IsCalibrated = 0;
 70 
u8 NCMAG_IsCalibrated = 0;
71 
 
 71 
 
72 
// supported magnetic sensor types
 72 
// supported magnetic sensor types
73 
#define TYPE_NONE                       0
 73 
#define TYPE_NONE                       0
74 
#define TYPE_HMC5843            1
 74 
#define TYPE_HMC5843            1
75 
#define TYPE_LSM303DLH          2
 75 
#define TYPE_LSM303DLH          2
76 
#define TYPE_LSM303DLM          3
 76 
#define TYPE_LSM303DLM          3
77 
 
 77 
 
78 
u8 NCMAG_SensorType = TYPE_NONE;
 78 
u8 NCMAG_SensorType = TYPE_NONE;
79 
 
 79 
 
80 
#define EEPROM_ADR_MAG_CALIBRATION              50
 80 
#define EEPROM_ADR_MAG_CALIBRATION              50
81 
#define CALIBRATION_VERSION                     1
 81 
#define CALIBRATION_VERSION                     1
82 
#define MAG_CALIBRATION_COMPATIBLE              0xA2
 82 
#define MAG_CALIBRATION_COMPATIBLE              0xA2
83 
 
 83 
 
84 
#define NCMAG_MIN_RAWVALUE -2047
 84 
#define NCMAG_MIN_RAWVALUE -2047
85 
#define NCMAG_MAX_RAWVALUE  2047
 85 
#define NCMAG_MAX_RAWVALUE  2047
86 
#define NCMAG_INVALID_DATA -4096
 86 
#define NCMAG_INVALID_DATA -4096
87 
 
 87 
 
88 
typedef struct
 88 
typedef struct
89 
{
 89 
{
90 
        s16 Range;
 90 
        s16 Range;
91 
        s16 Offset;
 91 
        s16 Offset;
92 
} __attribute__((packed)) Scaling_t;
 92 
} __attribute__((packed)) Scaling_t;
93 
 
 93 
 
94 
typedef struct
 94 
typedef struct
95 
{
 95 
{
96 
        Scaling_t MagX;
 96 
        Scaling_t MagX;
97 
        Scaling_t MagY;
 97 
        Scaling_t MagY;
98 
        Scaling_t MagZ;
 98 
        Scaling_t MagZ;
99 
        u8 Version;
 99 
        u8 Version;
100 
        u8 crc;
 100 
        u8 crc;
101 
} __attribute__((packed)) Calibration_t;
 101 
} __attribute__((packed)) Calibration_t;
102 
 
 102 
 
103 
Calibration_t Calibration;              // calibration data in RAM
103 
Calibration_t Calibration;              // calibration data in RAM
104 
volatile s16vec_t AccRawVector;
 104 
volatile s16vec_t AccRawVector;
105 
volatile s16vec_t MagRawVector;
 105 
volatile s16vec_t MagRawVector;
106 
 
 106 
 
107 
// i2c MAG interface
 107 
// i2c MAG interface
108 
#define MAG_SLAVE_ADDRESS       0x3C    // i2C slave address mag. sensor registers
 108 
#define MAG_SLAVE_ADDRESS       0x3C    // i2C slave address mag. sensor registers
109 
 
 109 
 
110 
// register mapping
 110 
// register mapping
111 
#define REG_MAG_CRA                     0x00
 111 
#define REG_MAG_CRA                     0x00
112 
#define REG_MAG_CRB                     0x01
 112 
#define REG_MAG_CRB                     0x01
113 
#define REG_MAG_MODE            0x02
 113 
#define REG_MAG_MODE            0x02
114 
#define REG_MAG_DATAX_MSB       0x03
 114 
#define REG_MAG_DATAX_MSB       0x03
115 
#define REG_MAG_DATAX_LSB       0x04
 115 
#define REG_MAG_DATAX_LSB       0x04
116 
#define REG_MAG_DATAY_MSB       0x05
 116 
#define REG_MAG_DATAY_MSB       0x05
117 
#define REG_MAG_DATAY_LSB       0x06
 117 
#define REG_MAG_DATAY_LSB       0x06
118 
#define REG_MAG_DATAZ_MSB       0x07
 118 
#define REG_MAG_DATAZ_MSB       0x07
119 
#define REG_MAG_DATAZ_LSB       0x08
 119 
#define REG_MAG_DATAZ_LSB       0x08
120 
#define REG_MAG_STATUS          0x09
 120 
#define REG_MAG_STATUS          0x09
121 
 
 121 
 
122 
#define REG_MAG_IDA                     0x0A
 122 
#define REG_MAG_IDA                     0x0A
123 
#define REG_MAG_IDB                     0x0B
 123 
#define REG_MAG_IDB                     0x0B
124 
#define REG_MAG_IDC                     0x0C
 124 
#define REG_MAG_IDC                     0x0C
125 
#define REG_MAG_IDF                     0x0F  // WHO_AM_I _M = 0x03c when LSM303DLM is connected
 125 
#define REG_MAG_IDF                     0x0F  // WHO_AM_I _M = 0x03c when LSM303DLM is connected
126 
 
 126 
 
127 
// bit mask for configuration mode
 127 
// bit mask for configuration mode
128 
#define CRA_MODE_MASK           0x03
 128 
#define CRA_MODE_MASK           0x03
129 
#define CRA_MODE_NORMAL         0x00    //default
 129 
#define CRA_MODE_NORMAL         0x00    //default
130 
#define CRA_MODE_POSBIAS        0x01
 130 
#define CRA_MODE_POSBIAS        0x01
131 
#define CRA_MODE_NEGBIAS        0x02
 131 
#define CRA_MODE_NEGBIAS        0x02
132 
#define CRA_MODE_SELFTEST       0x03
 132 
#define CRA_MODE_SELFTEST       0x03
133 
 
 133 
 
134 
// bit mask for measurement mode
 134 
// bit mask for measurement mode
135 
#define MODE_MASK                       0xFF
 135 
#define MODE_MASK                       0xFF
136 
#define MODE_CONTINUOUS         0x00
 136 
#define MODE_CONTINUOUS         0x00
137 
#define MODE_SINGLE                     0x01    // default
 137 
#define MODE_SINGLE                     0x01    // default
138 
#define MODE_IDLE                       0x02
 138 
#define MODE_IDLE                       0x02
139 
#define MODE_SLEEP                      0x03
 139 
#define MODE_SLEEP                      0x03
140 
 
 140 
 
141 
// bit mask for rate
 141 
// bit mask for rate
142 
#define CRA_RATE_MASK           0x1C
 142 
#define CRA_RATE_MASK           0x1C
143 
 
 143 
 
144 
// bit mask for gain
 144 
// bit mask for gain
145 
#define CRB_GAIN_MASK           0xE0
 145 
#define CRB_GAIN_MASK           0xE0
146 
 
 146 
 
147 
// ids
 147 
// ids
148 
#define MAG_IDA         0x48
 148 
#define MAG_IDA         0x48
149 
#define MAG_IDB         0x34
 149 
#define MAG_IDB         0x34
150 
#define MAG_IDC         0x33
 150 
#define MAG_IDC         0x33
151 
#define MAG_IDF_LSM303DLM       0x3C
 151 
#define MAG_IDF_LSM303DLM       0x3C
152 
 
 152 
 
153 
// the special HMC5843 interface
 153 
// the special HMC5843 interface
154 
// bit mask for rate
 154 
// bit mask for rate
155 
#define HMC5843_CRA_RATE_0_5HZ          0x00
 155 
#define HMC5843_CRA_RATE_0_5HZ          0x00
156 
#define HMC5843_CRA_RATE_1HZ            0x04
 156 
#define HMC5843_CRA_RATE_1HZ            0x04
157 
#define HMC5843_CRA_RATE_2HZ            0x08
 157 
#define HMC5843_CRA_RATE_2HZ            0x08
158 
#define HMC5843_CRA_RATE_5HZ            0x0C
 158 
#define HMC5843_CRA_RATE_5HZ            0x0C
159 
#define HMC5843_CRA_RATE_10HZ           0x10    //default
 159 
#define HMC5843_CRA_RATE_10HZ           0x10    //default
160 
#define HMC5843_CRA_RATE_20HZ           0x14
 160 
#define HMC5843_CRA_RATE_20HZ           0x14
161 
#define HMC5843_CRA_RATE_50HZ           0x18
 161 
#define HMC5843_CRA_RATE_50HZ           0x18
162 
// bit mask for gain
 162 
// bit mask for gain
163 
#define HMC5843_CRB_GAIN_07GA           0x00
 163 
#define HMC5843_CRB_GAIN_07GA           0x00
164 
#define HMC5843_CRB_GAIN_10GA           0x20    //default
 164 
#define HMC5843_CRB_GAIN_10GA           0x20    //default
165 
#define HMC5843_CRB_GAIN_15GA           0x40    // <--- we use this    
165 
#define HMC5843_CRB_GAIN_15GA           0x40    // <--- we use this    
166 
#define HMC5843_CRB_GAIN_20GA           0x60
 166 
#define HMC5843_CRB_GAIN_20GA           0x60
167 
#define HMC5843_CRB_GAIN_32GA           0x80
 167 
#define HMC5843_CRB_GAIN_32GA           0x80
168 
#define HMC5843_CRB_GAIN_38GA           0xA0
 168 
#define HMC5843_CRB_GAIN_38GA           0xA0
169 
#define HMC5843_CRB_GAIN_45GA           0xC0
 169 
#define HMC5843_CRB_GAIN_45GA           0xC0
170 
#define HMC5843_CRB_GAIN_65GA           0xE0
 170 
#define HMC5843_CRB_GAIN_65GA           0xE0
171 
// self test value
 171 
// self test value
172 
#define HMC5843_TEST_XSCALE             555
 172 
#define HMC5843_TEST_XSCALE             555
173 
#define HMC5843_TEST_YSCALE             555
 173 
#define HMC5843_TEST_YSCALE             555
174 
#define HMC5843_TEST_ZSCALE             555
 174 
#define HMC5843_TEST_ZSCALE             555
175 
// calibration range
 175 
// calibration range
176 
#define HMC5843_CALIBRATION_RANGE   600
 176 
#define HMC5843_CALIBRATION_RANGE   600
177 
 
 177 
 
178 
// the special LSM302DLH interface
 178 
// the special LSM302DLH interface
179 
// bit mask for rate
 179 
// bit mask for rate
180 
#define LSM303DLH_CRA_RATE_0_75HZ       0x00
 180 
#define LSM303DLH_CRA_RATE_0_75HZ       0x00
181 
#define LSM303DLH_CRA_RATE_1_5HZ        0x04
 181 
#define LSM303DLH_CRA_RATE_1_5HZ        0x04
182 
#define LSM303DLH_CRA_RATE_3_0HZ        0x08
 182 
#define LSM303DLH_CRA_RATE_3_0HZ        0x08
183 
#define LSM303DLH_CRA_RATE_7_5HZ        0x0C
 183 
#define LSM303DLH_CRA_RATE_7_5HZ        0x0C
184 
#define LSM303DLH_CRA_RATE_15HZ         0x10    //default
 184 
#define LSM303DLH_CRA_RATE_15HZ         0x10    //default
185 
#define LSM303DLH_CRA_RATE_30HZ         0x14
 185 
#define LSM303DLH_CRA_RATE_30HZ         0x14
186 
#define LSM303DLH_CRA_RATE_75HZ         0x18
 186 
#define LSM303DLH_CRA_RATE_75HZ         0x18
187 
 
 187 
 
188 
// bit mask for gain
 188 
// bit mask for gain
189 
#define LSM303DLH_CRB_GAIN_XXGA         0x00
 189 
#define LSM303DLH_CRB_GAIN_XXGA         0x00
190 
#define LSM303DLH_CRB_GAIN_13GA         0x20    //default
 190 
#define LSM303DLH_CRB_GAIN_13GA         0x20    //default
191 
#define LSM303DLH_CRB_GAIN_19GA         0x40    // <--- we use this
 191 
#define LSM303DLH_CRB_GAIN_19GA         0x40    // <--- we use this
192 
#define LSM303DLH_CRB_GAIN_25GA         0x60
 192 
#define LSM303DLH_CRB_GAIN_25GA         0x60
193 
#define LSM303DLH_CRB_GAIN_40GA         0x80
 193 
#define LSM303DLH_CRB_GAIN_40GA         0x80
194 
#define LSM303DLH_CRB_GAIN_47GA         0xA0
 194 
#define LSM303DLH_CRB_GAIN_47GA         0xA0
195 
#define LSM303DLH_CRB_GAIN_56GA         0xC0
 195 
#define LSM303DLH_CRB_GAIN_56GA         0xC0
196 
#define LSM303DLH_CRB_GAIN_81GA         0xE0
 196 
#define LSM303DLH_CRB_GAIN_81GA         0xE0
197 
 
 197 
 
198 
typedef struct
 198 
typedef struct
199 
{
 199 
{
200 
        u8 A;
 200 
        u8 A;
201 
        u8 B;
 201 
        u8 B;
202 
        u8 C;
 202 
        u8 C;
203 
} __attribute__((packed)) Identification_t;
 203 
} __attribute__((packed)) Identification_t;
204 
volatile Identification_t NCMAG_Identification;
 204 
volatile Identification_t NCMAG_Identification;
205 
 
 205 
 
206 
typedef struct
 206 
typedef struct
207 
{
 207 
{
208 
        u8 Sub;
 208 
        u8 Sub;
209 
} __attribute__((packed)) Identification2_t;
 209 
} __attribute__((packed)) Identification2_t;
210 
volatile Identification2_t NCMAG_Identification2;
 210 
volatile Identification2_t NCMAG_Identification2;
211 
 
 211 
 
212 
typedef struct
 212 
typedef struct
213 
{
 213 
{
214 
        u8 cra;
 214 
        u8 cra;
215 
        u8 crb;
 215 
        u8 crb;
216 
        u8 mode;
 216 
        u8 mode;
217 
} __attribute__((packed)) MagConfig_t;
 217 
} __attribute__((packed)) MagConfig_t;
218 
 
 218 
 
219 
volatile MagConfig_t MagConfig;
 219 
volatile MagConfig_t MagConfig;
220 
 
 220 
 
221 
 
 221 
 
222 
// self test value
 222 
// self test value
223 
#define LSM303DLH_TEST_XSCALE   495
 223 
#define LSM303DLH_TEST_XSCALE   495
224 
#define LSM303DLH_TEST_YSCALE   495
 224 
#define LSM303DLH_TEST_YSCALE   495
225 
#define LSM303DLH_TEST_ZSCALE   470
 225 
#define LSM303DLH_TEST_ZSCALE   470
226 
// clibration range
 226 
// clibration range
227 
#define LSM303_CALIBRATION_RANGE   550
 227 
#define LSM303_CALIBRATION_RANGE   550
228 
 
 228 
 
229 
// the i2c ACC interface
 229 
// the i2c ACC interface
230 
#define ACC_SLAVE_ADDRESS               0x30    // i2c slave for acc. sensor registers
 230 
#define ACC_SLAVE_ADDRESS               0x30    // i2c slave for acc. sensor registers
231 
 
 231 
 
232 
// multiple byte read/write mask
 232 
// multiple byte read/write mask
233 
#define REG_ACC_MASK_AUTOINCREMENT 0x80
 233 
#define REG_ACC_MASK_AUTOINCREMENT 0x80
234 
 
 234 
 
235 
// register mapping
 235 
// register mapping
236 
#define REG_ACC_CTRL1                   0x20
 236 
#define REG_ACC_CTRL1                   0x20
237 
#define REG_ACC_CTRL2                   0x21
 237 
#define REG_ACC_CTRL2                   0x21
238 
#define REG_ACC_CTRL3                   0x22
 238 
#define REG_ACC_CTRL3                   0x22
239 
#define REG_ACC_CTRL4                   0x23
 239 
#define REG_ACC_CTRL4                   0x23
240 
#define REG_ACC_CTRL5                   0x24
 240 
#define REG_ACC_CTRL5                   0x24
241 
#define REG_ACC_HP_FILTER_RESET 0x25
 241 
#define REG_ACC_HP_FILTER_RESET 0x25
242 
#define REG_ACC_REFERENCE               0x26
 242 
#define REG_ACC_REFERENCE               0x26
243 
#define REG_ACC_STATUS                  0x27
 243 
#define REG_ACC_STATUS                  0x27
244 
#define REG_ACC_X_LSB                   0x28
 244 
#define REG_ACC_X_LSB                   0x28
245 
#define REG_ACC_X_MSB                   0x29
 245 
#define REG_ACC_X_MSB                   0x29
246 
#define REG_ACC_Y_LSB                   0x2A
 246 
#define REG_ACC_Y_LSB                   0x2A
247 
#define REG_ACC_Y_MSB                   0x2B
 247 
#define REG_ACC_Y_MSB                   0x2B
248 
#define REG_ACC_Z_LSB                   0x2C
 248 
#define REG_ACC_Z_LSB                   0x2C
249 
#define REG_ACC_Z_MSB                   0x2D
 249 
#define REG_ACC_Z_MSB                   0x2D
250 
 
 250 
 
251 
#define ACC_CRTL1_PM_DOWN               0x00
 251 
#define ACC_CRTL1_PM_DOWN               0x00
252 
#define ACC_CRTL1_PM_NORMAL             0x20
 252 
#define ACC_CRTL1_PM_NORMAL             0x20
253 
#define ACC_CRTL1_PM_LOW_0_5HZ  0x40
 253 
#define ACC_CRTL1_PM_LOW_0_5HZ  0x40
254 
#define ACC_CRTL1_PM_LOW_1HZ    0x60
 254 
#define ACC_CRTL1_PM_LOW_1HZ    0x60
255 
#define ACC_CRTL1_PM_LOW_2HZ    0x80
 255 
#define ACC_CRTL1_PM_LOW_2HZ    0x80
256 
#define ACC_CRTL1_PM_LOW_5HZ    0xA0
 256 
#define ACC_CRTL1_PM_LOW_5HZ    0xA0
257 
#define ACC_CRTL1_PM_LOW_10HZ   0xC0
 257 
#define ACC_CRTL1_PM_LOW_10HZ   0xC0
258 
// Output data rate in normal power mode
 258 
// Output data rate in normal power mode
259 
#define ACC_CRTL1_DR_50HZ               0x00
 259 
#define ACC_CRTL1_DR_50HZ               0x00
260 
#define ACC_CRTL1_DR_100HZ              0x08
 260 
#define ACC_CRTL1_DR_100HZ              0x08
261 
#define ACC_CRTL1_DR_400HZ              0x10
 261 
#define ACC_CRTL1_DR_400HZ              0x10
262 
#define ACC_CRTL1_DR_1000HZ             0x18
 262 
#define ACC_CRTL1_DR_1000HZ             0x18
263 
// axis anable flags                   
263 
// axis anable flags                   
264 
#define ACC_CRTL1_XEN                   0x01
 264 
#define ACC_CRTL1_XEN                   0x01
265 
#define ACC_CRTL1_YEN                   0x02
 265 
#define ACC_CRTL1_YEN                   0x02
266 
#define ACC_CRTL1_ZEN                   0x04
 266 
#define ACC_CRTL1_ZEN                   0x04
267 
 
 267 
 
268 
#define ACC_CRTL2_FILTER8       0x00
 268 
#define ACC_CRTL2_FILTER8       0x10
269 
#define ACC_CRTL2_FILTER16      0x01
 269 
#define ACC_CRTL2_FILTER16      0x11
270 
#define ACC_CRTL2_FILTER32      0x02
 270 
#define ACC_CRTL2_FILTER32      0x12
271 
#define ACC_CRTL2_FILTER64      0x03
 271 
#define ACC_CRTL2_FILTER64      0x13
272 
 
 272 
 
273 
#define ACC_CTRL4_BDU                   0x80 // Block data update, (0: continuos update; 1: output registers not updated between MSB and LSB reading)
 273 
#define ACC_CTRL4_BDU                   0x80 // Block data update, (0: continuos update; 1: output registers not updated between MSB and LSB reading)
274 
#define ACC_CTRL4_BLE                   0x40 // Big/little endian, (0: data LSB @ lower address; 1: data MSB @ lower address)
 274 
#define ACC_CTRL4_BLE                   0x40 // Big/little endian, (0: data LSB @ lower address; 1: data MSB @ lower address)
275 
#define ACC_CTRL4_FS_2G                 0x00
 275 
#define ACC_CTRL4_FS_2G                 0x00
276 
#define ACC_CTRL4_FS_4G                 0x10
 276 
#define ACC_CTRL4_FS_4G                 0x10
277 
#define ACC_CTRL4_FS_8G                 0x30
 277 
#define ACC_CTRL4_FS_8G                 0x30
278 
#define ACC_CTRL4_STSIGN_PLUS   0x00
 278 
#define ACC_CTRL4_STSIGN_PLUS   0x00
279 
#define ACC_CTRL4_STSIGN_MINUS  0x08
 279 
#define ACC_CTRL4_STSIGN_MINUS  0x08
280 
#define ACC_CTRL4_ST_ENABLE             0x02
 280 
#define ACC_CTRL4_ST_ENABLE             0x02
281 
 
 281 
 
282 
#define ACC_CTRL5_STW_ON                0x03
 282 
#define ACC_CTRL5_STW_ON                0x03
283 
#define ACC_CTRL5_STW_OFF               0x00
 283 
#define ACC_CTRL5_STW_OFF               0x00
284 
 
 284 
 
285 
typedef struct
 285 
typedef struct
286 
{
 286 
{
287 
        u8 ctrl_1;
 287 
        u8 ctrl_1;
288 
        u8 ctrl_2;
 288 
        u8 ctrl_2;
289 
        u8 ctrl_3;
 289 
        u8 ctrl_3;
290 
        u8 ctrl_4;
 290 
        u8 ctrl_4;
291 
        u8 ctrl_5;
 291 
        u8 ctrl_5;
292 
} __attribute__((packed)) AccConfig_t;
 292 
} __attribute__((packed)) AccConfig_t;
293 
 
 293 
 
294 
volatile AccConfig_t AccConfig;
 294 
volatile AccConfig_t AccConfig;
295 
 
 295 
 
296 
u8 NCMag_CalibrationWrite(void)
 296 
u8 NCMag_CalibrationWrite(void)
297 
{
 297 
{
298 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
 298 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
299 
        EEPROM_Result_t eres;
 299 
        EEPROM_Result_t eres;
300 
        u8 *pBuff = (u8*)&Calibration;
 300 
        u8 *pBuff = (u8*)&Calibration;
301 
 
 301 
 
302 
        Calibration.Version = CALIBRATION_VERSION;
 302 
        Calibration.Version = CALIBRATION_VERSION;
303 
        for(i = 0; i<(sizeof(Calibration)-1); i++)
 303 
        for(i = 0; i<(sizeof(Calibration)-1); i++)
304 
        {
 304 
        {
305 
                crc += pBuff[i];        
 305 
                crc += pBuff[i];        
306 
        }
 306 
        }
307 
        Calibration.crc = ~crc;
 307 
        Calibration.crc = ~crc;
308 
        eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration));
 308 
        eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration));
309 
        if(EEPROM_SUCCESS == eres) i = 1;
 309 
        if(EEPROM_SUCCESS == eres) i = 1;
310 
        else i = 0;
 310 
        else i = 0;
311 
        return(i);     
 311 
        return(i);     
312 
}
 312 
}
313 
 
 313 
 
314 
u8 NCMag_CalibrationRead(void)
 314 
u8 NCMag_CalibrationRead(void)
315 
{
 315 
{
316 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
 316 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
317 
        u8 *pBuff = (u8*)&Calibration;
 317 
        u8 *pBuff = (u8*)&Calibration;
318 
 
 318 
 
319 
        if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)))
 319 
        if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)))
320 
        {
 320 
        {
321 
                for(i = 0; i<(sizeof(Calibration)-1); i++)
 321 
                for(i = 0; i<(sizeof(Calibration)-1); i++)
322 
                {
 322 
                {
323 
                        crc += pBuff[i];        
 323 
                        crc += pBuff[i];        
324 
                }
 324 
                }
325 
                crc = ~crc;
 325 
                crc = ~crc;
326 
                if(Calibration.crc != crc) return(0); // crc mismatch
 326 
                if(Calibration.crc != crc) return(0); // crc mismatch
327 
                if(Calibration.Version == CALIBRATION_VERSION) return(1);
 327 
                if(Calibration.Version == CALIBRATION_VERSION) return(1);
328 
        }
 328 
        }
329 
        return(0);
 329 
        return(0);
330 
}
 330 
}
331 
 
 331 
 
332 
 
 332 
 
333 
void NCMAG_Calibrate(void)
 333 
void NCMAG_Calibrate(void)
334 
{
 334 
{
335 
        u8 msg[64];
 335 
        u8 msg[64];
336 
        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
 336 
        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
337 
        static s16 X = 0, Y = 0, Z = 0;
 337 
        static s16 X = 0, Y = 0, Z = 0;
338 
        static u8 OldCalState = 0;     
 338 
        static u8 OldCalState = 0;     
339 
        s16 MinCalibration = 450;
 339 
        s16 MinCalibration = 450;
340 
 
 340 
 
341 
        X = (4*X + MagRawVector.X + 3)/5;
 341 
        X = (4*X + MagRawVector.X + 3)/5;
342 
        Y = (4*Y + MagRawVector.Y + 3)/5;
 342 
        Y = (4*Y + MagRawVector.Y + 3)/5;
343 
        Z = (4*Z + MagRawVector.Z + 3)/5;
 343 
        Z = (4*Z + MagRawVector.Z + 3)/5;
344 
 
 344 
 
345 
        switch(Compass_CalState)
 345 
        switch(Compass_CalState)
346 
        {
 346 
        {
347 
                case 1:
 347 
                case 1:
348 
                        // 1st step of calibration
 348 
                        // 1st step of calibration
349 
                        // initialize ranges
 349 
                        // initialize ranges
350 
                        // used to change the orientation of the NC in the horizontal plane
 350 
                        // used to change the orientation of the NC in the horizontal plane
351 
                        Xmin =  10000;
 351 
                        Xmin =  10000;
352 
                        Xmax = -10000;
 352 
                        Xmax = -10000;
353 
                        Ymin =  10000;
 353 
                        Ymin =  10000;
354 
                        Ymax = -10000;
 354 
                        Ymax = -10000;
355 
                        Zmin =  10000;
 355 
                        Zmin =  10000;
356 
                        Zmax = -10000;
 356 
                        Zmax = -10000;
357 
                        break;
 357 
                        break;
358 
               
 358 
               
359 
                case 2: // 2nd step of calibration
 359 
                case 2: // 2nd step of calibration
360 
                        // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
 360 
                        // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
361 
                        if(X < Xmin)            { Xmin = X; BeepTime = 20;}
 361 
                        if(X < Xmin)            { Xmin = X; BeepTime = 20;}
362 
                        else if(X > Xmax)       { Xmax = X; BeepTime = 20;}
 362 
                        else if(X > Xmax)       { Xmax = X; BeepTime = 20;}
363 
                        if(Y < Ymin)            { Ymin = Y; BeepTime = 60;}
 363 
                        if(Y < Ymin)            { Ymin = Y; BeepTime = 60;}
364 
                        else if(Y > Ymax)       { Ymax = Y; BeepTime = 60;}
 364 
                        else if(Y > Ymax)       { Ymax = Y; BeepTime = 60;}
365 
                        break;
 365 
                        break;
366 
 
 366 
 
367 
                case 3: // 3rd step of calibration
 367 
                case 3: // 3rd step of calibration
368 
                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
 368 
                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
369 
                        break;
 369 
                        break;
370 
 
 370 
 
371 
                case 4:
 371 
                case 4:
372 
                        // find Min and Max of the Z-Sensor
 372 
                        // find Min and Max of the Z-Sensor
373 
                        if(Z < Zmin)      { Zmin = Z; BeepTime = 80;}
 373 
                        if(Z < Zmin)      { Zmin = Z; BeepTime = 80;}
374 
                        else if(Z > Zmax) { Zmax = Z; BeepTime = 80;}
 374 
                        else if(Z > Zmax) { Zmax = Z; BeepTime = 80;}
375 
                        break;
 375 
                        break;
376 
               
 376 
               
377 
                case 5:
 377 
                case 5:
378 
                        // Save values
 378 
                        // Save values
379 
                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
 379 
                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
380 
                        {
 380 
                        {
381 
                                switch(NCMAG_SensorType)
 381 
                                switch(NCMAG_SensorType)
382 
                                {
 382 
                                {
383 
                                        case TYPE_HMC5843:
 383 
                                        case TYPE_HMC5843:
384 
                                                UART1_PutString("\r\nHMC5843 calibration\n\r");
 384 
                                                UART1_PutString("\r\nHMC5843 calibration\n\r");
385 
                                                MinCalibration = HMC5843_CALIBRATION_RANGE;
 385 
                                                MinCalibration = HMC5843_CALIBRATION_RANGE;
386 
                                                break;
 386 
                                                break;
387 
 
 387 
 
388 
                                        case TYPE_LSM303DLH:
 388 
                                        case TYPE_LSM303DLH:
389 
                                        case TYPE_LSM303DLM:
 389 
                                        case TYPE_LSM303DLM:
390 
                                                UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
 390 
                                                UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
391 
                                                MinCalibration = LSM303_CALIBRATION_RANGE;
 391 
                                                MinCalibration = LSM303_CALIBRATION_RANGE;
392 
                                        break;
 392 
                                        break;
393 
                                }
 393 
                                }
394 
                                if(EarthMagneticStrengthTheoretic)
 394 
                                if(EarthMagneticStrengthTheoretic)
395 
                                 {
 395 
                                 {
396 
                                  MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
 396 
                                  MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
397 
                                  sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
 397 
                                  sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
398 
                                  UART1_PutString(msg);
 398 
                                  UART1_PutString(msg);
399 
                                 }
 399 
                                 }
400 
                            else UART1_PutString("without GPS\n\r");
 400 
                            else UART1_PutString("without GPS\n\r");
401 
 
 401 
 
402 
                                Calibration.MagX.Range = Xmax - Xmin;
 402 
                                Calibration.MagX.Range = Xmax - Xmin;
403 
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
 403 
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
404 
                                Calibration.MagY.Range = Ymax - Ymin;
 404 
                                Calibration.MagY.Range = Ymax - Ymin;
405 
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
 405 
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
406 
                                Calibration.MagZ.Range = Zmax - Zmin;
 406 
                                Calibration.MagZ.Range = Zmax - Zmin;
407 
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
 407 
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
408 
                                if((Calibration.MagX.Range > MinCalibration) && (Calibration.MagY.Range > MinCalibration) && (Calibration.MagZ.Range > MinCalibration))
 408 
                                if((Calibration.MagX.Range > MinCalibration) && (Calibration.MagY.Range > MinCalibration) && (Calibration.MagZ.Range > MinCalibration))
409 
                                {
 409 
                                {
410 
                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite();
 410 
                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite();
411 
                                        BeepTime = 2500;
 411 
                                        BeepTime = 2500;
412 
                                        UART1_PutString("\r\n-> Calibration okay <-\n\r");
 412 
                                        UART1_PutString("\r\n-> Calibration okay <-\n\r");
413 
                                }
 413 
                                }
414 
                                else
 414 
                                else
415 
                                {
 415 
                                {
416 
                                        UART1_PutString("\r\nCalibration FAILED - Values too low: ");
 416 
                                        UART1_PutString("\r\nCalibration FAILED - Values too low: ");
417 
                                    if(Calibration.MagX.Range < MinCalibration) UART1_PutString("X! ");
 417 
                                    if(Calibration.MagX.Range < MinCalibration) UART1_PutString("X! ");
418 
                                    if(Calibration.MagY.Range < MinCalibration) UART1_PutString("Y! ");
 418 
                                    if(Calibration.MagY.Range < MinCalibration) UART1_PutString("Y! ");
419 
                                    if(Calibration.MagZ.Range < MinCalibration) UART1_PutString("Z! ");
 419 
                                    if(Calibration.MagZ.Range < MinCalibration) UART1_PutString("Z! ");
420 
                                        UART1_PutString("\r\n");
 420 
                                        UART1_PutString("\r\n");
421 
 
 421 
 
422 
                                        // restore old calibration data from eeprom
 422 
                                        // restore old calibration data from eeprom
423 
                                        NCMAG_IsCalibrated = NCMag_CalibrationRead();
 423 
                                        NCMAG_IsCalibrated = NCMag_CalibrationRead();
424 
                                }
 424 
                                }
425 
                                        sprintf(msg, "X: (%i - %i = %i)\r\n",Xmax,Xmin,Xmax - Xmin);
 425 
                                        sprintf(msg, "X: (%i - %i = %i)\r\n",Xmax,Xmin,Xmax - Xmin);
426 
                                        UART1_PutString(msg);
 426 
                                        UART1_PutString(msg);
427 
                                        sprintf(msg, "Y: (%i - %i = %i)\r\n",Ymax,Ymin,Ymax - Ymin);
 427 
                                        sprintf(msg, "Y: (%i - %i = %i)\r\n",Ymax,Ymin,Ymax - Ymin);
428 
                                        UART1_PutString(msg);
 428 
                                        UART1_PutString(msg);
429 
                                        sprintf(msg, "Z: (%i - %i = %i)\r\n",Zmax,Zmin,Zmax - Zmin);
 429 
                                        sprintf(msg, "Z: (%i - %i = %i)\r\n",Zmax,Zmin,Zmax - Zmin);
430 
                                        UART1_PutString(msg);
 430 
                                        UART1_PutString(msg);
431 
                                        sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCalibration);
 431 
                                        sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCalibration);
432 
                                        UART1_PutString(msg);
 432 
                                        UART1_PutString(msg);
433 
                        }
 433 
                        }
434 
                        break;
 434 
                        break;
435 
                       
 435 
                       
436 
                default:
 436 
                default:
437 
                        break; 
 437 
                        break; 
438 
        }
 438 
        }
439 
        OldCalState = Compass_CalState;
 439 
        OldCalState = Compass_CalState;
440 
}
 440 
}
441 
 
 441 
 
442 
// ---------- call back handlers -----------------------------------------
 442 
// ---------- call back handlers -----------------------------------------
443 
 
 443 
 
444 
// rx data handler for id info request
 444 
// rx data handler for id info request
445 
void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
 445 
void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
446 
{       // if number of bytes are matching
 446 
{       // if number of bytes are matching
447 
        if(RxBufferSize == sizeof(NCMAG_Identification) )
 447 
        if(RxBufferSize == sizeof(NCMAG_Identification) )
448 
        {
 448 
        {
449 
                memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
 449 
                memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
450 
        }
 450 
        }
451 
}
 451 
}
452 
 
 452 
 
453 
void NCMAG_UpdateIdentification_Sub(u8* pRxBuffer, u8 RxBufferSize)
 453 
void NCMAG_UpdateIdentification_Sub(u8* pRxBuffer, u8 RxBufferSize)
454 
{       // if number of bytes are matching
 454 
{       // if number of bytes are matching
455 
        if(RxBufferSize == sizeof(NCMAG_Identification2))
 455 
        if(RxBufferSize == sizeof(NCMAG_Identification2))
456 
        {
 456 
        {
457 
                memcpy((u8 *)&NCMAG_Identification2, pRxBuffer, sizeof(NCMAG_Identification2));
 457 
                memcpy((u8 *)&NCMAG_Identification2, pRxBuffer, sizeof(NCMAG_Identification2));
458 
        }
 458 
        }
459 
}
 459 
}
460 
 
 460 
 
461 
// rx data handler for magnetic sensor raw data
 461 
// rx data handler for magnetic sensor raw data
462 
void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
 462 
void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
463 
{       // if number of bytes are matching
 463 
{       // if number of bytes are matching
464 
        if(RxBufferSize == sizeof(MagRawVector) )
 464 
        if(RxBufferSize == sizeof(MagRawVector) )
465 
        {       // byte order from big to little endian
 465 
        {       // byte order from big to little endian
466 
                s16 raw;
 466 
                s16 raw;
467 
                raw = pRxBuffer[0]<<8;
 467 
                raw = pRxBuffer[0]<<8;
468 
                raw+= pRxBuffer[1];
 468 
                raw+= pRxBuffer[1];
469 
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw;
 469 
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw;
470 
                raw = pRxBuffer[2]<<8;
 470 
                raw = pRxBuffer[2]<<8;
471 
                raw+= pRxBuffer[3];
 471 
                raw+= pRxBuffer[3];
472 
            if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
 472 
            if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
473 
                {
 473 
                {
474 
                        if(NCMAG_SensorType == TYPE_LSM303DLM)  MagRawVector.Z = raw; // here Z and Y are exchanged
 474 
                        if(NCMAG_SensorType == TYPE_LSM303DLM)  MagRawVector.Z = raw; // here Z and Y are exchanged
475 
                        else                                                                    MagRawVector.Y = raw;
 475 
                        else                                                                    MagRawVector.Y = raw;
476 
                }
 476 
                }
477 
                raw = pRxBuffer[4]<<8;
 477 
                raw = pRxBuffer[4]<<8;
478 
                raw+= pRxBuffer[5];
 478 
                raw+= pRxBuffer[5];
479 
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
 479 
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
480 
                {
 480 
                {
481 
                        if(NCMAG_SensorType == TYPE_LSM303DLM)  MagRawVector.Y = raw; // here Z and Y are exchanged
 481 
                        if(NCMAG_SensorType == TYPE_LSM303DLM)  MagRawVector.Y = raw; // here Z and Y are exchanged
482 
                        else                                                                    MagRawVector.Z = raw;
 482 
                        else                                                                    MagRawVector.Z = raw;
483 
                }
 483 
                }
484 
        }
 484 
        }
485 
        if(Compass_CalState || !NCMAG_IsCalibrated)
 485 
        if(Compass_CalState || !NCMAG_IsCalibrated)
486 
        {       // mark out data invalid
 486 
        {       // mark out data invalid
487 
                MagVector.X = MagRawVector.X;
 487 
                MagVector.X = MagRawVector.X;
488 
                MagVector.Y = MagRawVector.Y;
 488 
                MagVector.Y = MagRawVector.Y;
489 
                MagVector.Z = MagRawVector.Z;
 489 
                MagVector.Z = MagRawVector.Z;
490 
                Compass_Heading = -1;
 490 
                Compass_Heading = -1;
491 
        }
 491 
        }
492 
        else
 492 
        else
493 
        {
 493 
        {
494 
                // update MagVector from MagRaw Vector by Scaling
 494 
                // update MagVector from MagRaw Vector by Scaling
495 
                MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range);
 495 
                MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range);
496 
                MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range);
 496 
                MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range);
497 
                MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range);
 497 
                MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range);
498 
                Compass_CalcHeading();
 498 
                Compass_CalcHeading();
499 
        }
 499 
        }
500 
}
 500 
}
501 
// rx data handler  for acceleration raw data
 501 
// rx data handler  for acceleration raw data
502 
void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
 502 
void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
503 
{       // if number of byte are matching
 503 
{       // if number of byte are matching
- 
 
 504 
static s32 filter_z;
504 
        if(RxBufferSize == sizeof(AccRawVector) )
 505 
        if(RxBufferSize == sizeof(AccRawVector) )
505 
        {
 506 
        {
506 
                memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
 507 
                memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
507 
        }
 508 
        }
508 
        DebugOut.Analog[16] = AccRawVector.X;
 509 
        DebugOut.Analog[16] = AccRawVector.X;
509 
        DebugOut.Analog[17] = AccRawVector.Y;
 510 
        DebugOut.Analog[17] = AccRawVector.Y;
- 
 
 511 
filter_z = (filter_z * 7 + AccRawVector.Z) / 8;
- 
 
 512 
 
510 
        DebugOut.Analog[18] = AccRawVector.Z;
 513 
        DebugOut.Analog[18] = filter_z;
511 
DebugOut.Analog[19] = (DebugOut.Analog[19] * 3 + AccRawVector.Z) / 4;
 514 
        DebugOut.Analog[19] = AccRawVector.Z;
512 
}
 515 
}
513 
// rx data handler for reading magnetic sensor configuration
 516 
// rx data handler for reading magnetic sensor configuration
514 
void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
 517 
void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
515 
{       // if number of byte are matching
 518 
{       // if number of byte are matching
516 
        if(RxBufferSize == sizeof(MagConfig) )
 519 
        if(RxBufferSize == sizeof(MagConfig) )
517 
        {
 520 
        {
518 
                memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig));
 521 
                memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig));
519 
        }
 522 
        }
520 
}
 523 
}
521 
// rx data handler for reading acceleration sensor configuration
 524 
// rx data handler for reading acceleration sensor configuration
522 
void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize)
 525 
void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize)
523 
{       // if number of byte are matching
 526 
{       // if number of byte are matching
524 
        if(RxBufferSize == sizeof(AccConfig) )
 527 
        if(RxBufferSize == sizeof(AccConfig) )
525 
        {
 528 
        {
526 
                memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
 529 
                memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
527 
        }
 530 
        }
528 
}
 531 
}
529 
//----------------------------------------------------------------------
 532 
//----------------------------------------------------------------------
530 
 
 533 
 
531 
 
 534 
 
532 
// ---------------------------------------------------------------------
 535 
// ---------------------------------------------------------------------
533 
u8 NCMAG_SetMagConfig(void)
 536 
u8 NCMAG_SetMagConfig(void)
534 
{
 537 
{
535 
        u8 retval = 0;
 538 
        u8 retval = 0;
536 
        // try to catch the i2c buffer within 100 ms timeout
 539 
        // try to catch the i2c buffer within 100 ms timeout
537 
        if(I2C_LockBuffer(100))
 540 
        if(I2C_LockBuffer(100))
538 
        {
 541 
        {
539 
                u8 TxBytes = 0;
 542 
                u8 TxBytes = 0;
540 
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;    
 543 
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;    
541 
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&MagConfig, sizeof(MagConfig));
 544 
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&MagConfig, sizeof(MagConfig));
542 
                TxBytes += sizeof(MagConfig);
 545 
                TxBytes += sizeof(MagConfig);
543 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, 0, 0))
 546 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, 0, 0))
544 
                {
 547 
                {
545 
                        if(I2C_WaitForEndOfTransmission(100))
 548 
                        if(I2C_WaitForEndOfTransmission(100))
546 
                        {
 549 
                        {
547 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 550 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
548 
                        }
 551 
                        }
549 
                }
 552 
                }
550 
        }
 553 
        }
551 
        return(retval);        
 554 
        return(retval);        
552 
}
 555 
}
553 
 
 556 
 
554 
// ----------------------------------------------------------------------------------------
 557 
// ----------------------------------------------------------------------------------------
555 
u8 NCMAG_GetMagConfig(void)
 558 
u8 NCMAG_GetMagConfig(void)
556 
{
 559 
{
557 
        u8 retval = 0;
 560 
        u8 retval = 0;
558 
        // try to catch the i2c buffer within 100 ms timeout
 561 
        // try to catch the i2c buffer within 100 ms timeout
559 
        if(I2C_LockBuffer(100))
 562 
        if(I2C_LockBuffer(100))
560 
        {
 563 
        {
561 
                u8 TxBytes = 0;
 564 
                u8 TxBytes = 0;
562 
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;
 565 
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;
563 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
 566 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
564 
                {
 567 
                {
565 
                        if(I2C_WaitForEndOfTransmission(100))
 568 
                        if(I2C_WaitForEndOfTransmission(100))
566 
                        {
 569 
                        {
567 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 570 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
568 
                        }
 571 
                        }
569 
                }
 572 
                }
570 
        }
 573 
        }
571 
        return(retval);        
 574 
        return(retval);        
572 
}
 575 
}
573 
 
 576 
 
574 
// ----------------------------------------------------------------------------------------
 577 
// ----------------------------------------------------------------------------------------
575 
u8 NCMAG_SetAccConfig(void)
 578 
u8 NCMAG_SetAccConfig(void)
576 
{
 579 
{
577 
        u8 retval = 0;
 580 
        u8 retval = 0;
578 
        // try to catch the i2c buffer within 100 ms timeout
 581 
        // try to catch the i2c buffer within 100 ms timeout
579 
        if(I2C_LockBuffer(100))
 582 
        if(I2C_LockBuffer(100))
580 
        {
 583 
        {
581 
                u8 TxBytes = 0;
 584 
                u8 TxBytes = 0;
582 
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;      
 585 
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;      
583 
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
 586 
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
584 
                TxBytes += sizeof(AccConfig);
 587 
                TxBytes += sizeof(AccConfig);
585 
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
 588 
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
586 
                {
 589 
                {
587 
                        if(I2C_WaitForEndOfTransmission(100))
 590 
                        if(I2C_WaitForEndOfTransmission(100))
588 
                        {
 591 
                        {
589 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 592 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
590 
                        }
 593 
                        }
591 
                }
 594 
                }
592 
        }
 595 
        }
593 
        return(retval);        
 596 
        return(retval);        
594 
}
 597 
}
595 
 
 598 
 
596 
// ----------------------------------------------------------------------------------------
 599 
// ----------------------------------------------------------------------------------------
597 
u8 NCMAG_GetAccConfig(void)
 600 
u8 NCMAG_GetAccConfig(void)
598 
{
 601 
{
599 
        u8 retval = 0;
 602 
        u8 retval = 0;
600 
        // try to catch the i2c buffer within 100 ms timeout
 603 
        // try to catch the i2c buffer within 100 ms timeout
601 
        if(I2C_LockBuffer(100))
 604 
        if(I2C_LockBuffer(100))
602 
        {
 605 
        {
603 
                u8 TxBytes = 0;
 606 
                u8 TxBytes = 0;
604 
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
 607 
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
605 
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
 608 
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
606 
                {
 609 
                {
607 
                        if(I2C_WaitForEndOfTransmission(100))
 610 
                        if(I2C_WaitForEndOfTransmission(100))
608 
                        {
 611 
                        {
609 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 612 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
610 
                        }
 613 
                        }
611 
                }
 614 
                }
612 
        }
 615 
        }
613 
        return(retval);        
 616 
        return(retval);        
614 
}
 617 
}
615 
 
 618 
 
616 
// ----------------------------------------------------------------------------------------
 619 
// ----------------------------------------------------------------------------------------
617 
u8 NCMAG_GetIdentification(void)
 620 
u8 NCMAG_GetIdentification(void)
618 
{
 621 
{
619 
        u8 retval = 0;
 622 
        u8 retval = 0;
620 
        // try to catch the i2c buffer within 100 ms timeout
 623 
        // try to catch the i2c buffer within 100 ms timeout
621 
        if(I2C_LockBuffer(100))
 624 
        if(I2C_LockBuffer(100))
622 
        {
 625 
        {
623 
                u16 TxBytes = 0;
 626 
                u16 TxBytes = 0;
624 
                NCMAG_Identification.A = 0xFF;
 627 
                NCMAG_Identification.A = 0xFF;
625 
                NCMAG_Identification.B = 0xFF;
 628 
                NCMAG_Identification.B = 0xFF;
626 
                NCMAG_Identification.C = 0xFF;
 629 
                NCMAG_Identification.C = 0xFF;
627 
                I2C_Buffer[TxBytes++] = REG_MAG_IDA;
 630 
                I2C_Buffer[TxBytes++] = REG_MAG_IDA;
628 
                // initiate transmission
 631 
                // initiate transmission
629 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
 632 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
630 
                {
 633 
                {
631 
                        if(I2C_WaitForEndOfTransmission(100))
 634 
                        if(I2C_WaitForEndOfTransmission(100))
632 
                        {
 635 
                        {
633 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 636 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
634 
                        }
 637 
                        }
635 
                }
 638 
                }
636 
        }
 639 
        }
637 
        return(retval);
 640 
        return(retval);
638 
}
 641 
}
639 
 
 642 
 
640 
u8 NCMAG_GetIdentification_Sub(void)
 643 
u8 NCMAG_GetIdentification_Sub(void)
641 
{
 644 
{
642 
        u8 retval = 0;
 645 
        u8 retval = 0;
643 
        // try to catch the i2c buffer within 100 ms timeout
 646 
        // try to catch the i2c buffer within 100 ms timeout
644 
        if(I2C_LockBuffer(100))
 647 
        if(I2C_LockBuffer(100))
645 
        {
 648 
        {
646 
                u16 TxBytes = 0;
 649 
                u16 TxBytes = 0;
647 
                NCMAG_Identification2.Sub = 0xFF;
 650 
                NCMAG_Identification2.Sub = 0xFF;
648 
                I2C_Buffer[TxBytes++] = REG_MAG_IDF;
 651 
                I2C_Buffer[TxBytes++] = REG_MAG_IDF;
649 
                // initiate transmission
 652 
                // initiate transmission
650 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
 653 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
651 
                {
 654 
                {
652 
                        if(I2C_WaitForEndOfTransmission(100))
 655 
                        if(I2C_WaitForEndOfTransmission(100))
653 
                        {
 656 
                        {
654 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 657 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
655 
                        }
 658 
                        }
656 
                }
 659 
                }
657 
        }
 660 
        }
658 
        return(retval);
 661 
        return(retval);
659 
}
 662 
}
660 
 
 663 
 
661 
 
 664 
 
662 
// ----------------------------------------------------------------------------------------
 665 
// ----------------------------------------------------------------------------------------
663 
void NCMAG_GetMagVector(void)
 666 
void NCMAG_GetMagVector(void)
664 
{
 667 
{
665 
        // try to catch the I2C buffer within 0 ms
 668 
        // try to catch the I2C buffer within 0 ms
666 
        if(I2C_LockBuffer(0))
 669 
        if(I2C_LockBuffer(0))
667 
        {
 670 
        {
668 
                u16 TxBytes = 0;
 671 
                u16 TxBytes = 0;
669 
                // set register pointer
 672 
                // set register pointer
670 
                I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB;
 673 
                I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB;
671 
                // initiate transmission
 674 
                // initiate transmission
672 
                I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
 675 
                I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
673 
        }
 676 
        }
674 
}
 677 
}
675 
 
 678 
 
676 
//----------------------------------------------------------------
 679 
//----------------------------------------------------------------
677 
void NCMAG_GetAccVector(void)
 680 
void NCMAG_GetAccVector(void)
678 
{
 681 
{
679 
        // try to catch the I2C buffer within 0 ms
 682 
        // try to catch the I2C buffer within 0 ms
680 
        if(I2C_LockBuffer(0))
 683 
        if(I2C_LockBuffer(0))
681 
        {
 684 
        {
682 
                u16 TxBytes = 0;
 685 
                u16 TxBytes = 0;
683 
                // set register pointer
 686 
                // set register pointer
684 
                I2C_Buffer[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT;
 687 
                I2C_Buffer[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT;
685 
                // initiate transmission
 688 
                // initiate transmission
686 
                I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
 689 
                I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
687 
        }
 690 
        }
688 
}
 691 
}
689 
 
 692 
 
690 
//----------------------------------------------------------------
 693 
//----------------------------------------------------------------
691 
u8 InitNC_MagnetSensor(void)
 694 
u8 InitNC_MagnetSensor(void)
692 
{
 695 
{
693 
        u8 crb_gain, cra_rate;
 696 
        u8 crb_gain, cra_rate;
694 
 
 697 
 
695 
        switch(NCMAG_SensorType)
 698 
        switch(NCMAG_SensorType)
696 
        {
 699 
        {
697 
                case TYPE_HMC5843:
 700 
                case TYPE_HMC5843:
698 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
 701 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
699 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
 702 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
700 
                        break;
 703 
                        break;
701 
 
 704 
 
702 
                case TYPE_LSM303DLH:
 705 
                case TYPE_LSM303DLH:
703 
                case TYPE_LSM303DLM:
 706 
                case TYPE_LSM303DLM:
704 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
 707 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
705 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
 708 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
706 
                        break;
 709 
                        break;
707 
 
 710 
 
708 
                default:
 711 
                default:
709 
                return(0);
 712 
                return(0);
710 
        }
 713 
        }
711 
 
 714 
 
712 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
 715 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
713 
        MagConfig.crb = crb_gain;
 716 
        MagConfig.crb = crb_gain;
714 
        MagConfig.mode = MODE_CONTINUOUS;
 717 
        MagConfig.mode = MODE_CONTINUOUS;
715 
        return(NCMAG_SetMagConfig());
 718 
        return(NCMAG_SetMagConfig());
716 
}
 719 
}
717 
 
 720 
 
718 
 
 721 
 
719 
//----------------------------------------------------------------
 722 
//----------------------------------------------------------------
720 
u8 NCMAG_Init_ACCSensor(void)
 723 
u8 NCMAG_Init_ACCSensor(void)
721 
{
 724 
{
722 
        AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN;
 725 
        AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN;
723 
        AccConfig.ctrl_2 = ACC_CRTL2_FILTER64;
 726 
        AccConfig.ctrl_2 = 0;//ACC_CRTL2_FILTER32;
724 
        AccConfig.ctrl_3 = 0x00;
 727 
        AccConfig.ctrl_3 = 0x00;
725 
        AccConfig.ctrl_4 = ACC_CTRL4_BDU|ACC_CTRL4_FS_8G;
 728 
        AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G;
726 
        AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF;
 729 
        AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF;
727 
        return(NCMAG_SetAccConfig());
 730 
        return(NCMAG_SetAccConfig());
728 
}
 731 
}
729 
// --------------------------------------------------------
 732 
// --------------------------------------------------------
730 
void NCMAG_Update(void)
 733 
void NCMAG_Update(void)
731 
{
 734 
{
732 
        static u32 TimerUpdate = 0;
 735 
        static u32 TimerUpdate = 0;
733 
        static u8 send_config = 0;
 736 
        static u8 send_config = 0;
734 
        u32 delay = 20;
 737 
        u32 delay = 20;
735 
 
 738 
 
736 
        if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
 739 
        if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
737 
        {
 740 
        {
738 
                Compass_Heading = -1;
 741 
                Compass_Heading = -1;
739 
                DebugOut.Analog[14]++; // count I2C error
 742 
                DebugOut.Analog[14]++; // count I2C error
740 
                return;
 743 
                return;
741 
        }
 744 
        }
742 
        if(CheckDelay(TimerUpdate))
 745 
        if(CheckDelay(TimerUpdate))
743 
        {
 746 
        {
744 
                if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
 747 
                if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
745 
        if(++send_config == 25)   // 500ms
 748 
        if(++send_config == 25)   // 500ms
746 
                {
 749 
                {
747 
                        send_config = 0;
 750 
                        send_config = 0;
748 
                InitNC_MagnetSensor();
 751 
                InitNC_MagnetSensor();
749 
                        TimerUpdate = SetDelay(15);    // back into the old time-slot
 752 
                        TimerUpdate = SetDelay(15);    // back into the old time-slot
750 
                }
 753 
                }
751 
                else
 754 
                else
752 
                {
 755 
                {
753 
                        static u8 s = 0;
 756 
                        static u8 s = 0;
754 
                        // check for new calibration state
 757 
                        // check for new calibration state
755 
                        Compass_UpdateCalState();
 758 
                        Compass_UpdateCalState();
756 
                        if(Compass_CalState) NCMAG_Calibrate();
 759 
                        if(Compass_CalState) NCMAG_Calibrate();
757 
                       
 760 
                       
758 
                        // in case of LSM303 type
 761 
                        // in case of LSM303 type
759 
                        switch(NCMAG_SensorType)
 762 
                        switch(NCMAG_SensorType)
760 
                        {
 763 
                        {
761 
                                case TYPE_HMC5843:                             
 764 
                                case TYPE_HMC5843:                             
762 
                                        NCMAG_GetMagVector();
 765 
                                        NCMAG_GetMagVector();
763 
                                        delay = 20;
 766 
                                        delay = 20;
764 
                                        break;
 767 
                                        break;
765 
                                case TYPE_LSM303DLH:
 768 
                                case TYPE_LSM303DLH:
766 
                                case TYPE_LSM303DLM:
 769 
                                case TYPE_LSM303DLM:
- 
 
 770 
                                        NCMAG_GetMagVector();
- 
 
 771 
                                        delay = 20;
767 
                                        if(s){ NCMAG_GetMagVector(); s = 0;}
 772 
/*                                      if(s){ NCMAG_GetMagVector(); s = 0;}
768 
                                        else { NCMAG_GetAccVector(); s = 1;}
 773 
                                        else { NCMAG_GetAccVector(); s = 1;}
769 
                                        delay = 10;
 774 
                                        delay = 10;
- 
 
 775 
*/
770 
                                        break;                           
 776 
                                        break;                           
771 
                        }
 777 
                        }
772 
                        if(send_config == 24) TimerUpdate = SetDelay(5);    // next event is the re-configuration
 778 
                        if(send_config == 24) TimerUpdate = SetDelay(5);    // next event is the re-configuration
773 
                        else TimerUpdate = SetDelay(delay);    // every 20 ms are 50 Hz
 779 
                        else TimerUpdate = SetDelay(delay);    // every 20 ms are 50 Hz
774 
                }
 780 
                }
775 
        }
 781 
        }
776 
}
 782 
}
777 
 
 783 
 
778 
 
 784 
 
779 
// --------------------------------------------------------
 785 
// --------------------------------------------------------
780 
u8 NCMAG_SelfTest(void)
 786 
u8 NCMAG_SelfTest(void)
781 
{
 787 
{
782 
        u8 msg[64];
 788 
        u8 msg[64];
783 
        static u8 done = 0;
 789 
        static u8 done = 0;
784 
 
 790 
 
785 
        if(done) return(1);        // just make it once
 791 
        if(done) return(1);        // just make it once
786 
       
 792 
       
787 
        #define LIMITS(value, min, max) {min = (80 * value)/100; max = (120 * value)/100;}
 793 
        #define LIMITS(value, min, max) {min = (80 * value)/100; max = (120 * value)/100;}
788 
        u32 time;
 794 
        u32 time;
789 
        s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
 795 
        s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
790 
        s16 xscale, yscale, zscale, scale_min, scale_max;
 796 
        s16 xscale, yscale, zscale, scale_min, scale_max;
791 
        u8 crb_gain, cra_rate;
 797 
        u8 crb_gain, cra_rate;
792 
        u8 i = 0, retval = 1;
 798 
        u8 i = 0, retval = 1;
793 
 
 799 
 
794 
        switch(NCMAG_SensorType)
 800 
        switch(NCMAG_SensorType)
795 
        {
 801 
        {
796 
                case TYPE_HMC5843:
 802 
                case TYPE_HMC5843:
797 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
 803 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
798 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
 804 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
799 
                        xscale = HMC5843_TEST_XSCALE;
 805 
                        xscale = HMC5843_TEST_XSCALE;
800 
                        yscale = HMC5843_TEST_YSCALE;
 806 
                        yscale = HMC5843_TEST_YSCALE;
801 
                        zscale = HMC5843_TEST_ZSCALE;
 807 
                        zscale = HMC5843_TEST_ZSCALE;
802 
                        break;
 808 
                        break;
803 
 
 809 
 
804 
                case TYPE_LSM303DLH:
 810 
                case TYPE_LSM303DLH:
805 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
 811 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
806 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
 812 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
807 
                        xscale = LSM303DLH_TEST_XSCALE;
 813 
                        xscale = LSM303DLH_TEST_XSCALE;
808 
                        yscale = LSM303DLH_TEST_YSCALE;
 814 
                        yscale = LSM303DLH_TEST_YSCALE;
809 
                        zscale = LSM303DLH_TEST_ZSCALE;
 815 
                        zscale = LSM303DLH_TEST_ZSCALE;
810 
                        break;
 816 
                        break;
811 
 
 817 
 
812 
                case TYPE_LSM303DLM:
 818 
                case TYPE_LSM303DLM:
813 
                        // does not support self test feature
819 
                        // does not support self test feature
814 
                        done = retval;
 820 
                        done = retval;
815 
                        return(retval);
 821 
                        return(retval);
816 
                        break;
 822 
                        break;
817 
 
 823 
 
818 
                default:
 824 
                default:
819 
                        return(0);
 825 
                        return(0);
820 
        }
 826 
        }
821 
 
 827 
 
822 
        MagConfig.cra = cra_rate|CRA_MODE_POSBIAS;
 828 
        MagConfig.cra = cra_rate|CRA_MODE_POSBIAS;
823 
        MagConfig.crb = crb_gain;
 829 
        MagConfig.crb = crb_gain;
824 
        MagConfig.mode = MODE_CONTINUOUS;
 830 
        MagConfig.mode = MODE_CONTINUOUS;
825 
        // activate positive bias field
 831 
        // activate positive bias field
826 
        NCMAG_SetMagConfig();
 832 
        NCMAG_SetMagConfig();
827 
        // wait for stable readings
 833 
        // wait for stable readings
828 
        time = SetDelay(50);
 834 
        time = SetDelay(50);
829 
        while(!CheckDelay(time));
 835 
        while(!CheckDelay(time));
830 
        // averaging
 836 
        // averaging
831 
        #define AVERAGE 20
 837 
        #define AVERAGE 20
832 
        for(i = 0; i<AVERAGE; i++)
 838 
        for(i = 0; i<AVERAGE; i++)
833 
        {
 839 
        {
834 
                NCMAG_GetMagVector();
 840 
                NCMAG_GetMagVector();
835 
                time = SetDelay(20);
 841 
                time = SetDelay(20);
836 
        while(!CheckDelay(time));
 842 
        while(!CheckDelay(time));
837 
                XMax += MagRawVector.X;
 843 
                XMax += MagRawVector.X;
838 
                YMax += MagRawVector.Y;
 844 
                YMax += MagRawVector.Y;
839 
                ZMax += MagRawVector.Z;
 845 
                ZMax += MagRawVector.Z;
840 
        }
 846 
        }
841 
        MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS;
 847 
        MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS;
842 
        // activate positive bias field
 848 
        // activate positive bias field
843 
        NCMAG_SetMagConfig();
 849 
        NCMAG_SetMagConfig();
844 
    // wait for stable readings
 850 
    // wait for stable readings
845 
        time = SetDelay(50);
 851 
        time = SetDelay(50);
846 
        while(!CheckDelay(time));
 852 
        while(!CheckDelay(time));
847 
        // averaging
 853 
        // averaging
848 
        for(i = 0; i < AVERAGE; i++)
 854 
        for(i = 0; i < AVERAGE; i++)
849 
        {
 855 
        {
850 
                NCMAG_GetMagVector();
 856 
                NCMAG_GetMagVector();
851 
                time = SetDelay(20);
 857 
                time = SetDelay(20);
852 
        while(!CheckDelay(time));
 858 
        while(!CheckDelay(time));
853 
                XMin += MagRawVector.X;
 859 
                XMin += MagRawVector.X;
854 
                YMin += MagRawVector.Y;
 860 
                YMin += MagRawVector.Y;
855 
                ZMin += MagRawVector.Z;
 861 
                ZMin += MagRawVector.Z;
856 
        }
 862 
        }
857 
        // setup final configuration
 863 
        // setup final configuration
858 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
 864 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
859 
        // activate positive bias field
 865 
        // activate positive bias field
860 
        NCMAG_SetMagConfig();
 866 
        NCMAG_SetMagConfig();
861 
        // check scale for all axes
 867 
        // check scale for all axes
862 
        // prepare scale limits
 868 
        // prepare scale limits
863 
        LIMITS(xscale, scale_min, scale_max);
 869 
        LIMITS(xscale, scale_min, scale_max);
864 
        xscale = (XMax - XMin)/(2*AVERAGE);
 870 
        xscale = (XMax - XMin)/(2*AVERAGE);
865 
        if((xscale > scale_max) || (xscale < scale_min))
 871 
        if((xscale > scale_max) || (xscale < scale_min))
866 
    {
 872 
    {
867 
                retval = 0;
 873 
                retval = 0;
868 
        sprintf(msg, "\r\n Value X: %d not %d-%d !", xscale, scale_min,scale_max);
 874 
        sprintf(msg, "\r\n Value X: %d not %d-%d !", xscale, scale_min,scale_max);
869 
                UART1_PutString(msg);
 875 
                UART1_PutString(msg);
870 
    }
 876 
    }
871 
        LIMITS(yscale, scale_min, scale_max);
 877 
        LIMITS(yscale, scale_min, scale_max);
872 
        yscale = (YMax - YMin)/(2*AVERAGE);
 878 
        yscale = (YMax - YMin)/(2*AVERAGE);
873 
        if((yscale > scale_max) || (yscale < scale_min))
 879 
        if((yscale > scale_max) || (yscale < scale_min))
874 
    {
 880 
    {
875 
                retval = 0;
 881 
                retval = 0;
876 
        sprintf(msg, "\r\n Value Y: %d not %d-%d !", yscale, scale_min,scale_max);
 882 
        sprintf(msg, "\r\n Value Y: %d not %d-%d !", yscale, scale_min,scale_max);
877 
                UART1_PutString(msg);
 883 
                UART1_PutString(msg);
878 
    }
 884 
    }
879 
        LIMITS(zscale, scale_min, scale_max);
 885 
        LIMITS(zscale, scale_min, scale_max);
880 
        zscale = (ZMax - ZMin)/(2*AVERAGE);
 886 
        zscale = (ZMax - ZMin)/(2*AVERAGE);
881 
        if((zscale > scale_max) || (zscale < scale_min))      
 887 
        if((zscale > scale_max) || (zscale < scale_min))      
882 
        {
 888 
        {
883 
                retval = 0;
 889 
                retval = 0;
884 
        sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
 890 
        sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
885 
                UART1_PutString(msg);
 891 
                UART1_PutString(msg);
886 
    }
 892 
    }
887 
        done = retval;
 893 
        done = retval;
888 
        return(retval);
 894 
        return(retval);
889 
}
 895 
}
890 
 
 896 
 
891 
 
 897 
 
892 
//----------------------------------------------------------------
 898 
//----------------------------------------------------------------
893 
u8 NCMAG_Init(void)
 899 
u8 NCMAG_Init(void)
894 
{
 900 
{
895 
        u8 msg[64];
 901 
        u8 msg[64];
896 
        u8 retval = 0;
 902 
        u8 retval = 0;
897 
        u8 repeat;
 903 
        u8 repeat;
898 
 
 904 
 
899 
        NCMAG_Present = 0;
 905 
        NCMAG_Present = 0;
900 
        NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
 906 
        NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
901 
        // polling for LSM302DLH/DLM option by ACC address ack
 907 
        // polling for LSM302DLH/DLM option by ACC address ack
902 
        repeat = 0;
 908 
        repeat = 0;
903 
        do
 909 
        do
904 
        {
 910 
        {
905 
                retval = NCMAG_GetAccConfig();
 911 
                retval = NCMAG_GetAccConfig();
906 
                if(retval) break; // break loop on success
 912 
                if(retval) break; // break loop on success
907 
                UART1_PutString(".");
 913 
                UART1_PutString(".");
908 
                repeat++;
 914 
                repeat++;
909 
        }while(repeat < 3);
 915 
        }while(repeat < 3);
910 
        if(retval)
 916 
        if(retval)
911 
        {
 917 
        {
912 
                // initialize ACC sensor
 918 
                // initialize ACC sensor
913 
                NCMAG_Init_ACCSensor();
 919 
                NCMAG_Init_ACCSensor();
914 
 
 920 
 
915 
                NCMAG_SensorType = TYPE_LSM303DLH;     
 921 
                NCMAG_SensorType = TYPE_LSM303DLH;     
916 
                // polling of sub identification
 922 
                // polling of sub identification
917 
                repeat = 0;
 923 
                repeat = 0;
918 
                do
 924 
                do
919 
                {
 925 
                {
920 
                        retval = NCMAG_GetIdentification_Sub();
 926 
                        retval = NCMAG_GetIdentification_Sub();
921 
                        if(retval) break; // break loop on success
 927 
                        if(retval) break; // break loop on success
922 
                        UART1_PutString(".");
 928 
                        UART1_PutString(".");
923 
                        repeat++;
 929 
                        repeat++;
924 
                }while(repeat < 12);
 930 
                }while(repeat < 12);
925 
                if(retval)
 931 
                if(retval)
926 
                {
 932 
                {
927 
                        if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM)      NCMAG_SensorType = TYPE_LSM303DLM;
 933 
                        if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM)      NCMAG_SensorType = TYPE_LSM303DLM;
928 
                }      
 934 
                }      
929 
        }
 935 
        }
930 
        // get id bytes
 936 
        // get id bytes
931 
        retval = 0;
 937 
        retval = 0;
932 
        do
 938 
        do
933 
        {
 939 
        {
934 
                retval = NCMAG_GetIdentification();
 940 
                retval = NCMAG_GetIdentification();
935 
                if(retval) break; // break loop on success
 941 
                if(retval) break; // break loop on success
936 
                UART1_PutString(".");
 942 
                UART1_PutString(".");
937 
                repeat++;
 943 
                repeat++;
938 
        }while(repeat < 12);
 944 
        }while(repeat < 12);
939 
 
 945 
 
940 
        // if we got an answer to id request
 946 
        // if we got an answer to id request
941 
        if(retval)
 947 
        if(retval)
942 
        {
 948 
        {
943 
                u8 n1[] = "\n\r HMC5843";
 949 
                u8 n1[] = "\n\r HMC5843";
944 
                u8 n2[] = "\n\r LSM303DLH";
 950 
                u8 n2[] = "\n\r LSM303DLH";
945 
                u8 n3[] = "\n\r LSM303DLM";
 951 
                u8 n3[] = "\n\r LSM303DLM";
946 
                u8* pn = n1;
 952 
                u8* pn = n1;
947 
               
 953 
               
948 
                switch(NCMAG_SensorType)
 954 
                switch(NCMAG_SensorType)
949 
                {
 955 
                {
950 
                        case TYPE_HMC5843:
 956 
                        case TYPE_HMC5843:
951 
                                pn = n1;
 957 
                                pn = n1;
952 
                                break;
 958 
                                break;
953 
                        case TYPE_LSM303DLH:
 959 
                        case TYPE_LSM303DLH:
954 
                                pn = n2;
 960 
                                pn = n2;
955 
                                break;
 961 
                                break;
956 
                        case TYPE_LSM303DLM:
 962 
                        case TYPE_LSM303DLM:
957 
                                pn = n3;
 963 
                                pn = n3;
958 
                                break;
 964 
                                break;
959 
                }
 965 
                }
960 
 
 966 
 
961 
                sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
 967 
                sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
962 
                UART1_PutString(msg);
 968 
                UART1_PutString(msg);
963 
                if (    (NCMAG_Identification.A == MAG_IDA)
 969 
                if (    (NCMAG_Identification.A == MAG_IDA)
964 
                     && (NCMAG_Identification.B == MAG_IDB)
 970 
                     && (NCMAG_Identification.B == MAG_IDB)
965 
                         && (NCMAG_Identification.C == MAG_IDC))
 971 
                         && (NCMAG_Identification.C == MAG_IDC))
966 
                {
 972 
                {
967 
                        NCMAG_Present = 1;
 973 
                        NCMAG_Present = 1;
968 
 
 974 
 
969 
                        if(EEPROM_Init())
 975 
                        if(EEPROM_Init())
970 
                        {
 976 
                        {
971 
                                NCMAG_IsCalibrated = NCMag_CalibrationRead();
 977 
                                NCMAG_IsCalibrated = NCMag_CalibrationRead();
972 
                                if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
 978 
                                if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
973 
                        }
 979 
                        }
974 
                        else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
 980 
                        else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
975 
                        // perform self test
 981 
                        // perform self test
976 
                        if(!NCMAG_SelfTest())
 982 
                        if(!NCMAG_SelfTest())
977 
                        {
 983 
                        {
978 
                                UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
 984 
                                UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
979 
                                LED_RED_ON;
 985 
                                LED_RED_ON;
980 
                                NCMAG_IsCalibrated = 0;
 986 
                                NCMAG_IsCalibrated = 0;
981 
                        }
 987 
                        }
982 
                        else UART1_PutString("\r\n Selftest ok");
 988 
                        else UART1_PutString("\r\n Selftest ok");
983 
 
 989 
 
984 
                        // initialize magnetic sensor configuration
 990 
                        // initialize magnetic sensor configuration
985 
                        InitNC_MagnetSensor();
 991 
                        InitNC_MagnetSensor();
986 
                }
 992 
                }
987 
                else
 993 
                else
988 
                {
 994 
                {
989 
                        UART1_PutString("\n\r Not compatible!");
 995 
                        UART1_PutString("\n\r Not compatible!");
990 
                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
 996 
                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
991 
                        LED_RED_ON;
 997 
                        LED_RED_ON;
992 
                }
 998 
                }
993 
        }
 999 
        }
994 
        else // nothing found
 1000 
        else // nothing found
995 
        {
 1001 
        {
996 
                NCMAG_SensorType = TYPE_NONE;
 1002 
                NCMAG_SensorType = TYPE_NONE;
997 
                UART1_PutString("not found!");  
 1003 
                UART1_PutString("not found!");  
998 
        }
 1004 
        }
999 
        return(NCMAG_Present);
 1005 
        return(NCMAG_Present);
1000 
}
 1006 
}
1001 
 
 1007 
 
1002 
 
 1008