Subversion Repositories NaviCtrl

Rev

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

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