Subversion Repositories NaviCtrl

Rev

Rev 454 | Rev 469 | Go to most recent revision | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 454 Rev 465
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 "i2c0.h"
- 
 
 63 
 
62 
#include "timer1.h"
 64 
#include "timer1.h"
63 
#include "led.h"
 65 
#include "led.h"
64 
#include "uart1.h"
 66 
#include "uart1.h"
65 
#include "eeprom.h"
 67 
#include "eeprom.h"
66 
#include "mymath.h"
 68 
#include "mymath.h"
67 
#include "main.h"
 69 
#include "main.h"
68 
#include "spi_slave.h"
 70 
#include "spi_slave.h"
69 
 
 71 
 
70 
u8 NCMAG_Present = 0;
 72 
u8 NCMAG_Present = 0;
71 
u8 NCMAG_IsCalibrated = 0;
 73 
u8 NCMAG_IsCalibrated = 0;
- 
 
 74 
 
- 
 
 75 
u8 *I2C_BufferPnt;
- 
 
 76 
u8 *I2C_ErrorPnt;
- 
 
 77 
I2C_TransmissionFunc_t                          I2C_TransmissionFunc;
- 
 
 78 
I2C_LockBufferFunc_t                            I2C_LockBufferFunc;
- 
 
 79 
I2C_WaitForEndOfTransmissionFunc_t      I2C_WaitForEndOfTransmissionFunc;
- 
 
 80 
 
72 
 
 81 
 
73 
// supported magnetic sensor types
 82 
// supported magnetic sensor types
74 
#define TYPE_NONE                       0
 83 
#define TYPE_NONE                       0
75 
#define TYPE_HMC5843            1
 84 
#define TYPE_HMC5843            1
76 
#define TYPE_LSM303DLH          2
 85 
#define TYPE_LSM303DLH          2
77 
#define TYPE_LSM303DLM          3
 86 
#define TYPE_LSM303DLM          3
78 
 
 87 
 
79 
u8 NCMAG_SensorType = TYPE_NONE;
 88 
u8 NCMAG_SensorType = TYPE_NONE;
80 
 
 89 
 
81 
#define EEPROM_ADR_MAG_CALIBRATION              50
 90 
#define EEPROM_ADR_MAG_CALIBRATION              50
82 
#define CALIBRATION_VERSION                     1
 91 
#define CALIBRATION_VERSION                     1
83 
#define MAG_CALIBRATION_COMPATIBLE              0xA2
 92 
#define MAG_CALIBRATION_COMPATIBLE              0xA2
84 
 
 93 
 
85 
#define NCMAG_MIN_RAWVALUE -2047
 94 
#define NCMAG_MIN_RAWVALUE -2047
86 
#define NCMAG_MAX_RAWVALUE  2047
 95 
#define NCMAG_MAX_RAWVALUE  2047
87 
#define NCMAG_INVALID_DATA -4096
 96 
#define NCMAG_INVALID_DATA -4096
88 
 
 97 
 
89 
typedef struct
 98 
typedef struct
90 
{
 99 
{
91 
        s16 Range;
 100 
        s16 Range;
92 
        s16 Offset;
 101 
        s16 Offset;
93 
} __attribute__((packed)) Scaling_t;
 102 
} __attribute__((packed)) Scaling_t;
94 
 
 103 
 
95 
typedef struct
 104 
typedef struct
96 
{
 105 
{
97 
        Scaling_t MagX;
 106 
        Scaling_t MagX;
98 
        Scaling_t MagY;
 107 
        Scaling_t MagY;
99 
        Scaling_t MagZ;
 108 
        Scaling_t MagZ;
100 
        u8 Version;
 109 
        u8 Version;
101 
        u8 crc;
 110 
        u8 crc;
102 
} __attribute__((packed)) Calibration_t;
 111 
} __attribute__((packed)) Calibration_t;
103 
 
 112 
 
104 
Calibration_t Calibration;              // calibration data in RAM
113 
Calibration_t Calibration;              // calibration data in RAM
105 
volatile s16vec_t AccRawVector;
 114 
volatile s16vec_t AccRawVector;
106 
volatile s16vec_t MagRawVector;
 115 
volatile s16vec_t MagRawVector;
107 
 
 116 
 
108 
// i2c MAG interface
 117 
// i2c MAG interface
109 
#define MAG_SLAVE_ADDRESS       0x3C    // i2C slave address mag. sensor registers
 118 
#define MAG_SLAVE_ADDRESS       0x3C    // i2C slave address mag. sensor registers
110 
 
 119 
 
111 
// register mapping
 120 
// register mapping
112 
#define REG_MAG_CRA                     0x00
 121 
#define REG_MAG_CRA                     0x00
113 
#define REG_MAG_CRB                     0x01
 122 
#define REG_MAG_CRB                     0x01
114 
#define REG_MAG_MODE            0x02
 123 
#define REG_MAG_MODE            0x02
115 
#define REG_MAG_DATAX_MSB       0x03
 124 
#define REG_MAG_DATAX_MSB       0x03
116 
#define REG_MAG_DATAX_LSB       0x04
 125 
#define REG_MAG_DATAX_LSB       0x04
117 
#define REG_MAG_DATAY_MSB       0x05
 126 
#define REG_MAG_DATAY_MSB       0x05
118 
#define REG_MAG_DATAY_LSB       0x06
 127 
#define REG_MAG_DATAY_LSB       0x06
119 
#define REG_MAG_DATAZ_MSB       0x07
 128 
#define REG_MAG_DATAZ_MSB       0x07
120 
#define REG_MAG_DATAZ_LSB       0x08
 129 
#define REG_MAG_DATAZ_LSB       0x08
121 
#define REG_MAG_STATUS          0x09
 130 
#define REG_MAG_STATUS          0x09
122 
 
 131 
 
123 
#define REG_MAG_IDA                     0x0A
 132 
#define REG_MAG_IDA                     0x0A
124 
#define REG_MAG_IDB                     0x0B
 133 
#define REG_MAG_IDB                     0x0B
125 
#define REG_MAG_IDC                     0x0C
 134 
#define REG_MAG_IDC                     0x0C
126 
#define REG_MAG_IDF                     0x0F  // WHO_AM_I _M = 0x03c when LSM303DLM is connected
 135 
#define REG_MAG_IDF                     0x0F  // WHO_AM_I _M = 0x03c when LSM303DLM is connected
127 
 
 136 
 
128 
// bit mask for configuration mode
 137 
// bit mask for configuration mode
129 
#define CRA_MODE_MASK           0x03
 138 
#define CRA_MODE_MASK           0x03
130 
#define CRA_MODE_NORMAL         0x00    //default
 139 
#define CRA_MODE_NORMAL         0x00    //default
131 
#define CRA_MODE_POSBIAS        0x01
 140 
#define CRA_MODE_POSBIAS        0x01
132 
#define CRA_MODE_NEGBIAS        0x02
 141 
#define CRA_MODE_NEGBIAS        0x02
133 
#define CRA_MODE_SELFTEST       0x03
 142 
#define CRA_MODE_SELFTEST       0x03
134 
 
 143 
 
135 
// bit mask for measurement mode
 144 
// bit mask for measurement mode
136 
#define MODE_MASK                       0xFF
 145 
#define MODE_MASK                       0xFF
137 
#define MODE_CONTINUOUS         0x00
 146 
#define MODE_CONTINUOUS         0x00
138 
#define MODE_SINGLE                     0x01    // default
 147 
#define MODE_SINGLE                     0x01    // default
139 
#define MODE_IDLE                       0x02
 148 
#define MODE_IDLE                       0x02
140 
#define MODE_SLEEP                      0x03
 149 
#define MODE_SLEEP                      0x03
141 
 
 150 
 
142 
// bit mask for rate
 151 
// bit mask for rate
143 
#define CRA_RATE_MASK           0x1C
 152 
#define CRA_RATE_MASK           0x1C
144 
 
 153 
 
145 
// bit mask for gain
 154 
// bit mask for gain
146 
#define CRB_GAIN_MASK           0xE0
 155 
#define CRB_GAIN_MASK           0xE0
147 
 
 156 
 
148 
// ids
 157 
// ids
149 
#define MAG_IDA         0x48
 158 
#define MAG_IDA         0x48
150 
#define MAG_IDB         0x34
 159 
#define MAG_IDB         0x34
151 
#define MAG_IDC         0x33
 160 
#define MAG_IDC         0x33
152 
#define MAG_IDF_LSM303DLM       0x3C
 161 
#define MAG_IDF_LSM303DLM       0x3C
153 
 
 162 
 
154 
// the special HMC5843 interface
 163 
// the special HMC5843 interface
155 
// bit mask for rate
 164 
// bit mask for rate
156 
#define HMC5843_CRA_RATE_0_5HZ          0x00
 165 
#define HMC5843_CRA_RATE_0_5HZ          0x00
157 
#define HMC5843_CRA_RATE_1HZ            0x04
 166 
#define HMC5843_CRA_RATE_1HZ            0x04
158 
#define HMC5843_CRA_RATE_2HZ            0x08
 167 
#define HMC5843_CRA_RATE_2HZ            0x08
159 
#define HMC5843_CRA_RATE_5HZ            0x0C
 168 
#define HMC5843_CRA_RATE_5HZ            0x0C
160 
#define HMC5843_CRA_RATE_10HZ           0x10    //default
 169 
#define HMC5843_CRA_RATE_10HZ           0x10    //default
161 
#define HMC5843_CRA_RATE_20HZ           0x14
 170 
#define HMC5843_CRA_RATE_20HZ           0x14
162 
#define HMC5843_CRA_RATE_50HZ           0x18
 171 
#define HMC5843_CRA_RATE_50HZ           0x18
163 
// bit mask for gain
 172 
// bit mask for gain
164 
#define HMC5843_CRB_GAIN_07GA           0x00
 173 
#define HMC5843_CRB_GAIN_07GA           0x00
165 
#define HMC5843_CRB_GAIN_10GA           0x20    //default
 174 
#define HMC5843_CRB_GAIN_10GA           0x20    //default
166 
#define HMC5843_CRB_GAIN_15GA           0x40    // <--- we use this    
175 
#define HMC5843_CRB_GAIN_15GA           0x40    // <--- we use this    
167 
#define HMC5843_CRB_GAIN_20GA           0x60
 176 
#define HMC5843_CRB_GAIN_20GA           0x60
168 
#define HMC5843_CRB_GAIN_32GA           0x80
 177 
#define HMC5843_CRB_GAIN_32GA           0x80
169 
#define HMC5843_CRB_GAIN_38GA           0xA0
 178 
#define HMC5843_CRB_GAIN_38GA           0xA0
170 
#define HMC5843_CRB_GAIN_45GA           0xC0
 179 
#define HMC5843_CRB_GAIN_45GA           0xC0
171 
#define HMC5843_CRB_GAIN_65GA           0xE0
 180 
#define HMC5843_CRB_GAIN_65GA           0xE0
172 
// self test value
 181 
// self test value
173 
#define HMC5843_TEST_XSCALE             555
 182 
#define HMC5843_TEST_XSCALE             555
174 
#define HMC5843_TEST_YSCALE             555
 183 
#define HMC5843_TEST_YSCALE             555
175 
#define HMC5843_TEST_ZSCALE             555
 184 
#define HMC5843_TEST_ZSCALE             555
176 
// calibration range
 185 
// calibration range
177 
#define HMC5843_CALIBRATION_RANGE   600
 186 
#define HMC5843_CALIBRATION_RANGE   600
178 
 
 187 
 
179 
// the special LSM302DLH interface
 188 
// the special LSM302DLH interface
180 
// bit mask for rate
 189 
// bit mask for rate
181 
#define LSM303DLH_CRA_RATE_0_75HZ       0x00
 190 
#define LSM303DLH_CRA_RATE_0_75HZ       0x00
182 
#define LSM303DLH_CRA_RATE_1_5HZ        0x04
 191 
#define LSM303DLH_CRA_RATE_1_5HZ        0x04
183 
#define LSM303DLH_CRA_RATE_3_0HZ        0x08
 192 
#define LSM303DLH_CRA_RATE_3_0HZ        0x08
184 
#define LSM303DLH_CRA_RATE_7_5HZ        0x0C
 193 
#define LSM303DLH_CRA_RATE_7_5HZ        0x0C
185 
#define LSM303DLH_CRA_RATE_15HZ         0x10    //default
 194 
#define LSM303DLH_CRA_RATE_15HZ         0x10    //default
186 
#define LSM303DLH_CRA_RATE_30HZ         0x14
 195 
#define LSM303DLH_CRA_RATE_30HZ         0x14
187 
#define LSM303DLH_CRA_RATE_75HZ         0x18
 196 
#define LSM303DLH_CRA_RATE_75HZ         0x18
188 
 
 197 
 
189 
// bit mask for gain
 198 
// bit mask for gain
190 
#define LSM303DLH_CRB_GAIN_XXGA         0x00
 199 
#define LSM303DLH_CRB_GAIN_XXGA         0x00
191 
#define LSM303DLH_CRB_GAIN_13GA         0x20    //default
 200 
#define LSM303DLH_CRB_GAIN_13GA         0x20    //default
192 
#define LSM303DLH_CRB_GAIN_19GA         0x40    // <--- we use this
 201 
#define LSM303DLH_CRB_GAIN_19GA         0x40    // <--- we use this
193 
#define LSM303DLH_CRB_GAIN_25GA         0x60
 202 
#define LSM303DLH_CRB_GAIN_25GA         0x60
194 
#define LSM303DLH_CRB_GAIN_40GA         0x80
 203 
#define LSM303DLH_CRB_GAIN_40GA         0x80
195 
#define LSM303DLH_CRB_GAIN_47GA         0xA0
 204 
#define LSM303DLH_CRB_GAIN_47GA         0xA0
196 
#define LSM303DLH_CRB_GAIN_56GA         0xC0
 205 
#define LSM303DLH_CRB_GAIN_56GA         0xC0
197 
#define LSM303DLH_CRB_GAIN_81GA         0xE0
 206 
#define LSM303DLH_CRB_GAIN_81GA         0xE0
198 
 
 207 
 
199 
typedef struct
 208 
typedef struct
200 
{
 209 
{
201 
        u8 A;
 210 
        u8 A;
202 
        u8 B;
 211 
        u8 B;
203 
        u8 C;
 212 
        u8 C;
204 
} __attribute__((packed)) Identification_t;
 213 
} __attribute__((packed)) Identification_t;
205 
volatile Identification_t NCMAG_Identification;
 214 
volatile Identification_t NCMAG_Identification;
206 
 
 215 
 
207 
typedef struct
 216 
typedef struct
208 
{
 217 
{
209 
        u8 Sub;
 218 
        u8 Sub;
210 
} __attribute__((packed)) Identification2_t;
 219 
} __attribute__((packed)) Identification2_t;
211 
volatile Identification2_t NCMAG_Identification2;
 220 
volatile Identification2_t NCMAG_Identification2;
212 
 
 221 
 
213 
typedef struct
 222 
typedef struct
214 
{
 223 
{
215 
        u8 cra;
 224 
        u8 cra;
216 
        u8 crb;
 225 
        u8 crb;
217 
        u8 mode;
 226 
        u8 mode;
218 
} __attribute__((packed)) MagConfig_t;
 227 
} __attribute__((packed)) MagConfig_t;
219 
 
 228 
 
220 
volatile MagConfig_t MagConfig;
 229 
volatile MagConfig_t MagConfig;
221 
 
 230 
 
- 
 
 231 
 
- 
 
 232 
 
- 
 
 233 
 
222 
 
 234 
 
223 
// self test value
 235 
// self test value
224 
#define LSM303DLH_TEST_XSCALE   495
 236 
#define LSM303DLH_TEST_XSCALE   495
225 
#define LSM303DLH_TEST_YSCALE   495
 237 
#define LSM303DLH_TEST_YSCALE   495
226 
#define LSM303DLH_TEST_ZSCALE   470
 238 
#define LSM303DLH_TEST_ZSCALE   470
227 
// clibration range
 239 
// clibration range
228 
#define LSM303_CALIBRATION_RANGE   550
 240 
#define LSM303_CALIBRATION_RANGE   550
229 
 
 241 
 
230 
// the i2c ACC interface
 242 
// the i2c ACC interface
231 
#define ACC_SLAVE_ADDRESS               0x30    // i2c slave for acc. sensor registers
 243 
#define ACC_SLAVE_ADDRESS               0x30    // i2c slave for acc. sensor registers
232 
 
 244 
 
233 
// multiple byte read/write mask
 245 
// multiple byte read/write mask
234 
#define REG_ACC_MASK_AUTOINCREMENT 0x80
 246 
#define REG_ACC_MASK_AUTOINCREMENT 0x80
235 
 
 247 
 
236 
// register mapping
 248 
// register mapping
237 
#define REG_ACC_CTRL1                   0x20
 249 
#define REG_ACC_CTRL1                   0x20
238 
#define REG_ACC_CTRL2                   0x21
 250 
#define REG_ACC_CTRL2                   0x21
239 
#define REG_ACC_CTRL3                   0x22
 251 
#define REG_ACC_CTRL3                   0x22
240 
#define REG_ACC_CTRL4                   0x23
 252 
#define REG_ACC_CTRL4                   0x23
241 
#define REG_ACC_CTRL5                   0x24
 253 
#define REG_ACC_CTRL5                   0x24
242 
#define REG_ACC_HP_FILTER_RESET 0x25
 254 
#define REG_ACC_HP_FILTER_RESET 0x25
243 
#define REG_ACC_REFERENCE               0x26
 255 
#define REG_ACC_REFERENCE               0x26
244 
#define REG_ACC_STATUS                  0x27
 256 
#define REG_ACC_STATUS                  0x27
245 
#define REG_ACC_X_LSB                   0x28
 257 
#define REG_ACC_X_LSB                   0x28
246 
#define REG_ACC_X_MSB                   0x29
 258 
#define REG_ACC_X_MSB                   0x29
247 
#define REG_ACC_Y_LSB                   0x2A
 259 
#define REG_ACC_Y_LSB                   0x2A
248 
#define REG_ACC_Y_MSB                   0x2B
 260 
#define REG_ACC_Y_MSB                   0x2B
249 
#define REG_ACC_Z_LSB                   0x2C
 261 
#define REG_ACC_Z_LSB                   0x2C
250 
#define REG_ACC_Z_MSB                   0x2D
 262 
#define REG_ACC_Z_MSB                   0x2D
251 
 
 263 
 
252 
#define ACC_CRTL1_PM_DOWN               0x00
 264 
#define ACC_CRTL1_PM_DOWN               0x00
253 
#define ACC_CRTL1_PM_NORMAL             0x20
 265 
#define ACC_CRTL1_PM_NORMAL             0x20
254 
#define ACC_CRTL1_PM_LOW_0_5HZ  0x40
 266 
#define ACC_CRTL1_PM_LOW_0_5HZ  0x40
255 
#define ACC_CRTL1_PM_LOW_1HZ    0x60
 267 
#define ACC_CRTL1_PM_LOW_1HZ    0x60
256 
#define ACC_CRTL1_PM_LOW_2HZ    0x80
 268 
#define ACC_CRTL1_PM_LOW_2HZ    0x80
257 
#define ACC_CRTL1_PM_LOW_5HZ    0xA0
 269 
#define ACC_CRTL1_PM_LOW_5HZ    0xA0
258 
#define ACC_CRTL1_PM_LOW_10HZ   0xC0
 270 
#define ACC_CRTL1_PM_LOW_10HZ   0xC0
259 
// Output data rate in normal power mode
 271 
// Output data rate in normal power mode
260 
#define ACC_CRTL1_DR_50HZ               0x00
 272 
#define ACC_CRTL1_DR_50HZ               0x00
261 
#define ACC_CRTL1_DR_100HZ              0x08
 273 
#define ACC_CRTL1_DR_100HZ              0x08
262 
#define ACC_CRTL1_DR_400HZ              0x10
 274 
#define ACC_CRTL1_DR_400HZ              0x10
263 
#define ACC_CRTL1_DR_1000HZ             0x18
 275 
#define ACC_CRTL1_DR_1000HZ             0x18
264 
// axis anable flags                   
276 
// axis anable flags                   
265 
#define ACC_CRTL1_XEN                   0x01
 277 
#define ACC_CRTL1_XEN                   0x01
266 
#define ACC_CRTL1_YEN                   0x02
 278 
#define ACC_CRTL1_YEN                   0x02
267 
#define ACC_CRTL1_ZEN                   0x04
 279 
#define ACC_CRTL1_ZEN                   0x04
268 
 
 280 
 
269 
#define ACC_CRTL2_FILTER8       0x10
 281 
#define ACC_CRTL2_FILTER8       0x10
270 
#define ACC_CRTL2_FILTER16      0x11
 282 
#define ACC_CRTL2_FILTER16      0x11
271 
#define ACC_CRTL2_FILTER32      0x12
 283 
#define ACC_CRTL2_FILTER32      0x12
272 
#define ACC_CRTL2_FILTER64      0x13
 284 
#define ACC_CRTL2_FILTER64      0x13
273 
 
 285 
 
274 
#define ACC_CTRL4_BDU                   0x80 // Block data update, (0: continuos update; 1: output registers not updated between MSB and LSB reading)
 286 
#define ACC_CTRL4_BDU                   0x80 // Block data update, (0: continuos update; 1: output registers not updated between MSB and LSB reading)
275 
#define ACC_CTRL4_BLE                   0x40 // Big/little endian, (0: data LSB @ lower address; 1: data MSB @ lower address)
 287 
#define ACC_CTRL4_BLE                   0x40 // Big/little endian, (0: data LSB @ lower address; 1: data MSB @ lower address)
276 
#define ACC_CTRL4_FS_2G                 0x00
 288 
#define ACC_CTRL4_FS_2G                 0x00
277 
#define ACC_CTRL4_FS_4G                 0x10
 289 
#define ACC_CTRL4_FS_4G                 0x10
278 
#define ACC_CTRL4_FS_8G                 0x30
 290 
#define ACC_CTRL4_FS_8G                 0x30
279 
#define ACC_CTRL4_STSIGN_PLUS   0x00
 291 
#define ACC_CTRL4_STSIGN_PLUS   0x00
280 
#define ACC_CTRL4_STSIGN_MINUS  0x08
 292 
#define ACC_CTRL4_STSIGN_MINUS  0x08
281 
#define ACC_CTRL4_ST_ENABLE             0x02
 293 
#define ACC_CTRL4_ST_ENABLE             0x02
282 
 
 294 
 
283 
#define ACC_CTRL5_STW_ON                0x03
 295 
#define ACC_CTRL5_STW_ON                0x03
284 
#define ACC_CTRL5_STW_OFF               0x00
 296 
#define ACC_CTRL5_STW_OFF               0x00
285 
 
 297 
 
286 
typedef struct
 298 
typedef struct
287 
{
 299 
{
288 
        u8 ctrl_1;
 300 
        u8 ctrl_1;
289 
        u8 ctrl_2;
 301 
        u8 ctrl_2;
290 
        u8 ctrl_3;
 302 
        u8 ctrl_3;
291 
        u8 ctrl_4;
 303 
        u8 ctrl_4;
292 
        u8 ctrl_5;
 304 
        u8 ctrl_5;
293 
} __attribute__((packed)) AccConfig_t;
 305 
} __attribute__((packed)) AccConfig_t;
294 
 
 306 
 
295 
volatile AccConfig_t AccConfig;
 307 
volatile AccConfig_t AccConfig;
296 
 
 308 
 
297 
u8 NCMag_CalibrationWrite(void)
 309 
u8 NCMag_CalibrationWrite(void)
298 
{
 310 
{
299 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
 311 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
300 
        EEPROM_Result_t eres;
 312 
        EEPROM_Result_t eres;
301 
        u8 *pBuff = (u8*)&Calibration;
 313 
        u8 *pBuff = (u8*)&Calibration;
302 
 
 314 
 
303 
        Calibration.Version = CALIBRATION_VERSION;
 315 
        Calibration.Version = CALIBRATION_VERSION;
304 
        for(i = 0; i<(sizeof(Calibration)-1); i++)
 316 
        for(i = 0; i<(sizeof(Calibration)-1); i++)
305 
        {
 317 
        {
306 
                crc += pBuff[i];        
 318 
                crc += pBuff[i];        
307 
        }
 319 
        }
308 
        Calibration.crc = ~crc;
 320 
        Calibration.crc = ~crc;
309 
        eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration));
 321 
        eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration));
310 
        if(EEPROM_SUCCESS == eres) i = 1;
 322 
        if(EEPROM_SUCCESS == eres) i = 1;
311 
        else i = 0;
 323 
        else i = 0;
312 
        return(i);     
 324 
        return(i);     
313 
}
 325 
}
314 
 
 326 
 
315 
u8 NCMag_CalibrationRead(void)
 327 
u8 NCMag_CalibrationRead(void)
316 
{
 328 
{
317 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
 329 
        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
318 
        u8 *pBuff = (u8*)&Calibration;
 330 
        u8 *pBuff = (u8*)&Calibration;
319 
 
 331 
 
320 
        if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)))
 332 
        if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)))
321 
        {
 333 
        {
322 
                for(i = 0; i<(sizeof(Calibration)-1); i++)
 334 
                for(i = 0; i<(sizeof(Calibration)-1); i++)
323 
                {
 335 
                {
324 
                        crc += pBuff[i];        
 336 
                        crc += pBuff[i];        
325 
                }
 337 
                }
326 
                crc = ~crc;
 338 
                crc = ~crc;
327 
                if(Calibration.crc != crc) return(0); // crc mismatch
 339 
                if(Calibration.crc != crc) return(0); // crc mismatch
328 
                if(Calibration.Version == CALIBRATION_VERSION) return(1);
 340 
                if(Calibration.Version == CALIBRATION_VERSION) return(1);
329 
        }
 341 
        }
330 
        return(0);
 342 
        return(0);
331 
}
 343 
}
332 
 
 344 
 
333 
 
 345 
 
334 
void NCMAG_Calibrate(void)
 346 
void NCMAG_Calibrate(void)
335 
{
 347 
{
336 
        u8 msg[64];
 348 
        u8 msg[64];
337 
        static u8 speak = 0;
 349 
        static u8 speak = 0;
338 
        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
 350 
        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
339 
        static s16 X = 0, Y = 0, Z = 0;
 351 
        static s16 X = 0, Y = 0, Z = 0;
340 
        static u8 OldCalState = 0;     
 352 
        static u8 OldCalState = 0;     
341 
        s16 MinCalibration = 450;
 353 
        s16 MinCalibration = 450;
342 
 
 354 
 
343 
        X = (4*X + MagRawVector.X + 3)/5;
 355 
        X = (4*X + MagRawVector.X + 3)/5;
344 
        Y = (4*Y + MagRawVector.Y + 3)/5;
 356 
        Y = (4*Y + MagRawVector.Y + 3)/5;
345 
        Z = (4*Z + MagRawVector.Z + 3)/5;
 357 
        Z = (4*Z + MagRawVector.Z + 3)/5;
346 
 
 358 
 
347 
        switch(Compass_CalState)
 359 
        switch(Compass_CalState)
348 
        {
 360 
        {
349 
                case 1:
 361 
                case 1:
350 
                        // 1st step of calibration
 362 
                        // 1st step of calibration
351 
                        // initialize ranges
 363 
                        // initialize ranges
352 
                        // used to change the orientation of the NC in the horizontal plane
 364 
                        // used to change the orientation of the NC in the horizontal plane
353 
                        Xmin =  10000;
 365 
                        Xmin =  10000;
354 
                        Xmax = -10000;
 366 
                        Xmax = -10000;
355 
                        Ymin =  10000;
 367 
                        Ymin =  10000;
356 
                        Ymax = -10000;
 368 
                        Ymax = -10000;
357 
                        Zmin =  10000;
 369 
                        Zmin =  10000;
358 
                        Zmax = -10000;
 370 
                        Zmax = -10000;
359 
                        speak = 1;
 371 
                        speak = 1;
360 
                        break;
 372 
                        break;
361 
               
 373 
               
362 
                case 2: // 2nd step of calibration
 374 
                case 2: // 2nd step of calibration
363 
                        // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
 375 
                        // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
364 
                        if(X < Xmin)            { Xmin = X; BeepTime = 20;}
 376 
                        if(X < Xmin)            { Xmin = X; BeepTime = 20;}
365 
                        else if(X > Xmax)       { Xmax = X; BeepTime = 20;}
 377 
                        else if(X > Xmax)       { Xmax = X; BeepTime = 20;}
366 
                        if(Y < Ymin)            { Ymin = Y; BeepTime = 60;}
 378 
                        if(Y < Ymin)            { Ymin = Y; BeepTime = 60;}
367 
                        else if(Y > Ymax)       { Ymax = Y; BeepTime = 60;}
 379 
                        else if(Y > Ymax)       { Ymax = Y; BeepTime = 60;}
368 
                        if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;
 380 
                        if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;
369 
                        break;
 381 
                        break;
370 
 
 382 
 
371 
                case 3: // 3rd step of calibration
 383 
                case 3: // 3rd step of calibration
372 
                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
 384 
                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
373 
                        speak = 1;
 385 
                        speak = 1;
374 
                        break;
 386 
                        break;
375 
 
 387 
 
376 
                case 4:
 388 
                case 4:
377 
                        // find Min and Max of the Z-Sensor
 389 
                        // find Min and Max of the Z-Sensor
378 
                        if(Z < Zmin)      { Zmin = Z; BeepTime = 80;}
 390 
                        if(Z < Zmin)      { Zmin = Z; BeepTime = 80;}
379 
                        else if(Z > Zmax) { Zmax = Z; BeepTime = 80;}
 391 
                        else if(Z > Zmax) { Zmax = Z; BeepTime = 80;}
380 
                        if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;
 392 
                        if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;
381 
                        break;
 393 
                        break;
382 
               
 394 
               
383 
                case 5:
 395 
                case 5:
384 
                        // Save values
 396 
                        // Save values
385 
                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
 397 
                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
386 
                        {
 398 
                        {
387 
                                switch(NCMAG_SensorType)
 399 
                                switch(NCMAG_SensorType)
388 
                                {
 400 
                                {
389 
                                        case TYPE_HMC5843:
 401 
                                        case TYPE_HMC5843:
390 
                                                UART1_PutString("\r\nHMC5843 calibration\n\r");
 402 
                                                UART1_PutString("\r\nHMC5843 calibration\n\r");
391 
                                                MinCalibration = HMC5843_CALIBRATION_RANGE;
 403 
                                                MinCalibration = HMC5843_CALIBRATION_RANGE;
392 
                                                break;
 404 
                                                break;
393 
 
 405 
 
394 
                                        case TYPE_LSM303DLH:
 406 
                                        case TYPE_LSM303DLH:
395 
                                        case TYPE_LSM303DLM:
 407 
                                        case TYPE_LSM303DLM:
396 
                                                UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
 408 
                                                UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
397 
                                                MinCalibration = LSM303_CALIBRATION_RANGE;
 409 
                                                MinCalibration = LSM303_CALIBRATION_RANGE;
398 
                                        break;
 410 
                                        break;
399 
                                }
 411 
                                }
400 
                                if(EarthMagneticStrengthTheoretic)
 412 
                                if(EarthMagneticStrengthTheoretic)
401 
                                 {
 413 
                                 {
402 
                                  MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
 414 
                                  MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
403 
                                  sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
 415 
                                  sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
404 
                                  UART1_PutString(msg);
 416 
                                  UART1_PutString(msg);
405 
                                 }
 417 
                                 }
406 
                            else UART1_PutString("without GPS\n\r");
 418 
                            else UART1_PutString("without GPS\n\r");
407 
 
 419 
 
408 
                                Calibration.MagX.Range = Xmax - Xmin;
 420 
                                Calibration.MagX.Range = Xmax - Xmin;
409 
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
 421 
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
410 
                                Calibration.MagY.Range = Ymax - Ymin;
 422 
                                Calibration.MagY.Range = Ymax - Ymin;
411 
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
 423 
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
412 
                                Calibration.MagZ.Range = Zmax - Zmin;
 424 
                                Calibration.MagZ.Range = Zmax - Zmin;
413 
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
 425 
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
414 
                                if((Calibration.MagX.Range > MinCalibration) && (Calibration.MagY.Range > MinCalibration) && (Calibration.MagZ.Range > MinCalibration))
 426 
                                if((Calibration.MagX.Range > MinCalibration) && (Calibration.MagY.Range > MinCalibration) && (Calibration.MagZ.Range > MinCalibration))
415 
                                {
 427 
                                {
416 
                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite();
 428 
                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite();
417 
                                        BeepTime = 2500;
 429 
                                        BeepTime = 2500;
418 
                                        UART1_PutString("\r\n-> Calibration okay <-\n\r");
 430 
                                        UART1_PutString("\r\n-> Calibration okay <-\n\r");
419 
SpeakHoTT = SPEAK_MIKROKOPTER;
 431 
SpeakHoTT = SPEAK_MIKROKOPTER;
420 
                                }
 432 
                                }
421 
                                else
 433 
                                else
422 
                                {
 434 
                                {
423 
SpeakHoTT = SPEAK_ERR_CALIBARTION;
 435 
SpeakHoTT = SPEAK_ERR_CALIBARTION;
424 
                                        UART1_PutString("\r\nCalibration FAILED - Values too low: ");
 436 
                                        UART1_PutString("\r\nCalibration FAILED - Values too low: ");
425 
                                    if(Calibration.MagX.Range < MinCalibration) UART1_PutString("X! ");
 437 
                                    if(Calibration.MagX.Range < MinCalibration) UART1_PutString("X! ");
426 
                                    if(Calibration.MagY.Range < MinCalibration) UART1_PutString("Y! ");
 438 
                                    if(Calibration.MagY.Range < MinCalibration) UART1_PutString("Y! ");
427 
                                    if(Calibration.MagZ.Range < MinCalibration) UART1_PutString("Z! ");
 439 
                                    if(Calibration.MagZ.Range < MinCalibration) UART1_PutString("Z! ");
428 
                                        UART1_PutString("\r\n");
 440 
                                        UART1_PutString("\r\n");
429 
 
 441 
 
430 
                                        // restore old calibration data from eeprom
 442 
                                        // restore old calibration data from eeprom
431 
                                        NCMAG_IsCalibrated = NCMag_CalibrationRead();
 443 
                                        NCMAG_IsCalibrated = NCMag_CalibrationRead();
432 
                                }
 444 
                                }
433 
                                        sprintf(msg, "X: (%i - %i = %i)\r\n",Xmax,Xmin,Xmax - Xmin);
 445 
                                        sprintf(msg, "X: (%i - %i = %i)\r\n",Xmax,Xmin,Xmax - Xmin);
434 
                                        UART1_PutString(msg);
 446 
                                        UART1_PutString(msg);
435 
                                        sprintf(msg, "Y: (%i - %i = %i)\r\n",Ymax,Ymin,Ymax - Ymin);
 447 
                                        sprintf(msg, "Y: (%i - %i = %i)\r\n",Ymax,Ymin,Ymax - Ymin);
436 
                                        UART1_PutString(msg);
 448 
                                        UART1_PutString(msg);
437 
                                        sprintf(msg, "Z: (%i - %i = %i)\r\n",Zmax,Zmin,Zmax - Zmin);
 449 
                                        sprintf(msg, "Z: (%i - %i = %i)\r\n",Zmax,Zmin,Zmax - Zmin);
438 
                                        UART1_PutString(msg);
 450 
                                        UART1_PutString(msg);
439 
                                        sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCalibration);
 451 
                                        sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCalibration);
440 
                                        UART1_PutString(msg);
 452 
                                        UART1_PutString(msg);
441 
                        }
 453 
                        }
442 
                        break;
 454 
                        break;
443 
                       
 455 
                       
444 
                default:
 456 
                default:
445 
                        break; 
 457 
                        break; 
446 
        }
 458 
        }
447 
        OldCalState = Compass_CalState;
 459 
        OldCalState = Compass_CalState;
448 
}
 460 
}
449 
 
 461 
 
450 
// ---------- call back handlers -----------------------------------------
 462 
// ---------- call back handlers -----------------------------------------
451 
 
 463 
 
452 
// rx data handler for id info request
 464 
// rx data handler for id info request
453 
void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
 465 
void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
454 
{       // if number of bytes are matching
 466 
{       // if number of bytes are matching
455 
        if(RxBufferSize == sizeof(NCMAG_Identification) )
 467 
        if(RxBufferSize == sizeof(NCMAG_Identification) )
456 
        {
 468 
        {
457 
                memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
 469 
                memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
458 
        }
 470 
        }
459 
}
 471 
}
460 
 
 472 
 
461 
void NCMAG_UpdateIdentification_Sub(u8* pRxBuffer, u8 RxBufferSize)
 473 
void NCMAG_UpdateIdentification_Sub(u8* pRxBuffer, u8 RxBufferSize)
462 
{       // if number of bytes are matching
 474 
{       // if number of bytes are matching
463 
        if(RxBufferSize == sizeof(NCMAG_Identification2))
 475 
        if(RxBufferSize == sizeof(NCMAG_Identification2))
464 
        {
 476 
        {
465 
                memcpy((u8 *)&NCMAG_Identification2, pRxBuffer, sizeof(NCMAG_Identification2));
 477 
                memcpy((u8 *)&NCMAG_Identification2, pRxBuffer, sizeof(NCMAG_Identification2));
466 
        }
 478 
        }
467 
}
 479 
}
468 
 
 480 
 
469 
// rx data handler for magnetic sensor raw data
 481 
// rx data handler for magnetic sensor raw data
470 
void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
 482 
void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
471 
{       // if number of bytes are matching
 483 
{       // if number of bytes are matching
472 
        if(RxBufferSize == sizeof(MagRawVector) )
 484 
        if(RxBufferSize == sizeof(MagRawVector) )
473 
        {       // byte order from big to little endian
 485 
        {       // byte order from big to little endian
474 
                s16 raw;
 486 
                s16 raw;
475 
                raw = pRxBuffer[0]<<8;
 487 
                raw = pRxBuffer[0]<<8;
476 
                raw+= pRxBuffer[1];
 488 
                raw+= pRxBuffer[1];
477 
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw;
 489 
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw;
478 
                raw = pRxBuffer[2]<<8;
 490 
                raw = pRxBuffer[2]<<8;
479 
                raw+= pRxBuffer[3];
 491 
                raw+= pRxBuffer[3];
480 
            if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
 492 
            if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
481 
                {
 493 
                {
482 
                        if(NCMAG_SensorType == TYPE_LSM303DLM)  MagRawVector.Z = raw; // here Z and Y are exchanged
 494 
                        if(NCMAG_SensorType == TYPE_LSM303DLM)  MagRawVector.Z = raw; // here Z and Y are exchanged
483 
                        else                                                                    MagRawVector.Y = raw;
 495 
                        else                                                                    MagRawVector.Y = raw;
484 
                }
 496 
                }
485 
                raw = pRxBuffer[4]<<8;
 497 
                raw = pRxBuffer[4]<<8;
486 
                raw+= pRxBuffer[5];
 498 
                raw+= pRxBuffer[5];
487 
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
 499 
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
488 
                {
 500 
                {
489 
                        if(NCMAG_SensorType == TYPE_LSM303DLM)  MagRawVector.Y = raw; // here Z and Y are exchanged
 501 
                        if(NCMAG_SensorType == TYPE_LSM303DLM)  MagRawVector.Y = raw; // here Z and Y are exchanged
490 
                        else                                                                    MagRawVector.Z = raw;
 502 
                        else                                                                    MagRawVector.Z = raw;
491 
                }
 503 
                }
492 
        }
 504 
        }
493 
        if(Compass_CalState || !NCMAG_IsCalibrated)
 505 
        if(Compass_CalState || !NCMAG_IsCalibrated)
494 
        {       // mark out data invalid
 506 
        {       // mark out data invalid
495 
                MagVector.X = MagRawVector.X;
 507 
                MagVector.X = MagRawVector.X;
496 
                MagVector.Y = MagRawVector.Y;
 508 
                MagVector.Y = MagRawVector.Y;
497 
                MagVector.Z = MagRawVector.Z;
 509 
                MagVector.Z = MagRawVector.Z;
498 
                Compass_Heading = -1;
 510 
                Compass_Heading = -1;
499 
        }
 511 
        }
500 
        else
 512 
        else
501 
        {
 513 
        {
502 
                // update MagVector from MagRaw Vector by Scaling
 514 
                // update MagVector from MagRaw Vector by Scaling
503 
                MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range);
 515 
                MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range);
504 
                MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range);
 516 
                MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range);
505 
                MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range);
 517 
                MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range);
506 
                Compass_CalcHeading();
 518 
                Compass_CalcHeading();
507 
        }
 519 
        }
508 
}
 520 
}
509 
// rx data handler  for acceleration raw data
 521 
// rx data handler  for acceleration raw data
510 
void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
 522 
void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
511 
{       // if number of byte are matching
 523 
{       // if number of byte are matching
512 
static s32 filter_z;
 524 
static s32 filter_z;
513 
        if(RxBufferSize == sizeof(AccRawVector) )
 525 
        if(RxBufferSize == sizeof(AccRawVector) )
514 
        {
 526 
        {
515 
                memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
 527 
                memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
516 
        }
 528 
        }
517 
//      DebugOut.Analog[16] = AccRawVector.X;
 529 
//      DebugOut.Analog[16] = AccRawVector.X;
518 
//      DebugOut.Analog[17] = AccRawVector.Y;
 530 
//      DebugOut.Analog[17] = AccRawVector.Y;
519 
        filter_z = (filter_z * 7 + AccRawVector.Z) / 8;
 531 
        filter_z = (filter_z * 7 + AccRawVector.Z) / 8;
520 
 
 532 
 
521 
//      DebugOut.Analog[18] = filter_z;
 533 
//      DebugOut.Analog[18] = filter_z;
522 
//      DebugOut.Analog[19] = AccRawVector.Z;
 534 
//      DebugOut.Analog[19] = AccRawVector.Z;
523 
}
 535 
}
524 
// rx data handler for reading magnetic sensor configuration
 536 
// rx data handler for reading magnetic sensor configuration
525 
void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
 537 
void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
526 
{       // if number of byte are matching
 538 
{       // if number of byte are matching
527 
        if(RxBufferSize == sizeof(MagConfig) )
 539 
        if(RxBufferSize == sizeof(MagConfig) )
528 
        {
 540 
        {
529 
                memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig));
 541 
                memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig));
530 
        }
 542 
        }
531 
}
 543 
}
532 
// rx data handler for reading acceleration sensor configuration
 544 
// rx data handler for reading acceleration sensor configuration
533 
void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize)
 545 
void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize)
534 
{       // if number of byte are matching
 546 
{       // if number of byte are matching
535 
        if(RxBufferSize == sizeof(AccConfig) )
 547 
        if(RxBufferSize == sizeof(AccConfig) )
536 
        {
 548 
        {
537 
                memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
 549 
                memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
538 
        }
 550 
        }
539 
}
 551 
}
540 
//----------------------------------------------------------------------
 552 
//----------------------------------------------------------------------
541 
 
 553 
 
542 
 
 554 
 
543 
// ---------------------------------------------------------------------
 555 
// ---------------------------------------------------------------------
544 
u8 NCMAG_SetMagConfig(void)
 556 
u8 NCMAG_SetMagConfig(void)
545 
{
 557 
{
546 
        u8 retval = 0;
 558 
        u8 retval = 0;
547 
        // try to catch the i2c buffer within 100 ms timeout
 559 
        // try to catch the i2c buffer within 100 ms timeout
548 
        if(I2C_LockBuffer(100))
 560 
        if(I2C_LockBufferFunc(100))
549 
        {
 561 
        {
550 
                u8 TxBytes = 0;
 562 
                u8 TxBytes = 0;
551 
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;    
 563 
                I2C_BufferPnt[TxBytes++] = REG_MAG_CRA;        
552 
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&MagConfig, sizeof(MagConfig));
 564 
                memcpy((u8*)(&I2C_BufferPnt[TxBytes]), (u8*)&MagConfig, sizeof(MagConfig));
553 
                TxBytes += sizeof(MagConfig);
 565 
                TxBytes += sizeof(MagConfig);
554 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, 0, 0))
 566 
                if(I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, 0, 0))
555 
                {
 567 
                {
556 
                        if(I2C_WaitForEndOfTransmission(100))
 568 
                        if(I2C_WaitForEndOfTransmissionFunc(100))
557 
                        {
 569 
                        {
558 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 570 
                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
559 
                        }
 571 
                        }
560 
                }
 572 
                }
561 
        }
 573 
        }
562 
        return(retval);        
 574 
        return(retval);        
563 
}
 575 
}
564 
 
 576 
 
565 
// ----------------------------------------------------------------------------------------
 577 
// ----------------------------------------------------------------------------------------
566 
u8 NCMAG_GetMagConfig(void)
 578 
u8 NCMAG_GetMagConfig(void)
567 
{
 579 
{
568 
        u8 retval = 0;
 580 
        u8 retval = 0;
569 
        // try to catch the i2c buffer within 100 ms timeout
 581 
        // try to catch the i2c buffer within 100 ms timeout
570 
        if(I2C_LockBuffer(100))
 582 
        if(I2C_LockBufferFunc(100))
571 
        {
 583 
        {
572 
                u8 TxBytes = 0;
 584 
                u8 TxBytes = 0;
573 
                I2C_Buffer[TxBytes++] = REG_MAG_CRA;
 585 
                I2C_BufferPnt[TxBytes++] = REG_MAG_CRA;
574 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
 586 
                if(I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
575 
                {
 587 
                {
576 
                        if(I2C_WaitForEndOfTransmission(100))
 588 
                        if(I2C_WaitForEndOfTransmissionFunc(100))
577 
                        {
 589 
                        {
578 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 590 
                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
579 
                        }
 591 
                        }
580 
                }
 592 
                }
581 
        }
 593 
        }
582 
        return(retval);        
 594 
        return(retval);        
583 
}
 595 
}
584 
 
 596 
 
585 
// ----------------------------------------------------------------------------------------
 597 
// ----------------------------------------------------------------------------------------
586 
u8 NCMAG_SetAccConfig(void)
 598 
u8 NCMAG_SetAccConfig(void)
587 
{
 599 
{
588 
        u8 retval = 0;
 600 
        u8 retval = 0;
589 
        // try to catch the i2c buffer within 100 ms timeout
 601 
        // try to catch the i2c buffer within 100 ms timeout
590 
        if(I2C_LockBuffer(100))
 602 
        if(I2C_LockBufferFunc(100))
591 
        {
 603 
        {
592 
                u8 TxBytes = 0;
 604 
                u8 TxBytes = 0;
593 
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;      
 605 
                I2C_BufferPnt[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;    
594 
                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
 606 
                memcpy((u8*)(&I2C_BufferPnt[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
595 
                TxBytes += sizeof(AccConfig);
 607 
                TxBytes += sizeof(AccConfig);
596 
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
 608 
                if(I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
597 
                {
 609 
                {
598 
                        if(I2C_WaitForEndOfTransmission(100))
 610 
                        if(I2C_WaitForEndOfTransmissionFunc(100))
599 
                        {
 611 
                        {
600 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 612 
                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
601 
                        }
 613 
                        }
602 
                }
 614 
                }
603 
        }
 615 
        }
604 
        return(retval);        
 616 
        return(retval);        
605 
}
 617 
}
606 
 
 618 
 
607 
// ----------------------------------------------------------------------------------------
 619 
// ----------------------------------------------------------------------------------------
608 
u8 NCMAG_GetAccConfig(void)
 620 
u8 NCMAG_GetAccConfig(void)
609 
{
 621 
{
610 
        u8 retval = 0;
 622 
        u8 retval = 0;
611 
        // try to catch the i2c buffer within 100 ms timeout
 623 
        // try to catch the i2c buffer within 100 ms timeout
612 
        if(I2C_LockBuffer(100))
 624 
        if(I2C_LockBufferFunc(100))
613 
        {
 625 
        {
614 
                u8 TxBytes = 0;
 626 
                u8 TxBytes = 0;
615 
                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
 627 
                I2C_BufferPnt[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
616 
                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
 628 
                if(I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
617 
                {
 629 
                {
618 
                        if(I2C_WaitForEndOfTransmission(100))
 630 
                        if(I2C_WaitForEndOfTransmissionFunc(100))
619 
                        {
 631 
                        {
620 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 632 
                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
621 
                        }
 633 
                        }
622 
                }
 634 
                }
623 
        }
 635 
        }
624 
        return(retval);        
 636 
        return(retval);        
625 
}
 637 
}
626 
 
 638 
 
627 
// ----------------------------------------------------------------------------------------
 639 
// ----------------------------------------------------------------------------------------
628 
u8 NCMAG_GetIdentification(void)
 640 
u8 NCMAG_GetIdentification(void)
629 
{
 641 
{
630 
        u8 retval = 0;
 642 
        u8 retval = 0;
631 
        // try to catch the i2c buffer within 100 ms timeout
 643 
        // try to catch the i2c buffer within 100 ms timeout
632 
        if(I2C_LockBuffer(100))
 644 
        if(I2C_LockBufferFunc(100))
633 
        {
 645 
        {
634 
                u16 TxBytes = 0;
 646 
                u16 TxBytes = 0;
635 
                NCMAG_Identification.A = 0xFF;
 647 
                NCMAG_Identification.A = 0xFF;
636 
                NCMAG_Identification.B = 0xFF;
 648 
                NCMAG_Identification.B = 0xFF;
637 
                NCMAG_Identification.C = 0xFF;
 649 
                NCMAG_Identification.C = 0xFF;
638 
                I2C_Buffer[TxBytes++] = REG_MAG_IDA;
 650 
                I2C_BufferPnt[TxBytes++] = REG_MAG_IDA;
639 
                // initiate transmission
 651 
                // initiate transmission
640 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
 652 
                if(I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
641 
                {
 653 
                {
642 
                        if(I2C_WaitForEndOfTransmission(100))
 654 
                        if(I2C_WaitForEndOfTransmissionFunc(100))
643 
                        {
 655 
                        {
644 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 656 
                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
645 
                        }
 657 
                        }
646 
                }
 658 
                }
647 
        }
 659 
        }
648 
        return(retval);
 660 
        return(retval);
649 
}
 661 
}
650 
 
 662 
 
651 
u8 NCMAG_GetIdentification_Sub(void)
 663 
u8 NCMAG_GetIdentification_Sub(void)
652 
{
 664 
{
653 
        u8 retval = 0;
 665 
        u8 retval = 0;
654 
        // try to catch the i2c buffer within 100 ms timeout
 666 
        // try to catch the i2c buffer within 100 ms timeout
655 
        if(I2C_LockBuffer(100))
 667 
        if(I2C_LockBufferFunc(100))
656 
        {
 668 
        {
657 
                u16 TxBytes = 0;
 669 
                u16 TxBytes = 0;
658 
                NCMAG_Identification2.Sub = 0xFF;
 670 
                NCMAG_Identification2.Sub = 0xFF;
659 
                I2C_Buffer[TxBytes++] = REG_MAG_IDF;
 671 
                I2C_BufferPnt[TxBytes++] = REG_MAG_IDF;
660 
                // initiate transmission
 672 
                // initiate transmission
661 
                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
 673 
                if(I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
662 
                {
 674 
                {
663 
                        if(I2C_WaitForEndOfTransmission(100))
 675 
                        if(I2C_WaitForEndOfTransmissionFunc(100))
664 
                        {
 676 
                        {
665 
                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 677 
                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
666 
                        }
 678 
                        }
667 
                }
 679 
                }
668 
        }
 680 
        }
669 
        return(retval);
 681 
        return(retval);
670 
}
 682 
}
671 
 
 683 
 
672 
 
 684 
 
673 
// ----------------------------------------------------------------------------------------
 685 
// ----------------------------------------------------------------------------------------
674 
void NCMAG_GetMagVector(void)
 686 
void NCMAG_GetMagVector(void)
675 
{
 687 
{
676 
        // try to catch the I2C buffer within 0 ms
 688 
        // try to catch the I2C buffer within 0 ms
677 
        if(I2C_LockBuffer(0))
 689 
        if(I2C_LockBufferFunc(0))
678 
        {
 690 
        {
679 
                u16 TxBytes = 0;
 691 
                u16 TxBytes = 0;
680 
                // set register pointer
 692 
                // set register pointer
681 
                I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB;
 693 
                I2C_BufferPnt[TxBytes++] = REG_MAG_DATAX_MSB;
682 
                // initiate transmission
 694 
                // initiate transmission
683 
                I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
 695 
                I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
684 
        }
 696 
        }
685 
}
 697 
}
686 
 
 698 
 
687 
//----------------------------------------------------------------
 699 
//----------------------------------------------------------------
688 
void NCMAG_GetAccVector(void)
 700 
void NCMAG_GetAccVector(void)
689 
{
 701 
{
690 
        // try to catch the I2C buffer within 0 ms
 702 
        // try to catch the I2C buffer within 0 ms
691 
        if(I2C_LockBuffer(0))
 703 
        if(I2C_LockBufferFunc(0))
692 
        {
 704 
        {
693 
                u16 TxBytes = 0;
 705 
                u16 TxBytes = 0;
694 
                // set register pointer
 706 
                // set register pointer
695 
                I2C_Buffer[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT;
 707 
                I2C_BufferPnt[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT;
696 
                // initiate transmission
 708 
                // initiate transmission
697 
                I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
 709 
                I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
698 
        }
 710 
        }
699 
}
 711 
}
700 
 
 712 
 
701 
//----------------------------------------------------------------
 713 
//----------------------------------------------------------------
702 
u8 InitNC_MagnetSensor(void)
 714 
u8 InitNC_MagnetSensor(void)
703 
{
 715 
{
704 
        u8 crb_gain, cra_rate;
 716 
        u8 crb_gain, cra_rate;
705 
 
 717 
 
706 
        switch(NCMAG_SensorType)
 718 
        switch(NCMAG_SensorType)
707 
        {
 719 
        {
708 
                case TYPE_HMC5843:
 720 
                case TYPE_HMC5843:
709 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
 721 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
710 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
 722 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
711 
                        break;
 723 
                        break;
712 
 
 724 
 
713 
                case TYPE_LSM303DLH:
 725 
                case TYPE_LSM303DLH:
714 
                case TYPE_LSM303DLM:
 726 
                case TYPE_LSM303DLM:
715 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
 727 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
716 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
 728 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
717 
                        break;
 729 
                        break;
718 
 
 730 
 
719 
                default:
 731 
                default:
720 
                return(0);
 732 
                return(0);
721 
        }
 733 
        }
722 
 
 734 
 
723 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
 735 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
724 
        MagConfig.crb = crb_gain;
 736 
        MagConfig.crb = crb_gain;
725 
        MagConfig.mode = MODE_CONTINUOUS;
 737 
        MagConfig.mode = MODE_CONTINUOUS;
726 
        return(NCMAG_SetMagConfig());
 738 
        return(NCMAG_SetMagConfig());
727 
}
 739 
}
728 
 
 740 
 
729 
 
 741 
 
730 
//----------------------------------------------------------------
 742 
//----------------------------------------------------------------
731 
u8 NCMAG_Init_ACCSensor(void)
 743 
u8 NCMAG_Init_ACCSensor(void)
732 
{
 744 
{
733 
        AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN;
 745 
        AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN;
734 
        AccConfig.ctrl_2 = 0;//ACC_CRTL2_FILTER32;
 746 
        AccConfig.ctrl_2 = 0;//ACC_CRTL2_FILTER32;
735 
        AccConfig.ctrl_3 = 0x00;
 747 
        AccConfig.ctrl_3 = 0x00;
736 
        AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G;
 748 
        AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G;
737 
        AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF;
 749 
        AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF;
738 
        return(NCMAG_SetAccConfig());
 750 
        return(NCMAG_SetAccConfig());
739 
}
 751 
}
740 
// --------------------------------------------------------
 752 
// --------------------------------------------------------
741 
void NCMAG_Update(void)
 753 
void NCMAG_Update(void)
742 
{
 754 
{
743 
        static u32 TimerUpdate = 0;
 755 
        static u32 TimerUpdate = 0;
744 
        static s8 send_config = 0;
 756 
        static s8 send_config = 0;
745 
        u32 delay = 20;
 757 
        u32 delay = 20;
746 
 
 758 
 
747 
        if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
 759 
        if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
748 
        {
 760 
        {
749 
                Compass_Heading = -1;
 761 
                Compass_Heading = -1;
750 
                DebugOut.Analog[14]++; // count I2C error
 762 
                DebugOut.Analog[14]++; // count I2C error
751 
                return;
 763 
                return;
752 
        }
 764 
        }
753 
        if(CheckDelay(TimerUpdate))
 765 
        if(CheckDelay(TimerUpdate))
754 
        {
 766 
        {
755 
                if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
 767 
                if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
756 
        if(++send_config == 25)   // 500ms
 768 
        if(++send_config == 25)   // 500ms
757 
                {
 769 
                {
758 
                        send_config = -25;    // next try after 1 second
 770 
                        send_config = -25;    // next try after 1 second
759 
                InitNC_MagnetSensor();
 771 
                InitNC_MagnetSensor();
760 
                        TimerUpdate = SetDelay(20);    // back into the old time-slot
 772 
                        TimerUpdate = SetDelay(20);    // back into the old time-slot
761 
                }
 773 
                }
762 
                else
 774 
                else
763 
                {
 775 
                {
764 
//                      static u8 s = 0;
 776 
//                      static u8 s = 0;
765 
                        // check for new calibration state
 777 
                        // check for new calibration state
766 
                        Compass_UpdateCalState();
 778 
                        Compass_UpdateCalState();
767 
                        if(Compass_CalState) NCMAG_Calibrate();
 779 
                        if(Compass_CalState) NCMAG_Calibrate();
768 
                       
 780 
                       
769 
                        // in case of LSM303 type
 781 
                        // in case of LSM303 type
770 
                        switch(NCMAG_SensorType)
 782 
                        switch(NCMAG_SensorType)
771 
                        {
 783 
                        {
772 
                                case TYPE_HMC5843:                             
 784 
                                case TYPE_HMC5843:                             
773 
                                        NCMAG_GetMagVector();
 785 
                                        NCMAG_GetMagVector();
774 
                                        delay = 20;
 786 
                                        delay = 20;
775 
                                        break;
 787 
                                        break;
776 
                                case TYPE_LSM303DLH:
 788 
                                case TYPE_LSM303DLH:
777 
                                case TYPE_LSM303DLM:
 789 
                                case TYPE_LSM303DLM:
778 
                                        NCMAG_GetMagVector();
 790 
                                        NCMAG_GetMagVector();
779 
                                        delay = 20;
 791 
                                        delay = 20;
780 
/*                                      if(s){ NCMAG_GetMagVector(); s = 0;}
 792 
/*                                      if(s){ NCMAG_GetMagVector(); s = 0;}
781 
                                        else { NCMAG_GetAccVector(); s = 1;}
 793 
                                        else { NCMAG_GetAccVector(); s = 1;}
782 
                                        delay = 10;
 794 
                                        delay = 10;
783 
*/
 795 
*/
784 
                                        break;                           
 796 
                                        break;                           
785 
                        }
 797 
                        }
786 
                        if(send_config == 24) TimerUpdate = SetDelay(15);    // next event is the re-configuration
 798 
                        if(send_config == 24) TimerUpdate = SetDelay(15);    // next event is the re-configuration
787 
                        else TimerUpdate = SetDelay(delay);    // every 20 ms are 50 Hz
 799 
                        else TimerUpdate = SetDelay(delay);    // every 20 ms are 50 Hz
788 
                }
 800 
                }
789 
        }
 801 
        }
790 
}
 802 
}
791 
 
 803 
 
792 
 
 804 
 
793 
// --------------------------------------------------------
 805 
// --------------------------------------------------------
794 
u8 NCMAG_SelfTest(void)
 806 
u8 NCMAG_SelfTest(void)
795 
{
 807 
{
796 
        u8 msg[64];
 808 
        u8 msg[64];
797 
        static u8 done = 0;
 809 
        static u8 done = 0;
798 
 
 810 
 
799 
        if(done) return(1);        // just make it once
 811 
        if(done) return(1);        // just make it once
800 
       
 812 
       
801 
        #define LIMITS(value, min, max) {min = (80 * value)/100; max = (120 * value)/100;}
 813 
        #define LIMITS(value, min, max) {min = (80 * value)/100; max = (120 * value)/100;}
802 
        u32 time;
 814 
        u32 time;
803 
        s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
 815 
        s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
804 
        s16 xscale, yscale, zscale, scale_min, scale_max;
 816 
        s16 xscale, yscale, zscale, scale_min, scale_max;
805 
        u8 crb_gain, cra_rate;
 817 
        u8 crb_gain, cra_rate;
806 
        u8 i = 0, retval = 1;
 818 
        u8 i = 0, retval = 1;
807 
 
 819 
 
808 
        switch(NCMAG_SensorType)
 820 
        switch(NCMAG_SensorType)
809 
        {
 821 
        {
810 
                case TYPE_HMC5843:
 822 
                case TYPE_HMC5843:
811 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
 823 
                        crb_gain = HMC5843_CRB_GAIN_15GA;
812 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
 824 
                        cra_rate = HMC5843_CRA_RATE_50HZ;
813 
                        xscale = HMC5843_TEST_XSCALE;
 825 
                        xscale = HMC5843_TEST_XSCALE;
814 
                        yscale = HMC5843_TEST_YSCALE;
 826 
                        yscale = HMC5843_TEST_YSCALE;
815 
                        zscale = HMC5843_TEST_ZSCALE;
 827 
                        zscale = HMC5843_TEST_ZSCALE;
816 
                        break;
 828 
                        break;
817 
 
 829 
 
818 
                case TYPE_LSM303DLH:
 830 
                case TYPE_LSM303DLH:
819 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
 831 
                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
820 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
 832 
                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
821 
                        xscale = LSM303DLH_TEST_XSCALE;
 833 
                        xscale = LSM303DLH_TEST_XSCALE;
822 
                        yscale = LSM303DLH_TEST_YSCALE;
 834 
                        yscale = LSM303DLH_TEST_YSCALE;
823 
                        zscale = LSM303DLH_TEST_ZSCALE;
 835 
                        zscale = LSM303DLH_TEST_ZSCALE;
824 
                        break;
 836 
                        break;
825 
 
 837 
 
826 
                case TYPE_LSM303DLM:
 838 
                case TYPE_LSM303DLM:
827 
                        // does not support self test feature
839 
                        // does not support self test feature
828 
                        done = retval;
 840 
                        done = retval;
829 
                        return(retval);
 841 
                        return(retval);
830 
                        break;
 842 
                        break;
831 
 
 843 
 
832 
                default:
 844 
                default:
833 
                        return(0);
 845 
                        return(0);
834 
        }
 846 
        }
835 
 
 847 
 
836 
        MagConfig.cra = cra_rate|CRA_MODE_POSBIAS;
 848 
        MagConfig.cra = cra_rate|CRA_MODE_POSBIAS;
837 
        MagConfig.crb = crb_gain;
 849 
        MagConfig.crb = crb_gain;
838 
        MagConfig.mode = MODE_CONTINUOUS;
 850 
        MagConfig.mode = MODE_CONTINUOUS;
839 
        // activate positive bias field
 851 
        // activate positive bias field
840 
        NCMAG_SetMagConfig();
 852 
        NCMAG_SetMagConfig();
841 
        // wait for stable readings
 853 
        // wait for stable readings
842 
        time = SetDelay(50);
 854 
        time = SetDelay(50);
843 
        while(!CheckDelay(time));
 855 
        while(!CheckDelay(time));
844 
        // averaging
 856 
        // averaging
845 
        #define AVERAGE 20
 857 
        #define AVERAGE 20
846 
        for(i = 0; i<AVERAGE; i++)
 858 
        for(i = 0; i<AVERAGE; i++)
847 
        {
 859 
        {
848 
                NCMAG_GetMagVector();
 860 
                NCMAG_GetMagVector();
849 
                time = SetDelay(20);
 861 
                time = SetDelay(20);
850 
        while(!CheckDelay(time));
 862 
        while(!CheckDelay(time));
851 
                XMax += MagRawVector.X;
 863 
                XMax += MagRawVector.X;
852 
                YMax += MagRawVector.Y;
 864 
                YMax += MagRawVector.Y;
853 
                ZMax += MagRawVector.Z;
 865 
                ZMax += MagRawVector.Z;
854 
        }
 866 
        }
855 
        MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS;
 867 
        MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS;
856 
        // activate positive bias field
 868 
        // activate positive bias field
857 
        NCMAG_SetMagConfig();
 869 
        NCMAG_SetMagConfig();
858 
    // wait for stable readings
 870 
    // wait for stable readings
859 
        time = SetDelay(50);
 871 
        time = SetDelay(50);
860 
        while(!CheckDelay(time));
 872 
        while(!CheckDelay(time));
861 
        // averaging
 873 
        // averaging
862 
        for(i = 0; i < AVERAGE; i++)
 874 
        for(i = 0; i < AVERAGE; i++)
863 
        {
 875 
        {
864 
                NCMAG_GetMagVector();
 876 
                NCMAG_GetMagVector();
865 
                time = SetDelay(20);
 877 
                time = SetDelay(20);
866 
        while(!CheckDelay(time));
 878 
        while(!CheckDelay(time));
867 
                XMin += MagRawVector.X;
 879 
                XMin += MagRawVector.X;
868 
                YMin += MagRawVector.Y;
 880 
                YMin += MagRawVector.Y;
869 
                ZMin += MagRawVector.Z;
 881 
                ZMin += MagRawVector.Z;
870 
        }
 882 
        }
871 
        // setup final configuration
 883 
        // setup final configuration
872 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
 884 
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
873 
        // activate positive bias field
 885 
        // activate positive bias field
874 
        NCMAG_SetMagConfig();
 886 
        NCMAG_SetMagConfig();
875 
        // check scale for all axes
 887 
        // check scale for all axes
876 
        // prepare scale limits
 888 
        // prepare scale limits
877 
        LIMITS(xscale, scale_min, scale_max);
 889 
        LIMITS(xscale, scale_min, scale_max);
878 
        xscale = (XMax - XMin)/(2*AVERAGE);
 890 
        xscale = (XMax - XMin)/(2*AVERAGE);
879 
        if((xscale > scale_max) || (xscale < scale_min))
 891 
        if((xscale > scale_max) || (xscale < scale_min))
880 
    {
 892 
    {
881 
                retval = 0;
 893 
                retval = 0;
882 
        sprintf(msg, "\r\n Value X: %d not %d-%d !", xscale, scale_min,scale_max);
 894 
        sprintf(msg, "\r\n Value X: %d not %d-%d !", xscale, scale_min,scale_max);
883 
                UART1_PutString(msg);
 895 
                UART1_PutString(msg);
884 
    }
 896 
    }
885 
        LIMITS(yscale, scale_min, scale_max);
 897 
        LIMITS(yscale, scale_min, scale_max);
886 
        yscale = (YMax - YMin)/(2*AVERAGE);
 898 
        yscale = (YMax - YMin)/(2*AVERAGE);
887 
        if((yscale > scale_max) || (yscale < scale_min))
 899 
        if((yscale > scale_max) || (yscale < scale_min))
888 
    {
 900 
    {
889 
                retval = 0;
 901 
                retval = 0;
890 
        sprintf(msg, "\r\n Value Y: %d not %d-%d !", yscale, scale_min,scale_max);
 902 
        sprintf(msg, "\r\n Value Y: %d not %d-%d !", yscale, scale_min,scale_max);
891 
                UART1_PutString(msg);
 903 
                UART1_PutString(msg);
892 
    }
 904 
    }
893 
        LIMITS(zscale, scale_min, scale_max);
 905 
        LIMITS(zscale, scale_min, scale_max);
894 
        zscale = (ZMax - ZMin)/(2*AVERAGE);
 906 
        zscale = (ZMax - ZMin)/(2*AVERAGE);
895 
        if((zscale > scale_max) || (zscale < scale_min))      
 907 
        if((zscale > scale_max) || (zscale < scale_min))      
896 
        {
 908 
        {
897 
                retval = 0;
 909 
                retval = 0;
898 
        sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
 910 
        sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
899 
                UART1_PutString(msg);
 911 
                UART1_PutString(msg);
900 
    }
 912 
    }
901 
        done = retval;
 913 
        done = retval;
902 
        return(retval);
 914 
        return(retval);
903 
}
 915 
}
904 
 
 916 
 
905 
 
 917 
 
906 
//----------------------------------------------------------------
 918 
//----------------------------------------------------------------
- 
 
 919 
void NCMAG_SelectI2CBus(u8 busno)
- 
 
 920 
{
- 
 
 921 
  if (busno == 0)
- 
 
 922 
  {
- 
 
 923 
    I2C_WaitForEndOfTransmissionFunc = &I2C0_WaitForEndOfTransmission;
- 
 
 924 
        I2C_LockBufferFunc = &I2C0_LockBuffer;
- 
 
 925 
        I2C_TransmissionFunc = &I2C0_Transmission;
- 
 
 926 
        I2C_BufferPnt = I2C0_Buffer;
- 
 
 927 
        I2C_ErrorPnt = &I2C0_Error;
- 
 
 928 
  }
- 
 
 929 
   else
- 
 
 930 
  {
- 
 
 931 
    I2C_WaitForEndOfTransmissionFunc = &I2C_WaitForEndOfTransmission;
- 
 
 932 
        I2C_LockBufferFunc = &I2C_LockBuffer;
- 
 
 933 
        I2C_TransmissionFunc = &I2C_Transmission;
- 
 
 934 
        I2C_BufferPnt = I2C_Buffer;
- 
 
 935 
        I2C_ErrorPnt = &I2C_Error;
- 
 
 936 
  }
- 
 
 937 
}
- 
 
 938 
 
- 
 
 939 
//----------------------------------------------------------------
907 
u8 NCMAG_Init(void)
 940 
u8 NCMAG_Init(void)
908 
{
 941 
{
909 
        u8 msg[64];
 942 
        u8 msg[64];
910 
        u8 retval = 0;
 943 
        u8 retval = 0;
911 
        u8 repeat;
 944 
        u8 repeat;
- 
 
 945 
 
- 
 
 946 
        NCMAG_SelectI2CBus(0);
912 
 
 947 
 
913 
        NCMAG_Present = 0;
 948 
        NCMAG_Present = 0;
914 
        NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
 949 
        NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
915 
        // polling for LSM302DLH/DLM option by ACC address ack
 950 
        // polling for LSM302DLH/DLM option by ACC address ack
916 
        repeat = 0;
 951 
        repeat = 0;
917 
        do
 952 
        do
918 
        {
 953 
        {
919 
                retval = NCMAG_GetAccConfig();
 954 
                retval = NCMAG_GetAccConfig();
920 
                if(retval) break; // break loop on success
 955 
                if(retval) break; // break loop on success
921 
                UART1_PutString(".");
 956 
                UART1_PutString("*");
922 
                repeat++;
 957 
                repeat++;
923 
        }while(repeat < 3);
 958 
        }while(repeat < 3);
924 
        if(retval)
 959 
        if(retval)
925 
        {
 960 
        {
926 
                // initialize ACC sensor
 961 
                // initialize ACC sensor
927 
                NCMAG_Init_ACCSensor();
 962 
                NCMAG_Init_ACCSensor();
928 
 
 963 
 
929 
                NCMAG_SensorType = TYPE_LSM303DLH;     
 964 
                NCMAG_SensorType = TYPE_LSM303DLH;     
930 
                // polling of sub identification
 965 
                // polling of sub identification
931 
                repeat = 0;
 966 
                repeat = 0;
932 
                do
 967 
                do
933 
                {
 968 
                {
934 
                        retval = NCMAG_GetIdentification_Sub();
 969 
                        retval = NCMAG_GetIdentification_Sub();
935 
                        if(retval) break; // break loop on success
 970 
                        if(retval) break; // break loop on success
936 
                        UART1_PutString(".");
 971 
                        UART1_PutString(".");
937 
                        repeat++;
 972 
                        repeat++;
938 
                }while(repeat < 12);
 973 
                }while(repeat < 12);
939 
                if(retval)
 974 
                if(retval)
940 
                {
 975 
                {
941 
                        if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM)      NCMAG_SensorType = TYPE_LSM303DLM;
 976 
                        if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM)      NCMAG_SensorType = TYPE_LSM303DLM;
942 
                }      
 977 
                }      
943 
        }
 978 
        }
944 
        // get id bytes
 979 
        // get id bytes
945 
        retval = 0;
 980 
        retval = 0;
946 
        do
 981 
        do
947 
        {
 982 
        {
948 
                retval = NCMAG_GetIdentification();
 983 
                retval = NCMAG_GetIdentification();
949 
                if(retval) break; // break loop on success
 984 
                if(retval) break; // break loop on success
950 
                UART1_PutString(".");
 985 
                UART1_PutString("#");
951 
                repeat++;
 986 
                repeat++;
952 
        }while(repeat < 12);
 987 
        }while(repeat < 12);
953 
 
 988 
 
954 
        // if we got an answer to id request
 989 
        // if we got an answer to id request
955 
        if(retval)
 990 
        if(retval)
956 
        {
 991 
        {
957 
                u8 n1[] = "\n\r HMC5843";
 992 
                u8 n1[] = "\n\r HMC5843";
958 
                u8 n2[] = "\n\r LSM303DLH";
 993 
                u8 n2[] = "\n\r LSM303DLH";
959 
                u8 n3[] = "\n\r LSM303DLM";
 994 
                u8 n3[] = "\n\r LSM303DLM";
960 
                u8* pn = n1;
 995 
                u8* pn = n1;
961 
               
 996 
               
962 
                switch(NCMAG_SensorType)
 997 
                switch(NCMAG_SensorType)
963 
                {
 998 
                {
964 
                        case TYPE_HMC5843:
 999 
                        case TYPE_HMC5843:
965 
                                pn = n1;
 1000 
                                pn = n1;
966 
                                break;
 1001 
                                break;
967 
                        case TYPE_LSM303DLH:
 1002 
                        case TYPE_LSM303DLH:
968 
                                pn = n2;
 1003 
                                pn = n2;
969 
                                break;
 1004 
                                break;
970 
                        case TYPE_LSM303DLM:
 1005 
                        case TYPE_LSM303DLM:
971 
                                pn = n3;
 1006 
                                pn = n3;
972 
                                break;
 1007 
                                break;
973 
                }
 1008 
                }
974 
 
 1009 
 
975 
                sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
 1010 
                sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
976 
                UART1_PutString(msg);
 1011 
                UART1_PutString(msg);
977 
                if (    (NCMAG_Identification.A == MAG_IDA)
 1012 
                if (    (NCMAG_Identification.A == MAG_IDA)
978 
                     && (NCMAG_Identification.B == MAG_IDB)
 1013 
                     && (NCMAG_Identification.B == MAG_IDB)
979 
                         && (NCMAG_Identification.C == MAG_IDC))
 1014 
                         && (NCMAG_Identification.C == MAG_IDC))
980 
                {
 1015 
                {
981 
                        NCMAG_Present = 1;
 1016 
                        NCMAG_Present = 1;
982 
 
 1017 
 
983 
                        if(EEPROM_Init())
 1018 
                        if(EEPROM_Init())
984 
                        {
 1019 
                        {
985 
                                NCMAG_IsCalibrated = NCMag_CalibrationRead();
 1020 
                                NCMAG_IsCalibrated = NCMag_CalibrationRead();
986 
                                if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
 1021 
                                if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
987 
                        }
 1022 
                        }
988 
                        else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
 1023 
                        else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
989 
                        // perform self test
 1024 
                        // perform self test
990 
                        if(!NCMAG_SelfTest())
 1025 
                        if(!NCMAG_SelfTest())
991 
                        {
 1026 
                        {
992 
                                UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
 1027 
                                UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
993 
                                LED_RED_ON;
 1028 
                                LED_RED_ON;
994 
                                NCMAG_IsCalibrated = 0;
 1029 
                                NCMAG_IsCalibrated = 0;
995 
                        }
 1030 
                        }
996 
                        else UART1_PutString("\r\n Selftest ok");
 1031 
                        else UART1_PutString("\r\n Selftest ok");
997 
 
 1032 
 
998 
                        // initialize magnetic sensor configuration
 1033 
                        // initialize magnetic sensor configuration
999 
                        InitNC_MagnetSensor();
 1034 
                        InitNC_MagnetSensor();
1000 
                }
 1035 
                }
1001 
                else
 1036 
                else
1002 
                {
 1037 
                {
1003 
                        UART1_PutString("\n\r Not compatible!");
 1038 
                        UART1_PutString("\n\r Not compatible!");
1004 
                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
 1039 
                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
1005 
                        LED_RED_ON;
 1040 
                        LED_RED_ON;
1006 
                }
 1041 
                }
1007 
        }
 1042 
        }
1008 
        else // nothing found
 1043 
        else // nothing found
1009 
        {
 1044 
        {
1010 
                NCMAG_SensorType = TYPE_NONE;
 1045 
                NCMAG_SensorType = TYPE_NONE;
1011 
                UART1_PutString("not found!");  
 1046 
                UART1_PutString("not found!");  
1012 
        }
 1047 
        }
1013 
        return(NCMAG_Present);
 1048 
        return(NCMAG_Present);
1014 
}
 1049 
}
1015 
 
 1050 
 
1016 
 
 1051