Subversion Repositories NaviCtrl

Rev

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

Rev 394 Rev 395
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 
 
- 
 
 268 
#define ACC_CRTL2_FILTER8       0x00
- 
 
 269 
#define ACC_CRTL2_FILTER16      0x01
- 
 
 270 
#define ACC_CRTL2_FILTER32      0x02
- 
 
 271 
#define ACC_CRTL2_FILTER64      0x03
267 
 
 272 
 
268 
#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)
269 
#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)
270 
#define ACC_CTRL4_FS_2G                 0x00
 275 
#define ACC_CTRL4_FS_2G                 0x00
271 
#define ACC_CTRL4_FS_4G                 0x10
 276 
#define ACC_CTRL4_FS_4G                 0x10
272 
#define ACC_CTRL4_FS_8G                 0x30
 277 
#define ACC_CTRL4_FS_8G                 0x30
273 
#define ACC_CTRL4_STSIGN_PLUS   0x00
 278 
#define ACC_CTRL4_STSIGN_PLUS   0x00
274 
#define ACC_CTRL4_STSIGN_MINUS  0x08
 279 
#define ACC_CTRL4_STSIGN_MINUS  0x08
275 
#define ACC_CTRL4_ST_ENABLE             0x02
 280 
#define ACC_CTRL4_ST_ENABLE             0x02
276 
 
 281 
 
277 
#define ACC_CTRL5_STW_ON                0x03
 282 
#define ACC_CTRL5_STW_ON                0x03
278 
#define ACC_CTRL5_STW_OFF               0x00
 283 
#define ACC_CTRL5_STW_OFF               0x00
279 
 
 284 
 
280 
typedef struct
 285 
typedef struct
281 
{
 286 
{
282 
        u8 ctrl_1;
 287 
        u8 ctrl_1;
283 
        u8 ctrl_2;
 288 
        u8 ctrl_2;
284 
        u8 ctrl_3;
 289 
        u8 ctrl_3;
285 
        u8 ctrl_4;
 290 
        u8 ctrl_4;
286 
        u8 ctrl_5;
 291 
        u8 ctrl_5;
287 
} __attribute__((packed)) AccConfig_t;
 292 
} __attribute__((packed)) AccConfig_t;
288 
 
 293 
 
289 
volatile AccConfig_t AccConfig;
 294 
volatile AccConfig_t AccConfig;
290 
 
 295 
 
291 
u8 NCMag_CalibrationWrite(void)
 296 
u8 NCMag_CalibrationWrite(void)
292 
{
 297 
{
293 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
 298 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
294 
        EEPROM_Result_t eres;
 299 
        EEPROM_Result_t eres;
295 
        u8 *pBuff = (u8*)&Calibration;
 300 
        u8 *pBuff = (u8*)&Calibration;
296 
 
 301 
 
297 
        Calibration.Version = CALIBRATION_VERSION;
 302 
        Calibration.Version = CALIBRATION_VERSION;
298 
        for(i = 0; i<(sizeof(Calibration)-1); i++)
 303 
        for(i = 0; i<(sizeof(Calibration)-1); i++)
299 
        {
 304 
        {
300 
                crc += pBuff[i];        
 305 
                crc += pBuff[i];        
301 
        }
 306 
        }
302 
        Calibration.crc = ~crc;
 307 
        Calibration.crc = ~crc;
303 
        eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration));
 308 
        eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration));
304 
        if(EEPROM_SUCCESS == eres) i = 1;
 309 
        if(EEPROM_SUCCESS == eres) i = 1;
305 
        else i = 0;
 310 
        else i = 0;
306 
        return(i);     
 311 
        return(i);     
307 
}
 312 
}
308 
 
 313 
 
309 
u8 NCMag_CalibrationRead(void)
 314 
u8 NCMag_CalibrationRead(void)
310 
{
 315 
{
311 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
 316 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
312 
        u8 *pBuff = (u8*)&Calibration;
 317 
        u8 *pBuff = (u8*)&Calibration;
313 
 
 318 
 
314 
        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)))
315 
        {
 320 
        {
316 
                for(i = 0; i<(sizeof(Calibration)-1); i++)
 321 
                for(i = 0; i<(sizeof(Calibration)-1); i++)
317 
                {
 322 
                {
318 
                        crc += pBuff[i];        
 323 
                        crc += pBuff[i];        
319 
                }
 324 
                }
320 
                crc = ~crc;
 325 
                crc = ~crc;
321 
                if(Calibration.crc != crc) return(0); // crc mismatch
 326 
                if(Calibration.crc != crc) return(0); // crc mismatch
322 
                if(Calibration.Version == CALIBRATION_VERSION) return(1);
 327 
                if(Calibration.Version == CALIBRATION_VERSION) return(1);
323 
        }
 328 
        }
324 
        return(0);
 329 
        return(0);
325 
}
 330 
}
326 
 
 331 
 
327 
 
 332 
 
328 
void NCMAG_Calibrate(void)
 333 
void NCMAG_Calibrate(void)
329 
{
 334 
{
330 
        u8 msg[64];
 335 
        u8 msg[64];
331 
        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;
332 
        static s16 X = 0, Y = 0, Z = 0;
 337 
        static s16 X = 0, Y = 0, Z = 0;
333 
        static u8 OldCalState = 0;     
 338 
        static u8 OldCalState = 0;     
334 
        s16 MinCalibration = 450;
 339 
        s16 MinCalibration = 450;
335 
 
 340 
 
336 
        X = (4*X + MagRawVector.X + 3)/5;
 341 
        X = (4*X + MagRawVector.X + 3)/5;
337 
        Y = (4*Y + MagRawVector.Y + 3)/5;
 342 
        Y = (4*Y + MagRawVector.Y + 3)/5;
338 
        Z = (4*Z + MagRawVector.Z + 3)/5;
 343 
        Z = (4*Z + MagRawVector.Z + 3)/5;
339 
 
 344 
 
340 
        switch(Compass_CalState)
 345 
        switch(Compass_CalState)
341 
        {
 346 
        {
342 
                case 1:
 347 
                case 1:
343 
                        // 1st step of calibration
 348 
                        // 1st step of calibration
344 
                        // initialize ranges
 349 
                        // initialize ranges
345 
                        // 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
346 
                        Xmin =  10000;
 351 
                        Xmin =  10000;
347 
                        Xmax = -10000;
 352 
                        Xmax = -10000;
348 
                        Ymin =  10000;
 353 
                        Ymin =  10000;
349 
                        Ymax = -10000;
 354 
                        Ymax = -10000;
350 
                        Zmin =  10000;
 355 
                        Zmin =  10000;
351 
                        Zmax = -10000;
 356 
                        Zmax = -10000;
352 
                        break;
 357 
                        break;
353 
               
 358 
               
354 
                case 2: // 2nd step of calibration
 359 
                case 2: // 2nd step of calibration
355 
                        // 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
356 
                        if(X < Xmin)            { Xmin = X; BeepTime = 20;}
 361 
                        if(X < Xmin)            { Xmin = X; BeepTime = 20;}
357 
                        else if(X > Xmax)       { Xmax = X; BeepTime = 20;}
 362 
                        else if(X > Xmax)       { Xmax = X; BeepTime = 20;}
358 
                        if(Y < Ymin)            { Ymin = Y; BeepTime = 60;}
 363 
                        if(Y < Ymin)            { Ymin = Y; BeepTime = 60;}
359 
                        else if(Y > Ymax)       { Ymax = Y; BeepTime = 60;}
 364 
                        else if(Y > Ymax)       { Ymax = Y; BeepTime = 60;}
360 
                        break;
 365 
                        break;
361 
 
 366 
 
362 
                case 3: // 3rd step of calibration
 367 
                case 3: // 3rd step of calibration
363 
                        // 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
364 
                        break;
 369 
                        break;
365 
 
 370 
 
366 
                case 4:
 371 
                case 4:
367 
                        // find Min and Max of the Z-Sensor
 372 
                        // find Min and Max of the Z-Sensor
368 
                        if(Z < Zmin)      { Zmin = Z; BeepTime = 80;}
 373 
                        if(Z < Zmin)      { Zmin = Z; BeepTime = 80;}
369 
                        else if(Z > Zmax) { Zmax = Z; BeepTime = 80;}
 374 
                        else if(Z > Zmax) { Zmax = Z; BeepTime = 80;}
370 
                        break;
 375 
                        break;
371 
               
 376 
               
372 
                case 5:
 377 
                case 5:
373 
                        // Save values
 378 
                        // Save values
374 
                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
 379 
                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
375 
                        {
 380 
                        {
376 
                                switch(NCMAG_SensorType)
 381 
                                switch(NCMAG_SensorType)
377 
                                {
 382 
                                {
378 
                                        case TYPE_HMC5843:
 383 
                                        case TYPE_HMC5843:
379 
                                                UART1_PutString("\r\nHMC5843 calibration\n\r");
 384 
                                                UART1_PutString("\r\nHMC5843 calibration\n\r");
380 
                                                MinCalibration = HMC5843_CALIBRATION_RANGE;
 385 
                                                MinCalibration = HMC5843_CALIBRATION_RANGE;
381 
                                                break;
 386 
                                                break;
382 
 
 387 
 
383 
                                        case TYPE_LSM303DLH:
 388 
                                        case TYPE_LSM303DLH:
384 
                                        case TYPE_LSM303DLM:
 389 
                                        case TYPE_LSM303DLM:
385 
                                                UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
 390 
                                                UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
386 
                                                MinCalibration = LSM303_CALIBRATION_RANGE;
 391 
                                                MinCalibration = LSM303_CALIBRATION_RANGE;
387 
                                        break;
 392 
                                        break;
388 
                                }
 393 
                                }
389 
                                if(EarthMagneticStrengthTheoretic)
 394 
                                if(EarthMagneticStrengthTheoretic)
390 
                                 {
 395 
                                 {
391 
                                  MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
 396 
                                  MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
392 
                                  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);
393 
                                  UART1_PutString(msg);
 398 
                                  UART1_PutString(msg);
394 
                                 }
 399 
                                 }
395 
                            else UART1_PutString("without GPS\n\r");
 400 
                            else UART1_PutString("without GPS\n\r");
396 
 
 401 
 
397 
                                Calibration.MagX.Range = Xmax - Xmin;
 402 
                                Calibration.MagX.Range = Xmax - Xmin;
398 
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
 403 
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
399 
                                Calibration.MagY.Range = Ymax - Ymin;
 404 
                                Calibration.MagY.Range = Ymax - Ymin;
400 
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
 405 
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
401 
                                Calibration.MagZ.Range = Zmax - Zmin;
 406 
                                Calibration.MagZ.Range = Zmax - Zmin;
402 
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
 407 
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
403 
                                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))
404 
                                {
 409 
                                {
405 
                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite();
 410 
                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite();
406 
                                        BeepTime = 2500;
 411 
                                        BeepTime = 2500;
407 
                                        UART1_PutString("\r\n-> Calibration okay <-\n\r");
 412 
                                        UART1_PutString("\r\n-> Calibration okay <-\n\r");
408 
                                }
 413 
                                }
409 
                                else
 414 
                                else
410 
                                {
 415 
                                {
411 
                                        UART1_PutString("\r\nCalibration FAILED - Values too low: ");
 416 
                                        UART1_PutString("\r\nCalibration FAILED - Values too low: ");
412 
                                    if(Calibration.MagX.Range < MinCalibration) UART1_PutString("X! ");
 417 
                                    if(Calibration.MagX.Range < MinCalibration) UART1_PutString("X! ");
413 
                                    if(Calibration.MagY.Range < MinCalibration) UART1_PutString("Y! ");
 418 
                                    if(Calibration.MagY.Range < MinCalibration) UART1_PutString("Y! ");
414 
                                    if(Calibration.MagZ.Range < MinCalibration) UART1_PutString("Z! ");
 419 
                                    if(Calibration.MagZ.Range < MinCalibration) UART1_PutString("Z! ");
415 
                                        UART1_PutString("\r\n");
 420 
                                        UART1_PutString("\r\n");
416 
 
 421 
 
417 
                                        // restore old calibration data from eeprom
 422 
                                        // restore old calibration data from eeprom
418 
                                        NCMAG_IsCalibrated = NCMag_CalibrationRead();
 423 
                                        NCMAG_IsCalibrated = NCMag_CalibrationRead();
419 
                                }
 424 
                                }
420 
                                        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);
421 
                                        UART1_PutString(msg);
 426 
                                        UART1_PutString(msg);
422 
                                        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);
423 
                                        UART1_PutString(msg);
 428 
                                        UART1_PutString(msg);
424 
                                        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);
425 
                                        UART1_PutString(msg);
 430 
                                        UART1_PutString(msg);
426 
                                        sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCalibration);
 431 
                                        sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCalibration);
427 
                                        UART1_PutString(msg);
 432 
                                        UART1_PutString(msg);
428 
                        }
 433 
                        }
429 
                        break;
 434 
                        break;
430 
                       
 435 
                       
431 
                default:
 436 
                default:
432 
                        break; 
 437 
                        break; 
433 
        }
 438 
        }
434 
        OldCalState = Compass_CalState;
 439 
        OldCalState = Compass_CalState;
435 
}
 440 
}
436 
 
 441 
 
437 
// ---------- call back handlers -----------------------------------------
 442 
// ---------- call back handlers -----------------------------------------
438 
 
 443 
 
439 
// rx data handler for id info request
 444 
// rx data handler for id info request
440 
void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
 445 
void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
441 
{       // if number of bytes are matching
 446 
{       // if number of bytes are matching
442 
        if(RxBufferSize == sizeof(NCMAG_Identification) )
 447 
        if(RxBufferSize == sizeof(NCMAG_Identification) )
443 
        {
 448 
        {
444 
                memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
 449 
                memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
445 
        }
 450 
        }
446 
}
 451 
}
447 
 
 452 
 
448 
void NCMAG_UpdateIdentification_Sub(u8* pRxBuffer, u8 RxBufferSize)
 453 
void NCMAG_UpdateIdentification_Sub(u8* pRxBuffer, u8 RxBufferSize)
449 
{       // if number of bytes are matching
 454 
{       // if number of bytes are matching
450 
        if(RxBufferSize == sizeof(NCMAG_Identification2))
 455 
        if(RxBufferSize == sizeof(NCMAG_Identification2))
451 
        {
 456 
        {
452 
                memcpy((u8 *)&NCMAG_Identification2, pRxBuffer, sizeof(NCMAG_Identification2));
 457 
                memcpy((u8 *)&NCMAG_Identification2, pRxBuffer, sizeof(NCMAG_Identification2));
453 
        }
 458 
        }
454 
}
 459 
}
455 
 
 460 
 
456 
// rx data handler for magnetic sensor raw data
 461 
// rx data handler for magnetic sensor raw data
457 
void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
 462 
void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
458 
{       // if number of bytes are matching
 463 
{       // if number of bytes are matching
459 
        if(RxBufferSize == sizeof(MagRawVector) )
 464 
        if(RxBufferSize == sizeof(MagRawVector) )
460 
        {       // byte order from big to little endian
 465 
        {       // byte order from big to little endian
461 
                s16 raw;
 466 
                s16 raw;
462 
                raw = pRxBuffer[0]<<8;
 467 
                raw = pRxBuffer[0]<<8;
463 
                raw+= pRxBuffer[1];
 468 
                raw+= pRxBuffer[1];
464 
                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;
465 
                raw = pRxBuffer[2]<<8;
 470 
                raw = pRxBuffer[2]<<8;
466 
                raw+= pRxBuffer[3];
 471 
                raw+= pRxBuffer[3];
467 
            if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
 472 
            if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
468 
                {
 473 
                {
469 
                        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
470 
                        else                                                                    MagRawVector.Y = raw;
 475 
                        else                                                                    MagRawVector.Y = raw;
471 
                }
 476 
                }
472 
                raw = pRxBuffer[4]<<8;
 477 
                raw = pRxBuffer[4]<<8;
473 
                raw+= pRxBuffer[5];
 478 
                raw+= pRxBuffer[5];
474 
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
 479 
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
475 
                {
 480 
                {
476 
                        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
477 
                        else                                                                    MagRawVector.Z = raw;
 482 
                        else                                                                    MagRawVector.Z = raw;
478 
                }
 483 
                }
479 
        }
 484 
        }
480 
        if(Compass_CalState || !NCMAG_IsCalibrated)
 485 
        if(Compass_CalState || !NCMAG_IsCalibrated)
481 
        {       // mark out data invalid
 486 
        {       // mark out data invalid
482 
                MagVector.X = MagRawVector.X;
 487 
                MagVector.X = MagRawVector.X;
483 
                MagVector.Y = MagRawVector.Y;
 488 
                MagVector.Y = MagRawVector.Y;
484 
                MagVector.Z = MagRawVector.Z;
 489 
                MagVector.Z = MagRawVector.Z;
485 
                Compass_Heading = -1;
 490 
                Compass_Heading = -1;
486 
        }
 491 
        }
487 
        else
 492 
        else
488 
        {
 493 
        {
489 
                // update MagVector from MagRaw Vector by Scaling
 494 
                // update MagVector from MagRaw Vector by Scaling
490 
                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);
491 
                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);
492 
                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);
493 
                Compass_CalcHeading();
 498 
                Compass_CalcHeading();
494 
        }
 499 
        }
495 
}
 500 
}
496 
// rx data handler  for acceleration raw data
 501 
// rx data handler  for acceleration raw data
497 
void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
 502 
void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
498 
{       // if number of byte are matching
 503 
{       // if number of byte are matching
499 
        if(RxBufferSize == sizeof(AccRawVector) )
 504 
        if(RxBufferSize == sizeof(AccRawVector) )
500 
        {
 505 
        {
501 
                memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
 506 
                memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
502 
        }
 507 
        }
503 
        DebugOut.Analog[16] = AccRawVector.X;
 508 
        DebugOut.Analog[16] = AccRawVector.X;
504 
        DebugOut.Analog[17] = AccRawVector.Y;
 509 
        DebugOut.Analog[17] = AccRawVector.Y;
505 
        DebugOut.Analog[18] = AccRawVector.Z;
 510 
        DebugOut.Analog[18] = AccRawVector.Z;
- 
 
 511 
DebugOut.Analog[19] = (DebugOut.Analog[19] * 3 + AccRawVector.Z) / 4;
506 
}
 512 
}
507 
// rx data handler for reading magnetic sensor configuration
 513 
// rx data handler for reading magnetic sensor configuration
508 
void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
 514 
void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
509 
{       // if number of byte are matching
 515 
{       // if number of byte are matching
510 
        if(RxBufferSize == sizeof(MagConfig) )
 516 
        if(RxBufferSize == sizeof(MagConfig) )
511 
        {
 517 
        {
512 
                memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig));
 518 
                memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig));
513 
        }
 519 
        }
514 
}
 520 
}
515 
// rx data handler for reading acceleration sensor configuration
 521 
// rx data handler for reading acceleration sensor configuration
516 
void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize)
 522 
void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize)
517 
{       // if number of byte are matching
 523 
{       // if number of byte are matching
518 
        if(RxBufferSize == sizeof(AccConfig) )
 524 
        if(RxBufferSize == sizeof(AccConfig) )
519 
        {
 525 
        {
520 
                memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
 526 
                memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
521 
        }
 527 
        }
522 
}
 528 
}
523 
//----------------------------------------------------------------------
 529 
//----------------------------------------------------------------------
524 
 
 530 
 
525 
 
 531 
 
526 
// ---------------------------------------------------------------------
 532 
// ---------------------------------------------------------------------
527 
u8 NCMAG_SetMagConfig(void)
 533 
u8 NCMAG_SetMagConfig(void)
528 
{
 534 
{
529 
        u8 retval = 0;
 535 
        u8 retval = 0;
530 
        // try to catch the i2c buffer within 100 ms timeout
 536 
        // try to catch the i2c buffer within 100 ms timeout
531 
        if(I2C_LockBuffer(100))
 537 
        if(I2C_LockBuffer(100))
532 
        {
 538 
        {
533 
                u8 TxBytes = 0;
 539 
                u8 TxBytes = 0;
534 
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;    
 540 
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;    
535 
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&MagConfig, sizeof(MagConfig));
 541 
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&MagConfig, sizeof(MagConfig));
536 
                TxBytes += sizeof(MagConfig);
 542 
                TxBytes += sizeof(MagConfig);
537 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, 0, 0))
 543 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, 0, 0))
538 
                {
 544 
                {
539 
                        if(I2C_WaitForEndOfTransmission(100))
 545 
                        if(I2C_WaitForEndOfTransmission(100))
540 
                        {
 546 
                        {
541 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 547 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
542 
                        }
 548 
                        }
543 
                }
 549 
                }
544 
        }
 550 
        }
545 
        return(retval);        
 551 
        return(retval);        
546 
}
 552 
}
547 
 
 553 
 
548 
// ----------------------------------------------------------------------------------------
 554 
// ----------------------------------------------------------------------------------------
549 
u8 NCMAG_GetMagConfig(void)
 555 
u8 NCMAG_GetMagConfig(void)
550 
{
 556 
{
551 
        u8 retval = 0;
 557 
        u8 retval = 0;
552 
        // try to catch the i2c buffer within 100 ms timeout
 558 
        // try to catch the i2c buffer within 100 ms timeout
553 
        if(I2C_LockBuffer(100))
 559 
        if(I2C_LockBuffer(100))
554 
        {
 560 
        {
555 
                u8 TxBytes = 0;
 561 
                u8 TxBytes = 0;
556 
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;
 562 
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;
557 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
 563 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
558 
                {
 564 
                {
559 
                        if(I2C_WaitForEndOfTransmission(100))
 565 
                        if(I2C_WaitForEndOfTransmission(100))
560 
                        {
 566 
                        {
561 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 567 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
562 
                        }
 568 
                        }
563 
                }
 569 
                }
564 
        }
 570 
        }
565 
        return(retval);        
 571 
        return(retval);        
566 
}
 572 
}
567 
 
 573 
 
568 
// ----------------------------------------------------------------------------------------
 574 
// ----------------------------------------------------------------------------------------
569 
u8 NCMAG_SetAccConfig(void)
 575 
u8 NCMAG_SetAccConfig(void)
570 
{
 576 
{
571 
        u8 retval = 0;
 577 
        u8 retval = 0;
572 
        // try to catch the i2c buffer within 100 ms timeout
 578 
        // try to catch the i2c buffer within 100 ms timeout
573 
        if(I2C_LockBuffer(100))
 579 
        if(I2C_LockBuffer(100))
574 
        {
 580 
        {
575 
                u8 TxBytes = 0;
 581 
                u8 TxBytes = 0;
576 
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;      
 582 
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;      
577 
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
 583 
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
578 
                TxBytes += sizeof(AccConfig);
 584 
                TxBytes += sizeof(AccConfig);
579 
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
 585 
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
580 
                {
 586 
                {
581 
                        if(I2C_WaitForEndOfTransmission(100))
 587 
                        if(I2C_WaitForEndOfTransmission(100))
582 
                        {
 588 
                        {
583 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 589 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
584 
                        }
 590 
                        }
585 
                }
 591 
                }
586 
        }
 592 
        }
587 
        return(retval);        
 593 
        return(retval);        
588 
}
 594 
}
589 
 
 595 
 
590 
// ----------------------------------------------------------------------------------------
 596 
// ----------------------------------------------------------------------------------------
591 
u8 NCMAG_GetAccConfig(void)
 597 
u8 NCMAG_GetAccConfig(void)
592 
{
 598 
{
593 
        u8 retval = 0;
 599 
        u8 retval = 0;
594 
        // try to catch the i2c buffer within 100 ms timeout
 600 
        // try to catch the i2c buffer within 100 ms timeout
595 
        if(I2C_LockBuffer(100))
 601 
        if(I2C_LockBuffer(100))
596 
        {
 602 
        {
597 
                u8 TxBytes = 0;
 603 
                u8 TxBytes = 0;
598 
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
 604 
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
599 
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
 605 
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
600 
                {
 606 
                {
601 
                        if(I2C_WaitForEndOfTransmission(100))
 607 
                        if(I2C_WaitForEndOfTransmission(100))
602 
                        {
 608 
                        {
603 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 609 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
604 
                        }
 610 
                        }
605 
                }
 611 
                }
606 
        }
 612 
        }
607 
        return(retval);        
 613 
        return(retval);        
608 
}
 614 
}
609 
 
 615 
 
610 
// ----------------------------------------------------------------------------------------
 616 
// ----------------------------------------------------------------------------------------
611 
u8 NCMAG_GetIdentification(void)
 617 
u8 NCMAG_GetIdentification(void)
612 
{
 618 
{
613 
        u8 retval = 0;
 619 
        u8 retval = 0;
614 
        // try to catch the i2c buffer within 100 ms timeout
 620 
        // try to catch the i2c buffer within 100 ms timeout
615 
        if(I2C_LockBuffer(100))
 621 
        if(I2C_LockBuffer(100))
616 
        {
 622 
        {
617 
                u16 TxBytes = 0;
 623 
                u16 TxBytes = 0;
618 
                NCMAG_Identification.A = 0xFF;
 624 
                NCMAG_Identification.A = 0xFF;
619 
                NCMAG_Identification.B = 0xFF;
 625 
                NCMAG_Identification.B = 0xFF;
620 
                NCMAG_Identification.C = 0xFF;
 626 
                NCMAG_Identification.C = 0xFF;
621 
                I2C_Buffer[TxBytes++] = REG_MAG_IDA;
 627 
                I2C_Buffer[TxBytes++] = REG_MAG_IDA;
622 
                // initiate transmission
 628 
                // initiate transmission
623 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
 629 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
624 
                {
 630 
                {
625 
                        if(I2C_WaitForEndOfTransmission(100))
 631 
                        if(I2C_WaitForEndOfTransmission(100))
626 
                        {
 632 
                        {
627 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 633 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
628 
                        }
 634 
                        }
629 
                }
 635 
                }
630 
        }
 636 
        }
631 
        return(retval);
 637 
        return(retval);
632 
}
 638 
}
633 
 
 639 
 
634 
u8 NCMAG_GetIdentification_Sub(void)
 640 
u8 NCMAG_GetIdentification_Sub(void)
635 
{
 641 
{
636 
        u8 retval = 0;
 642 
        u8 retval = 0;
637 
        // try to catch the i2c buffer within 100 ms timeout
 643 
        // try to catch the i2c buffer within 100 ms timeout
638 
        if(I2C_LockBuffer(100))
 644 
        if(I2C_LockBuffer(100))
639 
        {
 645 
        {
640 
                u16 TxBytes = 0;
 646 
                u16 TxBytes = 0;
641 
                NCMAG_Identification2.Sub = 0xFF;
 647 
                NCMAG_Identification2.Sub = 0xFF;
642 
                I2C_Buffer[TxBytes++] = REG_MAG_IDF;
 648 
                I2C_Buffer[TxBytes++] = REG_MAG_IDF;
643 
                // initiate transmission
 649 
                // initiate transmission
644 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
 650 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
645 
                {
 651 
                {
646 
                        if(I2C_WaitForEndOfTransmission(100))
 652 
                        if(I2C_WaitForEndOfTransmission(100))
647 
                        {
 653 
                        {
648 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 654 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
649 
                        }
 655 
                        }
650 
                }
 656 
                }
651 
        }
 657 
        }
652 
        return(retval);
 658 
        return(retval);
653 
}
 659 
}
654 
 
 660 
 
655 
 
 661 
 
656 
// ----------------------------------------------------------------------------------------
 662 
// ----------------------------------------------------------------------------------------
657 
void NCMAG_GetMagVector(void)
 663 
void NCMAG_GetMagVector(void)
658 
{
 664 
{
659 
        // try to catch the I2C buffer within 0 ms
 665 
        // try to catch the I2C buffer within 0 ms
660 
        if(I2C_LockBuffer(0))
 666 
        if(I2C_LockBuffer(0))
661 
        {
 667 
        {
662 
                u16 TxBytes = 0;
 668 
                u16 TxBytes = 0;
663 
                // set register pointer
 669 
                // set register pointer
664 
                I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB;
 670 
                I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB;
665 
                // initiate transmission
 671 
                // initiate transmission
666 
                I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
 672 
                I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
667 
        }
 673 
        }
668 
}
 674 
}
669 
 
 675 
 
670 
//----------------------------------------------------------------
 676 
//----------------------------------------------------------------
671 
void NCMAG_GetAccVector(void)
 677 
void NCMAG_GetAccVector(void)
672 
{
 678 
{
673 
        // try to catch the I2C buffer within 0 ms
 679 
        // try to catch the I2C buffer within 0 ms
674 
        if(I2C_LockBuffer(0))
 680 
        if(I2C_LockBuffer(0))
675 
        {
 681 
        {
676 
                u16 TxBytes = 0;
 682 
                u16 TxBytes = 0;
677 
                // set register pointer
 683 
                // set register pointer
678 
                I2C_Buffer[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT;
 684 
                I2C_Buffer[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT;
679 
                // initiate transmission
 685 
                // initiate transmission
680 
                I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
 686 
                I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
681 
        }
 687 
        }
682 
}
 688 
}
683 
 
 689 
 
684 
//----------------------------------------------------------------
 690 
//----------------------------------------------------------------
685 
u8 InitNC_MagnetSensor(void)
 691 
u8 InitNC_MagnetSensor(void)
686 
{
 692 
{
687 
        u8 crb_gain, cra_rate;
 693 
        u8 crb_gain, cra_rate;
688 
 
 694 
 
689 
        switch(NCMAG_SensorType)
 695 
        switch(NCMAG_SensorType)
690 
        {
 696 
        {
691 
                case TYPE_HMC5843:
 697 
                case TYPE_HMC5843:
692 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
 698 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
693 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
 699 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
694 
                        break;
 700 
                        break;
695 
 
 701 
 
696 
                case TYPE_LSM303DLH:
 702 
                case TYPE_LSM303DLH:
697 
                case TYPE_LSM303DLM:
 703 
                case TYPE_LSM303DLM:
698 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
 704 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
699 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
 705 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
700 
                        break;
 706 
                        break;
701 
 
 707 
 
702 
                default:
 708 
                default:
703 
                return(0);
 709 
                return(0);
704 
        }
 710 
        }
705 
 
 711 
 
706 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
 712 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
707 
        MagConfig.crb = crb_gain;
 713 
        MagConfig.crb = crb_gain;
708 
        MagConfig.mode = MODE_CONTINUOUS;
 714 
        MagConfig.mode = MODE_CONTINUOUS;
709 
        return(NCMAG_SetMagConfig());
 715 
        return(NCMAG_SetMagConfig());
710 
}
 716 
}
- 
 
 717 
 
711 
 
 718 
 
712 
//----------------------------------------------------------------
 719 
//----------------------------------------------------------------
713 
u8 NCMAG_Init_ACCSensor(void)
 720 
u8 NCMAG_Init_ACCSensor(void)
714 
{
 721 
{
715 
        AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_400HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN;
 722 
        AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN;
716 
        AccConfig.ctrl_2 = 0x00;
 723 
        AccConfig.ctrl_2 = ACC_CRTL2_FILTER64;
717 
        AccConfig.ctrl_3 = 0x00;
 724 
        AccConfig.ctrl_3 = 0x00;
718 
        AccConfig.ctrl_4 = ACC_CTRL4_BDU|ACC_CTRL4_FS_2G;
 725 
        AccConfig.ctrl_4 = ACC_CTRL4_BDU|ACC_CTRL4_FS_8G;
719 
        AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF;
 726 
        AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF;
720 
        return(NCMAG_SetAccConfig());
 727 
        return(NCMAG_SetAccConfig());
721 
}
 728 
}
722 
// --------------------------------------------------------
 729 
// --------------------------------------------------------
723 
void NCMAG_Update(void)
 730 
void NCMAG_Update(void)
724 
{
 731 
{
725 
        static u32 TimerUpdate = 0;
 732 
        static u32 TimerUpdate = 0;
726 
        static u8 send_config = 0;
 733 
        static u8 send_config = 0;
727 
        u32 delay = 20;
 734 
        u32 delay = 20;
728 
 
 735 
 
729 
        if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
 736 
        if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
730 
        {
 737 
        {
731 
                Compass_Heading = -1;
 738 
                Compass_Heading = -1;
732 
                DebugOut.Analog[14]++; // count I2C error
 739 
                DebugOut.Analog[14]++; // count I2C error
733 
                return;
 740 
                return;
734 
        }
 741 
        }
735 
        if(CheckDelay(TimerUpdate))
 742 
        if(CheckDelay(TimerUpdate))
736 
        {
 743 
        {
737 
                if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
 744 
                if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
738 
        if(++send_config == 25)   // 500ms
 745 
        if(++send_config == 25)   // 500ms
739 
                {
 746 
                {
740 
                        send_config = 0;
 747 
                        send_config = 0;
741 
                InitNC_MagnetSensor();
 748 
                InitNC_MagnetSensor();
742 
                        TimerUpdate = SetDelay(15);    // back into the old time-slot
 749 
                        TimerUpdate = SetDelay(15);    // back into the old time-slot
743 
                }
 750 
                }
744 
                else
 751 
                else
745 
                {
 752 
                {
746 
                        static u8 s = 0;
 753 
                        static u8 s = 0;
747 
                        // check for new calibration state
 754 
                        // check for new calibration state
748 
                        Compass_UpdateCalState();
 755 
                        Compass_UpdateCalState();
749 
                        if(Compass_CalState) NCMAG_Calibrate();
 756 
                        if(Compass_CalState) NCMAG_Calibrate();
750 
                       
 757 
                       
751 
                        // in case of LSM303 type
 758 
                        // in case of LSM303 type
752 
                        switch(NCMAG_SensorType)
 759 
                        switch(NCMAG_SensorType)
753 
                        {
 760 
                        {
754 
                                case TYPE_HMC5843:                             
 761 
                                case TYPE_HMC5843:                             
755 
                                        NCMAG_GetMagVector();
 762 
                                        NCMAG_GetMagVector();
756 
                                        delay = 20;
 763 
                                        delay = 20;
757 
                                        break;
 764 
                                        break;
758 
                                case TYPE_LSM303DLH:
 765 
                                case TYPE_LSM303DLH:
759 
                                case TYPE_LSM303DLM:
 766 
                                case TYPE_LSM303DLM:
760 
                                        if(s){ NCMAG_GetMagVector(); s = 0;}
 767 
                                        if(s){ NCMAG_GetMagVector(); s = 0;}
761 
                                        else { NCMAG_GetAccVector(); s = 1;}
 768 
                                        else { NCMAG_GetAccVector(); s = 1;}
762 
                                        delay = 10;
 769 
                                        delay = 10;
763 
                                        break;                           
 770 
                                        break;                           
764 
                        }
 771 
                        }
765 
                        if(send_config == 24) TimerUpdate = SetDelay(5);    // next event is the re-configuration
 772 
                        if(send_config == 24) TimerUpdate = SetDelay(5);    // next event is the re-configuration
766 
                        else TimerUpdate = SetDelay(delay);    // every 20 ms are 50 Hz
 773 
                        else TimerUpdate = SetDelay(delay);    // every 20 ms are 50 Hz
767 
                }
 774 
                }
768 
        }
 775 
        }
769 
}
 776 
}
770 
 
 777 
 
771 
 
 778 
 
772 
// --------------------------------------------------------
 779 
// --------------------------------------------------------
773 
u8 NCMAG_SelfTest(void)
 780 
u8 NCMAG_SelfTest(void)
774 
{
 781 
{
775 
        u8 msg[64];
 782 
        u8 msg[64];
776 
        static u8 done = 0;
 783 
        static u8 done = 0;
777 
 
 784 
 
778 
        if(done) return(1);        // just make it once
 785 
        if(done) return(1);        // just make it once
779 
       
 786 
       
780 
        #define LIMITS(value, min, max) {min = (80 * value)/100; max = (120 * value)/100;}
 787 
        #define LIMITS(value, min, max) {min = (80 * value)/100; max = (120 * value)/100;}
781 
        u32 time;
 788 
        u32 time;
782 
        s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
 789 
        s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
783 
        s16 xscale, yscale, zscale, scale_min, scale_max;
 790 
        s16 xscale, yscale, zscale, scale_min, scale_max;
784 
        u8 crb_gain, cra_rate;
 791 
        u8 crb_gain, cra_rate;
785 
        u8 i = 0, retval = 1;
 792 
        u8 i = 0, retval = 1;
786 
 
 793 
 
787 
        switch(NCMAG_SensorType)
 794 
        switch(NCMAG_SensorType)
788 
        {
 795 
        {
789 
                case TYPE_HMC5843:
 796 
                case TYPE_HMC5843:
790 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
 797 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
791 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
 798 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
792 
                        xscale = HMC5843_TEST_XSCALE;
 799 
                        xscale = HMC5843_TEST_XSCALE;
793 
                        yscale = HMC5843_TEST_YSCALE;
 800 
                        yscale = HMC5843_TEST_YSCALE;
794 
                        zscale = HMC5843_TEST_ZSCALE;
 801 
                        zscale = HMC5843_TEST_ZSCALE;
795 
                        break;
 802 
                        break;
796 
 
 803 
 
797 
                case TYPE_LSM303DLH:
 804 
                case TYPE_LSM303DLH:
798 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
 805 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
799 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
 806 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
800 
                        xscale = LSM303DLH_TEST_XSCALE;
 807 
                        xscale = LSM303DLH_TEST_XSCALE;
801 
                        yscale = LSM303DLH_TEST_YSCALE;
 808 
                        yscale = LSM303DLH_TEST_YSCALE;
802 
                        zscale = LSM303DLH_TEST_ZSCALE;
 809 
                        zscale = LSM303DLH_TEST_ZSCALE;
803 
                        break;
 810 
                        break;
804 
 
 811 
 
805 
                case TYPE_LSM303DLM:
 812 
                case TYPE_LSM303DLM:
806 
                        // does not support self test feature
813 
                        // does not support self test feature
807 
                        done = retval;
 814 
                        done = retval;
808 
                        return(retval);
 815 
                        return(retval);
809 
                        break;
 816 
                        break;
810 
 
 817 
 
811 
                default:
 818 
                default:
812 
                        return(0);
 819 
                        return(0);
813 
        }
 820 
        }
814 
 
 821 
 
815 
        MagConfig.cra = cra_rate|CRA_MODE_POSBIAS;
 822 
        MagConfig.cra = cra_rate|CRA_MODE_POSBIAS;
816 
        MagConfig.crb = crb_gain;
 823 
        MagConfig.crb = crb_gain;
817 
        MagConfig.mode = MODE_CONTINUOUS;
 824 
        MagConfig.mode = MODE_CONTINUOUS;
818 
        // activate positive bias field
 825 
        // activate positive bias field
819 
        NCMAG_SetMagConfig();
 826 
        NCMAG_SetMagConfig();
820 
        // wait for stable readings
 827 
        // wait for stable readings
821 
        time = SetDelay(50);
 828 
        time = SetDelay(50);
822 
        while(!CheckDelay(time));
 829 
        while(!CheckDelay(time));
823 
        // averaging
 830 
        // averaging
824 
        #define AVERAGE 20
 831 
        #define AVERAGE 20
825 
        for(i = 0; i<AVERAGE; i++)
 832 
        for(i = 0; i<AVERAGE; i++)
826 
        {
 833 
        {
827 
                NCMAG_GetMagVector();
 834 
                NCMAG_GetMagVector();
828 
                time = SetDelay(20);
 835 
                time = SetDelay(20);
829 
        while(!CheckDelay(time));
 836 
        while(!CheckDelay(time));
830 
                XMax += MagRawVector.X;
 837 
                XMax += MagRawVector.X;
831 
                YMax += MagRawVector.Y;
 838 
                YMax += MagRawVector.Y;
832 
                ZMax += MagRawVector.Z;
 839 
                ZMax += MagRawVector.Z;
833 
        }
 840 
        }
834 
        MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS;
 841 
        MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS;
835 
        // activate positive bias field
 842 
        // activate positive bias field
836 
        NCMAG_SetMagConfig();
 843 
        NCMAG_SetMagConfig();
837 
    // wait for stable readings
 844 
    // wait for stable readings
838 
        time = SetDelay(50);
 845 
        time = SetDelay(50);
839 
        while(!CheckDelay(time));
 846 
        while(!CheckDelay(time));
840 
        // averaging
 847 
        // averaging
841 
        for(i = 0; i < AVERAGE; i++)
 848 
        for(i = 0; i < AVERAGE; i++)
842 
        {
 849 
        {
843 
                NCMAG_GetMagVector();
 850 
                NCMAG_GetMagVector();
844 
                time = SetDelay(20);
 851 
                time = SetDelay(20);
845 
        while(!CheckDelay(time));
 852 
        while(!CheckDelay(time));
846 
                XMin += MagRawVector.X;
 853 
                XMin += MagRawVector.X;
847 
                YMin += MagRawVector.Y;
 854 
                YMin += MagRawVector.Y;
848 
                ZMin += MagRawVector.Z;
 855 
                ZMin += MagRawVector.Z;
849 
        }
 856 
        }
850 
        // setup final configuration
 857 
        // setup final configuration
851 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
 858 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
852 
        // activate positive bias field
 859 
        // activate positive bias field
853 
        NCMAG_SetMagConfig();
 860 
        NCMAG_SetMagConfig();
854 
        // check scale for all axes
 861 
        // check scale for all axes
855 
        // prepare scale limits
 862 
        // prepare scale limits
856 
        LIMITS(xscale, scale_min, scale_max);
 863 
        LIMITS(xscale, scale_min, scale_max);
857 
        xscale = (XMax - XMin)/(2*AVERAGE);
 864 
        xscale = (XMax - XMin)/(2*AVERAGE);
858 
        if((xscale > scale_max) || (xscale < scale_min))
 865 
        if((xscale > scale_max) || (xscale < scale_min))
859 
    {
 866 
    {
860 
                retval = 0;
 867 
                retval = 0;
861 
        sprintf(msg, "\r\n Value X: %d not %d-%d !", xscale, scale_min,scale_max);
 868 
        sprintf(msg, "\r\n Value X: %d not %d-%d !", xscale, scale_min,scale_max);
862 
                UART1_PutString(msg);
 869 
                UART1_PutString(msg);
863 
    }
 870 
    }
864 
        LIMITS(yscale, scale_min, scale_max);
 871 
        LIMITS(yscale, scale_min, scale_max);
865 
        yscale = (YMax - YMin)/(2*AVERAGE);
 872 
        yscale = (YMax - YMin)/(2*AVERAGE);
866 
        if((yscale > scale_max) || (yscale < scale_min))
 873 
        if((yscale > scale_max) || (yscale < scale_min))
867 
    {
 874 
    {
868 
                retval = 0;
 875 
                retval = 0;
869 
        sprintf(msg, "\r\n Value Y: %d not %d-%d !", yscale, scale_min,scale_max);
 876 
        sprintf(msg, "\r\n Value Y: %d not %d-%d !", yscale, scale_min,scale_max);
870 
                UART1_PutString(msg);
 877 
                UART1_PutString(msg);
871 
    }
 878 
    }
872 
        LIMITS(zscale, scale_min, scale_max);
 879 
        LIMITS(zscale, scale_min, scale_max);
873 
        zscale = (ZMax - ZMin)/(2*AVERAGE);
 880 
        zscale = (ZMax - ZMin)/(2*AVERAGE);
874 
        if((zscale > scale_max) || (zscale < scale_min))      
 881 
        if((zscale > scale_max) || (zscale < scale_min))      
875 
        {
 882 
        {
876 
                retval = 0;
 883 
                retval = 0;
877 
        sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
 884 
        sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
878 
                UART1_PutString(msg);
 885 
                UART1_PutString(msg);
879 
    }
 886 
    }
880 
        done = retval;
 887 
        done = retval;
881 
        return(retval);
 888 
        return(retval);
882 
}
 889 
}
883 
 
 890 
 
884 
 
 891 
 
885 
//----------------------------------------------------------------
 892 
//----------------------------------------------------------------
886 
u8 NCMAG_Init(void)
 893 
u8 NCMAG_Init(void)
887 
{
 894 
{
888 
        u8 msg[64];
 895 
        u8 msg[64];
889 
        u8 retval = 0;
 896 
        u8 retval = 0;
890 
        u8 repeat;
 897 
        u8 repeat;
891 
 
 898 
 
892 
        NCMAG_Present = 0;
 899 
        NCMAG_Present = 0;
893 
        NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
 900 
        NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
894 
        // polling for LSM302DLH/DLM option by ACC address ack
 901 
        // polling for LSM302DLH/DLM option by ACC address ack
895 
        repeat = 0;
 902 
        repeat = 0;
896 
        do
 903 
        do
897 
        {
 904 
        {
898 
                retval = NCMAG_GetAccConfig();
 905 
                retval = NCMAG_GetAccConfig();
899 
                if(retval) break; // break loop on success
 906 
                if(retval) break; // break loop on success
900 
                UART1_PutString(".");
 907 
                UART1_PutString(".");
901 
                repeat++;
 908 
                repeat++;
902 
        }while(repeat < 3);
 909 
        }while(repeat < 3);
903 
        if(retval)
 910 
        if(retval)
904 
        {
 911 
        {
905 
                // initialize ACC sensor
 912 
                // initialize ACC sensor
906 
                NCMAG_Init_ACCSensor();
 913 
                NCMAG_Init_ACCSensor();
907 
 
 914 
 
908 
                NCMAG_SensorType = TYPE_LSM303DLH;     
 915 
                NCMAG_SensorType = TYPE_LSM303DLH;     
909 
                // polling of sub identification
 916 
                // polling of sub identification
910 
                repeat = 0;
 917 
                repeat = 0;
911 
                do
 918 
                do
912 
                {
 919 
                {
913 
                        retval = NCMAG_GetIdentification_Sub();
 920 
                        retval = NCMAG_GetIdentification_Sub();
914 
                        if(retval) break; // break loop on success
 921 
                        if(retval) break; // break loop on success
915 
                        UART1_PutString(".");
 922 
                        UART1_PutString(".");
916 
                        repeat++;
 923 
                        repeat++;
917 
                }while(repeat < 12);
 924 
                }while(repeat < 12);
918 
                if(retval)
 925 
                if(retval)
919 
                {
 926 
                {
920 
                        if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM)      NCMAG_SensorType = TYPE_LSM303DLM;
 927 
                        if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM)      NCMAG_SensorType = TYPE_LSM303DLM;
921 
                }      
 928 
                }      
922 
        }
 929 
        }
923 
        // get id bytes
 930 
        // get id bytes
924 
        retval = 0;
 931 
        retval = 0;
925 
        do
 932 
        do
926 
        {
 933 
        {
927 
                retval = NCMAG_GetIdentification();
 934 
                retval = NCMAG_GetIdentification();
928 
                if(retval) break; // break loop on success
 935 
                if(retval) break; // break loop on success
929 
                UART1_PutString(".");
 936 
                UART1_PutString(".");
930 
                repeat++;
 937 
                repeat++;
931 
        }while(repeat < 12);
 938 
        }while(repeat < 12);
932 
 
 939 
 
933 
        // if we got an answer to id request
 940 
        // if we got an answer to id request
934 
        if(retval)
 941 
        if(retval)
935 
        {
 942 
        {
936 
                u8 n1[] = "\n\r HMC5843";
 943 
                u8 n1[] = "\n\r HMC5843";
937 
                u8 n2[] = "\n\r LSM303DLH";
 944 
                u8 n2[] = "\n\r LSM303DLH";
938 
                u8 n3[] = "\n\r LSM303DLM";
 945 
                u8 n3[] = "\n\r LSM303DLM";
939 
                u8* pn = n1;
 946 
                u8* pn = n1;
940 
               
 947 
               
941 
                switch(NCMAG_SensorType)
 948 
                switch(NCMAG_SensorType)
942 
                {
 949 
                {
943 
                        case TYPE_HMC5843:
 950 
                        case TYPE_HMC5843:
944 
                                pn = n1;
 951 
                                pn = n1;
945 
                                break;
 952 
                                break;
946 
                        case TYPE_LSM303DLH:
 953 
                        case TYPE_LSM303DLH:
947 
                                pn = n2;
 954 
                                pn = n2;
948 
                                break;
 955 
                                break;
949 
                        case TYPE_LSM303DLM:
 956 
                        case TYPE_LSM303DLM:
950 
                                pn = n3;
 957 
                                pn = n3;
951 
                                break;
 958 
                                break;
952 
                }
 959 
                }
953 
 
 960 
 
954 
                sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
 961 
                sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
955 
                UART1_PutString(msg);
 962 
                UART1_PutString(msg);
956 
                if (    (NCMAG_Identification.A == MAG_IDA)
 963 
                if (    (NCMAG_Identification.A == MAG_IDA)
957 
                     && (NCMAG_Identification.B == MAG_IDB)
 964 
                     && (NCMAG_Identification.B == MAG_IDB)
958 
                         && (NCMAG_Identification.C == MAG_IDC))
 965 
                         && (NCMAG_Identification.C == MAG_IDC))
959 
                {
 966 
                {
960 
                        NCMAG_Present = 1;
 967 
                        NCMAG_Present = 1;
961 
 
 968 
 
962 
                        if(EEPROM_Init())
 969 
                        if(EEPROM_Init())
963 
                        {
 970 
                        {
964 
                                NCMAG_IsCalibrated = NCMag_CalibrationRead();
 971 
                                NCMAG_IsCalibrated = NCMag_CalibrationRead();
965 
                                if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
 972 
                                if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
966 
                        }
 973 
                        }
967 
                        else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
 974 
                        else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
968 
                        // perform self test
 975 
                        // perform self test
969 
                        if(!NCMAG_SelfTest())
 976 
                        if(!NCMAG_SelfTest())
970 
                        {
 977 
                        {
971 
                                UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
 978 
                                UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
972 
                                LED_RED_ON;
 979 
                                LED_RED_ON;
973 
                                NCMAG_IsCalibrated = 0;
 980 
                                NCMAG_IsCalibrated = 0;
974 
                        }
 981 
                        }
975 
                        else UART1_PutString("\r\n Selftest ok");
 982 
                        else UART1_PutString("\r\n Selftest ok");
976 
 
 983 
 
977 
                        // initialize magnetic sensor configuration
 984 
                        // initialize magnetic sensor configuration
978 
                        InitNC_MagnetSensor();
 985 
                        InitNC_MagnetSensor();
979 
                }
 986 
                }
980 
                else
 987 
                else
981 
                {
 988 
                {
982 
                        UART1_PutString("\n\r Not compatible!");
 989 
                        UART1_PutString("\n\r Not compatible!");
983 
                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
 990 
                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
984 
                        LED_RED_ON;
 991 
                        LED_RED_ON;
985 
                }
 992 
                }
986 
        }
 993 
        }
987 
        else // nothing found
 994 
        else // nothing found
988 
        {
 995 
        {
989 
                NCMAG_SensorType = TYPE_NONE;
 996 
                NCMAG_SensorType = TYPE_NONE;
990 
                UART1_PutString("not found!");  
 997 
                UART1_PutString("not found!");  
991 
        }
 998 
        }
992 
        return(NCMAG_Present);
 999 
        return(NCMAG_Present);
993 
}
 1000 
}
994 
 
 1001 
 
995 
 
 1002