Subversion Repositories NaviCtrl

Rev

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

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