/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!! */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software Nutzungsbedingungen (english version: see below)
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
// + des Mitverschuldens offen.
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
// + 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.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software LICENSING TERMS
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
// + 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
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
// + The Software may only be used with the Licensor's products.
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
// + agreement shall be the property of the Licensor.
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
// + features that can be used to identify the program may not be altered or defaced by the customer.
// + The customer shall be responsible for taking reasonable precautions
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
// + #### END OF LICENSING TERMS ####
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "91x_lib.h"
#include "ncmag.h"
#include "i2c.h"
#include "timer1.h"
#include "led.h"
#include "uart1.h"
#include "eeprom.h"
#include "mymath.h"
#include "main.h"
#include "spi_slave.h"
u8 NCMAG_Present
= 0;
u8 NCMAG_IsCalibrated
= 0;
// supported magnetic sensor types
#define TYPE_NONE 0
#define TYPE_HMC5843 1
#define TYPE_LSM303DLH 2
#define TYPE_LSM303DLM 3
u8 NCMAG_SensorType
= TYPE_NONE
;
u8 NCMAG_Orientation
= 0; // 0 means unknown!
// two calibrtion sets for extern and intern sensor
#define EEPROM_ADR_MAG_CALIBRATION_INTERN 50
#define EEPROM_ADR_MAG_CALIBRATION_EXTERN 70
#define CALIBRATION_VERSION 1
#define MAG_CALIBRATION_COMPATIBLE 0xA3
#define NCMAG_MIN_RAWVALUE -2047
#define NCMAG_MAX_RAWVALUE 2047
#define NCMAG_INVALID_DATA -4096
typedef struct
{
s16 Range
;
s16 Offset
;
} __attribute__
((packed
)) Scaling_t
;
typedef struct
{
Scaling_t MagX
;
Scaling_t MagY
;
Scaling_t MagZ
;
u8 Version
;
u8 crc
;
} __attribute__
((packed
)) Calibration_t
;
Calibration_t Calibration
; // calibration data in RAM
volatile s16vec_t AccRawVector
;
volatile s16vec_t MagRawVector
;
// i2c MAG interface
#define MAG_SLAVE_ADDRESS 0x3C // i2C slave address mag. sensor registers
// register mapping
#define REG_MAG_CRA 0x00
#define REG_MAG_CRB 0x01
#define REG_MAG_MODE 0x02
#define REG_MAG_DATAX_MSB 0x03
#define REG_MAG_DATAX_LSB 0x04
#define REG_MAG_DATAY_MSB 0x05
#define REG_MAG_DATAY_LSB 0x06
#define REG_MAG_DATAZ_MSB 0x07
#define REG_MAG_DATAZ_LSB 0x08
#define REG_MAG_STATUS 0x09
#define REG_MAG_IDA 0x0A
#define REG_MAG_IDB 0x0B
#define REG_MAG_IDC 0x0C
#define REG_MAG_IDF 0x0F // WHO_AM_I _M = 0x03c when LSM303DLM is connected
// bit mask for configuration mode
#define CRA_MODE_MASK 0x03
#define CRA_MODE_NORMAL 0x00 //default
#define CRA_MODE_POSBIAS 0x01
#define CRA_MODE_NEGBIAS 0x02
#define CRA_MODE_SELFTEST 0x03
// bit mask for measurement mode
#define MODE_MASK 0xFF
#define MODE_CONTINUOUS 0x00
#define MODE_SINGLE 0x01 // default
#define MODE_IDLE 0x02
#define MODE_SLEEP 0x03
// bit mask for rate
#define CRA_RATE_MASK 0x1C
// bit mask for gain
#define CRB_GAIN_MASK 0xE0
// ids
#define MAG_IDA 0x48
#define MAG_IDB 0x34
#define MAG_IDC 0x33
#define MAG_IDF_LSM303DLM 0x3C
// the special HMC5843 interface
// bit mask for rate
#define HMC5843_CRA_RATE_0_5HZ 0x00
#define HMC5843_CRA_RATE_1HZ 0x04
#define HMC5843_CRA_RATE_2HZ 0x08
#define HMC5843_CRA_RATE_5HZ 0x0C
#define HMC5843_CRA_RATE_10HZ 0x10 //default
#define HMC5843_CRA_RATE_20HZ 0x14
#define HMC5843_CRA_RATE_50HZ 0x18
// bit mask for gain
#define HMC5843_CRB_GAIN_07GA 0x00
#define HMC5843_CRB_GAIN_10GA 0x20 //default
#define HMC5843_CRB_GAIN_15GA 0x40 // <--- we use this
#define HMC5843_CRB_GAIN_20GA 0x60
#define HMC5843_CRB_GAIN_32GA 0x80
#define HMC5843_CRB_GAIN_38GA 0xA0
#define HMC5843_CRB_GAIN_45GA 0xC0
#define HMC5843_CRB_GAIN_65GA 0xE0
// self test value
#define HMC5843_TEST_XSCALE 555
#define HMC5843_TEST_YSCALE 555
#define HMC5843_TEST_ZSCALE 555
// calibration range
#define HMC5843_CALIBRATION_RANGE 600
// the special LSM302DLH interface
// bit mask for rate
#define LSM303DLH_CRA_RATE_0_75HZ 0x00
#define LSM303DLH_CRA_RATE_1_5HZ 0x04
#define LSM303DLH_CRA_RATE_3_0HZ 0x08
#define LSM303DLH_CRA_RATE_7_5HZ 0x0C
#define LSM303DLH_CRA_RATE_15HZ 0x10 //default
#define LSM303DLH_CRA_RATE_30HZ 0x14
#define LSM303DLH_CRA_RATE_75HZ 0x18
// bit mask for gain
#define LSM303DLH_CRB_GAIN_XXGA 0x00
#define LSM303DLH_CRB_GAIN_13GA 0x20 //default
#define LSM303DLH_CRB_GAIN_19GA 0x40
#define LSM303DLH_CRB_GAIN_25GA 0x60
#define LSM303DLH_CRB_GAIN_40GA 0x80 // <--- we use this (Since V2.03)
#define LSM303DLH_CRB_GAIN_47GA 0xA0
#define LSM303DLH_CRB_GAIN_56GA 0xC0
#define LSM303DLH_CRB_GAIN_81GA 0xE0
typedef struct
{
u8 A
;
u8 B
;
u8 C
;
} __attribute__
((packed
)) Identification_t
;
volatile Identification_t NCMAG_Identification
;
typedef struct
{
u8 Sub
;
} __attribute__
((packed
)) Identification2_t
;
volatile Identification2_t NCMAG_Identification2
;
typedef struct
{
u8 cra
;
u8 crb
;
u8 mode
;
} __attribute__
((packed
)) MagConfig_t
;
volatile MagConfig_t MagConfig
;
// self test value
#define LSM303DLH_TEST_XSCALE 245
#define LSM303DLH_TEST_YSCALE 245
#define LSM303DLH_TEST_ZSCALE 235
// clibration range
#define LSM303_CALIBRATION_RANGE 300 // War bis V2.02: 550 -> Auflösung von 19Ga auf 40GA reduziert
// the i2c ACC interface
#define ACC_SLAVE_ADDRESS 0x30 // i2c slave for acc. sensor registers
// multiple byte read/write mask
#define REG_ACC_MASK_AUTOINCREMENT 0x80
// register mapping
#define REG_ACC_CTRL1 0x20
#define REG_ACC_CTRL2 0x21
#define REG_ACC_CTRL3 0x22
#define REG_ACC_CTRL4 0x23
#define REG_ACC_CTRL5 0x24
#define REG_ACC_HP_FILTER_RESET 0x25
#define REG_ACC_REFERENCE 0x26
#define REG_ACC_STATUS 0x27
#define REG_ACC_X_LSB 0x28
#define REG_ACC_X_MSB 0x29
#define REG_ACC_Y_LSB 0x2A
#define REG_ACC_Y_MSB 0x2B
#define REG_ACC_Z_LSB 0x2C
#define REG_ACC_Z_MSB 0x2D
#define ACC_CRTL1_PM_DOWN 0x00
#define ACC_CRTL1_PM_NORMAL 0x20
#define ACC_CRTL1_PM_LOW_0_5HZ 0x40
#define ACC_CRTL1_PM_LOW_1HZ 0x60
#define ACC_CRTL1_PM_LOW_2HZ 0x80
#define ACC_CRTL1_PM_LOW_5HZ 0xA0
#define ACC_CRTL1_PM_LOW_10HZ 0xC0
// Output data rate in normal power mode
#define ACC_CRTL1_DR_50HZ 0x00
#define ACC_CRTL1_DR_100HZ 0x08
#define ACC_CRTL1_DR_400HZ 0x10
#define ACC_CRTL1_DR_1000HZ 0x18
// axis anable flags
#define ACC_CRTL1_XEN 0x01
#define ACC_CRTL1_YEN 0x02
#define ACC_CRTL1_ZEN 0x04
#define ACC_CRTL2_FILTER8 0x10
#define ACC_CRTL2_FILTER16 0x11
#define ACC_CRTL2_FILTER32 0x12
#define ACC_CRTL2_FILTER64 0x13
#define ACC_CTRL4_BDU 0x80 // Block data update, (0: continuos update; 1: output registers not updated between MSB and LSB reading)
#define ACC_CTRL4_BLE 0x40 // Big/little endian, (0: data LSB @ lower address; 1: data MSB @ lower address)
#define ACC_CTRL4_FS_2G 0x00
#define ACC_CTRL4_FS_4G 0x10
#define ACC_CTRL4_FS_8G 0x30
#define ACC_CTRL4_STSIGN_PLUS 0x00
#define ACC_CTRL4_STSIGN_MINUS 0x08
#define ACC_CTRL4_ST_ENABLE 0x02
#define ACC_CTRL5_STW_ON 0x03
#define ACC_CTRL5_STW_OFF 0x00
typedef struct
{
u8 ctrl_1
;
u8 ctrl_2
;
u8 ctrl_3
;
u8 ctrl_4
;
u8 ctrl_5
;
} __attribute__
((packed
)) AccConfig_t
;
volatile AccConfig_t AccConfig
;
// write calibration data for external and internal sensor seperately
u8 NCMag_CalibrationWrite
(I2C_TypeDef
* I2Cx
)
{
u16 address
;
u8 i
= 0, crc
= MAG_CALIBRATION_COMPATIBLE
;
EEPROM_Result_t eres
;
u8
*pBuff
= (u8
*)&Calibration
;
if (I2Cx
== NCMAG_PORT_EXTERN
)
{
address
= EEPROM_ADR_MAG_CALIBRATION_EXTERN
;
Calibration.
Version = CALIBRATION_VERSION
+ (NCMAG_Orientation
<<4);;
}
else if (I2Cx
== NCMAG_PORT_INTERN
)
{
address
= EEPROM_ADR_MAG_CALIBRATION_INTERN
;
Calibration.
Version = CALIBRATION_VERSION
;
}
else return(i
);
for(i
= 0; i
<(sizeof(Calibration
)-1); i
++)
{
crc
+= pBuff
[i
];
}
Calibration.
crc = ~crc
;
eres
= EEPROM_WriteBlock
(address
, pBuff
, sizeof(Calibration
));
if(EEPROM_SUCCESS
== eres
) i
= 1;
else i
= 0;
return(i
);
}
// read calibration data for external and internal sensor seperately
u8 NCMag_CalibrationRead
(I2C_TypeDef
* I2Cx
)
{
u8 address
;
u8 i
= 0, crc
= MAG_CALIBRATION_COMPATIBLE
;
u8
*pBuff
= (u8
*)&Calibration
;
if (I2Cx
== NCMAG_PORT_EXTERN
) address
= EEPROM_ADR_MAG_CALIBRATION_EXTERN
;
else if (I2Cx
== NCMAG_PORT_INTERN
) address
= EEPROM_ADR_MAG_CALIBRATION_INTERN
;
else return(0);
if(EEPROM_SUCCESS
== EEPROM_ReadBlock
(address
, pBuff
, sizeof(Calibration
)))
{
for(i
= 0; i
<(sizeof(Calibration
)-1); i
++)
{
crc
+= pBuff
[i
];
}
crc
= ~crc
;
if(Calibration.
crc != crc
) return(0); // crc mismatch
if((Calibration.
Version & 0x0F) == CALIBRATION_VERSION
) return(1);
}
return(0);
}
void NCMAG_Calibrate
(void)
{
u8 msg
[64];
static u8 speak
= 0;
static s16 Xmin
= 0, Xmax
= 0, Ymin
= 0, Ymax
= 0, Zmin
= 0, Zmax
= 0, Zmin2
= 0, Zmax2
= 0;;
static s16 X
= 0, Y
= 0, Z
= 0;
static u8 OldCalState
= 0;
s16 MinCalibration
= 450;
X
= (X
+ MagRawVector.
X)/2;
Y
= (Y
+ MagRawVector.
Y)/2;
Z
= (Z
+ MagRawVector.
Z)/2;
switch(Compass_CalState
)
{
case 1:
// 1st step of calibration
// initialize ranges
// used to change the orientation of the NC in the horizontal plane
Xmin
= 10000;
Xmax
= -10000;
Ymin
= 10000;
Ymax
= -10000;
Zmin
= 10000;
Zmax
= -10000;
Zmin2
= 10000;
Zmax2
= -10000;
X
= 0; Y
= 0; Z
= 0;
speak
= 1;
CompassValueErrorCount
= 0;
if(Compass_CalState
!= OldCalState
) // only once per state
{
UART1_PutString
("\r\nStarting compass calibration");
if(Compass_I2CPort
== NCMAG_PORT_EXTERN
)
{
if(!NCMAG_Orientation
) NCMAG_Orientation
= NCMAG_GetOrientationFromAcc
();
UART1_PutString
(" - External sensor ");
sprintf(msg
, "with orientation: %d ", NCMAG_Orientation
);
UART1_PutString
(msg
);
}
else UART1_PutString
(" - Internal sensor ");
}
break;
case 2: // 2nd step of calibration
// find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
if(X
< Xmin
) { Xmin
= X
; BeepTime
= 20;}
else if(X
> Xmax
) { Xmax
= X
; BeepTime
= 20;}
if(Y
< Ymin
) { Ymin
= Y
; BeepTime
= 60;}
else if(Y
> Ymax
) { Ymax
= Y
; BeepTime
= 60;}
if(Z
< Zmin
) { Zmin
= Z
; } // silent
else if(Z
> Zmax
) { Zmax
= Z
; }
if(speak
) SpeakHoTT
= SPEAK_CALIBRATE
; speak
= 0;
break;
case 3: // 3rd step of calibration
// used to change the orientation of the MK3MAG vertical to the horizontal plane
speak
= 1;
break;
case 4:
// find Min and Max of the Z-Sensor
if(Z
< Zmin2
) { Zmin2
= Z
; BeepTime
= 80;}
else if(Z
> Zmax2
) { Zmax2
= Z
; BeepTime
= 80;}
if(X
< Xmin
) { Xmin
= X
; BeepTime
= 20;}
else if(X
> Xmax
) { Xmax
= X
; BeepTime
= 20;}
if(Y
< Ymin
) { Ymin
= Y
; BeepTime
= 60;}
else if(Y
> Ymax
) { Ymax
= Y
; BeepTime
= 60;}
if(speak
) SpeakHoTT
= SPEAK_CALIBRATE
; speak
= 0;
break;
case 5:
// Save values
if(Compass_CalState
!= OldCalState
) // avoid continously writing of eeprom!
{
switch(NCMAG_SensorType
)
{
case TYPE_HMC5843
:
UART1_PutString
("\r\nFinished: HMC5843 calibration\n\r");
MinCalibration
= HMC5843_CALIBRATION_RANGE
;
break;
case TYPE_LSM303DLH
:
case TYPE_LSM303DLM
:
UART1_PutString
("\r\nFinished: LSM303 calibration\n\r");
MinCalibration
= LSM303_CALIBRATION_RANGE
;
break;
}
if(EarthMagneticStrengthTheoretic
)
{
MinCalibration
= (MinCalibration
* EarthMagneticStrengthTheoretic
) / 50;
sprintf(msg
, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic
);
UART1_PutString
(msg
);
}
else UART1_PutString
("without GPS\n\r");
if(Zmin2
< Zmin
) Zmin
= Zmin2
;
if(Zmax2
> Zmax
) Zmax
= Zmax2
;
Calibration.
MagX.
Range = Xmax
- Xmin
;
Calibration.
MagX.
Offset = (Xmin
+ Xmax
) / 2;
Calibration.
MagY.
Range = Ymax
- Ymin
;
Calibration.
MagY.
Offset = (Ymin
+ Ymax
) / 2;
Calibration.
MagZ.
Range = Zmax
- Zmin
;
Calibration.
MagZ.
Offset = (Zmin
+ Zmax
) / 2;
if(CompassValueErrorCount
)
{
SpeakHoTT
= SPEAK_ERR_CALIBARTION
;
UART1_PutString
("\r\nCalibration FAILED - Compass sensor error !!!!\r\n ");
}
else
if((Calibration.
MagX.
Range > MinCalibration
) && (Calibration.
MagY.
Range > MinCalibration
) && (Calibration.
MagZ.
Range > MinCalibration
))
{
NCMAG_IsCalibrated
= NCMag_CalibrationWrite
(Compass_I2CPort
);
BeepTime
= 2500;
UART1_PutString
("\r\n-> Calibration okay <-\n\r");
SpeakHoTT
= SPEAK_MIKROKOPTER
;
}
else
{
SpeakHoTT
= SPEAK_ERR_CALIBARTION
;
UART1_PutString
("\r\nCalibration FAILED - Values too low: ");
if(Calibration.
MagX.
Range < MinCalibration
) UART1_PutString
("X! ");
if(Calibration.
MagY.
Range < MinCalibration
) UART1_PutString
("Y! ");
if(Calibration.
MagZ.
Range < MinCalibration
) UART1_PutString
("Z! ");
UART1_PutString
("\r\n");
// restore old calibration data from eeprom
NCMAG_IsCalibrated
= NCMag_CalibrationRead
(Compass_I2CPort
);
}
sprintf(msg
, "X: (%i - %i = %i)\r\n",Xmax
,Xmin
,Xmax
- Xmin
);
UART1_PutString
(msg
);
sprintf(msg
, "Y: (%i - %i = %i)\r\n",Ymax
,Ymin
,Ymax
- Ymin
);
UART1_PutString
(msg
);
sprintf(msg
, "Z: (%i - %i = %i)\r\n",Zmax
,Zmin
,Zmax
- Zmin
);
UART1_PutString
(msg
);
sprintf(msg
, "(Minimum ampilitude is: %i)\r\n",MinCalibration
);
UART1_PutString
(msg
);
}
break;
default:
break;
}
OldCalState
= Compass_CalState
;
}
// ---------- call back handlers -----------------------------------------
// rx data handler for id info request
void NCMAG_UpdateIdentification
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if number of bytes are matching
if(RxBufferSize
== sizeof(NCMAG_Identification
) )
{
memcpy((u8
*)&NCMAG_Identification
, pRxBuffer
, sizeof(NCMAG_Identification
));
}
}
void NCMAG_UpdateIdentification_Sub
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if number of bytes are matching
if(RxBufferSize
== sizeof(NCMAG_Identification2
))
{
memcpy((u8
*)&NCMAG_Identification2
, pRxBuffer
, sizeof(NCMAG_Identification2
));
}
}
// rx data handler for magnetic sensor raw data
void NCMAG_UpdateMagVector
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if number of bytes are matching
if(RxBufferSize
== sizeof(MagRawVector
) )
{ // byte order from big to little endian
s16 raw
, X
= 0, Y
= 0, Z
= 0;
raw
= pRxBuffer
[0]<<8;
raw
+= pRxBuffer
[1];
if(raw
>= NCMAG_MIN_RAWVALUE
&& raw
<= NCMAG_MAX_RAWVALUE
) X
= raw
;
else if(CompassValueErrorCount
< 35) CompassValueErrorCount
++; // invalid data
raw
= pRxBuffer
[2]<<8;
raw
+= pRxBuffer
[3];
if(raw
>= NCMAG_MIN_RAWVALUE
&& raw
<= NCMAG_MAX_RAWVALUE
)
{
if(NCMAG_SensorType
== TYPE_LSM303DLM
) Z
= raw
; // here Z and Y are exchanged
else Y
= raw
;
}
else if(CompassValueErrorCount
< 35) CompassValueErrorCount
++; // invalid data
raw
= pRxBuffer
[4]<<8;
raw
+= pRxBuffer
[5];
if(raw
>= NCMAG_MIN_RAWVALUE
&& raw
<= NCMAG_MAX_RAWVALUE
)
{
if(NCMAG_SensorType
== TYPE_LSM303DLM
) Y
= raw
; // here Z and Y are exchanged
else Z
= raw
;
}
else if(CompassValueErrorCount
< 35) CompassValueErrorCount
++; // invalid data
// correct compass orientation
switch(NCMAG_Orientation
)
{
case 0:
case 1:
default:
// 1:1 Mapping
MagRawVector.
X = X
;
MagRawVector.
Y = Y
;
MagRawVector.
Z = Z
;
break;
case 2:
MagRawVector.
X = -X
;
MagRawVector.
Y = Y
;
MagRawVector.
Z = -Z
;
break;
case 3:
MagRawVector.
X = -Z
;
MagRawVector.
Y = Y
;
MagRawVector.
Z = X
;
break;
case 4:
MagRawVector.
X = Z
;
MagRawVector.
Y = Y
;
MagRawVector.
Z = -X
;
break;
case 5:
MagRawVector.
X = X
;
MagRawVector.
Y = -Z
;
MagRawVector.
Z = Y
;
break;
case 6:
MagRawVector.
X = -X
;
MagRawVector.
Y = -Z
;
MagRawVector.
Z = -Y
;
break;
}
}
if(Compass_CalState
|| !NCMAG_IsCalibrated
)
{ // mark out data invalid
MagVector.
X = MagRawVector.
X;
MagVector.
Y = MagRawVector.
Y;
MagVector.
Z = MagRawVector.
Z;
Compass_Heading
= -1;
}
else
{
// update MagVector from MagRaw Vector by Scaling
MagVector.
X = (s16
)((1024L*(s32
)(MagRawVector.
X - Calibration.
MagX.
Offset))/Calibration.
MagX.
Range);
MagVector.
Y = (s16
)((1024L*(s32
)(MagRawVector.
Y - Calibration.
MagY.
Offset))/Calibration.
MagY.
Range);
MagVector.
Z = (s16
)((1024L*(s32
)(MagRawVector.
Z - Calibration.
MagZ.
Offset))/Calibration.
MagZ.
Range);
Compass_CalcHeading
();
}
}
// rx data handler for acceleration raw data
void NCMAG_UpdateAccVector
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if number of bytes are matching
if(RxBufferSize
== sizeof(AccRawVector
) )
{
// copy from I2C buffer
memcpy((u8
*)&AccRawVector
, pRxBuffer
,sizeof(AccRawVector
));
// scale and update Acc Vector, at the moment simply 1:1
memcpy((u8
*)&AccVector
, (u8
*)&AccRawVector
,sizeof(AccRawVector
));
}
}
// rx data handler for reading magnetic sensor configuration
void NCMAG_UpdateMagConfig
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if number of byte are matching
if(RxBufferSize
== sizeof(MagConfig
) )
{
memcpy((u8
*)(&MagConfig
), pRxBuffer
, sizeof(MagConfig
));
}
}
// rx data handler for reading acceleration sensor configuration
void NCMAG_UpdateAccConfig
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if number of byte are matching
if(RxBufferSize
== sizeof(AccConfig
) )
{
memcpy((u8
*)&AccConfig
, pRxBuffer
, sizeof(AccConfig
));
}
}
//----------------------------------------------------------------------
u8 NCMAG_GetOrientationFromAcc
(void)
{
// only if external compass connected
if(Compass_I2CPort
!= NCMAG_PORT_EXTERN
) return(0);
// MK must not be tilted
if((abs(FromFlightCtrl.
AngleNick) > 300) || (abs(FromFlightCtrl.
AngleRoll) > 300))
{
// UART1_PutString("\r\nTilted");
return(0);
}
// select orientation
if(AccRawVector.
Z > 3300) return(1); // Flach - Bestückung oben - Pfeil nach vorn
else
if(AccRawVector.
Z < -3300) return(2); // Flach - Bestückung unten - Pfeil nach vorn
else
if(AccRawVector.
X > 3300) return(3); // Flach - Bestückung Links - Pfeil nach vorn
else
if(AccRawVector.
X < -3300) return(4); // Flach - Bestückung rechts - Pfeil nach vorn
else
if(AccRawVector.
Y > 3300) return(5); // Stehend - Pfeil nach oben - 'front' nach vorn
else
if(AccRawVector.
Y < -3300) return(6); // Stehend - Pfeil nach unten - 'front' nach vorn
return(0);
}
// ---------------------------------------------------------------------
u8 NCMAG_SetMagConfig
(void)
{
u8 retval
= 0;
// try to catch the i2c buffer within 100 ms timeout
if(I2CBus_LockBuffer
(Compass_I2CPort
, 100))
{
u8 TxBytes
= 0;
u8 TxData
[sizeof(MagConfig
) + 3];
TxData
[TxBytes
++] = REG_MAG_CRA
;
memcpy(&TxData
[TxBytes
], (u8
*)&MagConfig
, sizeof(MagConfig
));
TxBytes
+= sizeof(MagConfig
);
if(I2CBus_Transmission
(Compass_I2CPort
, MAG_SLAVE_ADDRESS
, TxData
, TxBytes
, 0, 0))
{
if(I2CBus_WaitForEndOfTransmission
(Compass_I2CPort
, 100))
{
if(I2CBus
(Compass_I2CPort
)->Error
== I2C_ERROR_NONE
) retval
= 1;
}
}
}
return(retval
);
}
// ----------------------------------------------------------------------------------------
u8 NCMAG_GetMagConfig
(void)
{
u8 retval
= 0;
// try to catch the i2c buffer within 100 ms timeout
if(I2CBus_LockBuffer
(Compass_I2CPort
, 100))
{
u8 TxBytes
= 0;
u8 TxData
[3];
TxData
[TxBytes
++] = REG_MAG_CRA
;
if(I2CBus_Transmission
(Compass_I2CPort
, MAG_SLAVE_ADDRESS
, TxData
, TxBytes
, &NCMAG_UpdateMagConfig
, sizeof(MagConfig
)))
{
if(I2CBus_WaitForEndOfTransmission
(Compass_I2CPort
, 100))
{
if(I2CBus
(Compass_I2CPort
)->Error
== I2C_ERROR_NONE
) retval
= 1;
}
}
}
return(retval
);
}
// ----------------------------------------------------------------------------------------
u8 NCMAG_SetAccConfig
(void)
{
u8 retval
= 0;
// try to catch the i2c buffer within 50 ms timeout
if(I2CBus_LockBuffer
(Compass_I2CPort
, 50))
{
u8 TxBytes
= 0;
u8 TxData
[sizeof(AccConfig
) + 3];
TxData
[TxBytes
++] = REG_ACC_CTRL1
|REG_ACC_MASK_AUTOINCREMENT
;
memcpy(&TxData
[TxBytes
], (u8
*)&AccConfig
, sizeof(AccConfig
));
TxBytes
+= sizeof(AccConfig
);
if(I2CBus_Transmission
(Compass_I2CPort
, ACC_SLAVE_ADDRESS
, TxData
, TxBytes
, 0, 0))
{
if(I2CBus_WaitForEndOfTransmission
(Compass_I2CPort
, 50))
{
if(I2CBus
(Compass_I2CPort
)->Error
== I2C_ERROR_NONE
) retval
= 1;
}
}
}
return(retval
);
}
// ----------------------------------------------------------------------------------------
u8 NCMAG_GetAccConfig
(void)
{
u8 retval
= 0;
// try to catch the i2c buffer within 100 ms timeout
if(I2CBus_LockBuffer
(Compass_I2CPort
, 100))
{
u8 TxBytes
= 0;
u8 TxData
[3];
TxData
[TxBytes
++] = REG_ACC_CTRL1
|REG_ACC_MASK_AUTOINCREMENT
;
if(I2CBus_Transmission
(Compass_I2CPort
, ACC_SLAVE_ADDRESS
, TxData
, TxBytes
, &NCMAG_UpdateAccConfig
, sizeof(AccConfig
)))
{
if(I2CBus_WaitForEndOfTransmission
(Compass_I2CPort
, 100))
{
if(I2CBus
(Compass_I2CPort
)->Error
== I2C_ERROR_NONE
) retval
= 1;
}
}
}
return(retval
);
}
// ----------------------------------------------------------------------------------------
u8 NCMAG_GetIdentification
(void)
{
u8 retval
= 0;
// try to catch the i2c buffer within 100 ms timeout
if(I2CBus_LockBuffer
(Compass_I2CPort
, 100))
{
u8 TxBytes
= 0;
u8 TxData
[3];
NCMAG_Identification.
A = 0xFF;
NCMAG_Identification.
B = 0xFF;
NCMAG_Identification.
C = 0xFF;
TxData
[TxBytes
++] = REG_MAG_IDA
;
// initiate transmission
if(I2CBus_Transmission
(Compass_I2CPort
, MAG_SLAVE_ADDRESS
, TxData
, TxBytes
, &NCMAG_UpdateIdentification
, sizeof(NCMAG_Identification
)))
{
if(I2CBus_WaitForEndOfTransmission
(Compass_I2CPort
, 100))
{
if(I2CBus
(Compass_I2CPort
)->Error
== I2C_ERROR_NONE
) retval
= 1;
}
}
}
return(retval
);
}
u8 NCMAG_GetIdentification_Sub
(void)
{
u8 retval
= 0;
// try to catch the i2c buffer within 100 ms timeout
if(I2CBus_LockBuffer
(Compass_I2CPort
, 100))
{
u8 TxBytes
= 0;
u8 TxData
[3];
NCMAG_Identification2.
Sub = 0xFF;
TxData
[TxBytes
++] = REG_MAG_IDF
;
// initiate transmission
if(I2CBus_Transmission
(Compass_I2CPort
, MAG_SLAVE_ADDRESS
, TxData
, TxBytes
, &NCMAG_UpdateIdentification_Sub
, sizeof(NCMAG_Identification2
)))
{
if(I2CBus_WaitForEndOfTransmission
(Compass_I2CPort
, 100))
{
if(I2CBus
(Compass_I2CPort
)->Error
== I2C_ERROR_NONE
) retval
= 1;
}
}
}
return(retval
);
}
// ----------------------------------------------------------------------------------------
void NCMAG_GetMagVector
(u8 timeout
)
{
// try to catch the I2C buffer within timeout ms
if(I2CBus_LockBuffer
(Compass_I2CPort
, timeout
))
{
u8 TxBytes
= 0;
u8 TxData
[3];
// set register pointer
TxData
[TxBytes
++] = REG_MAG_DATAX_MSB
;
// initiate transmission
I2CBus_Transmission
(Compass_I2CPort
, MAG_SLAVE_ADDRESS
, TxData
, TxBytes
, &NCMAG_UpdateMagVector
, sizeof(MagVector
));
}
}
//----------------------------------------------------------------
void NCMAG_GetAccVector
(u8 timeout
)
{
// try to catch the I2C buffer within timeout ms
if(I2CBus_LockBuffer
(Compass_I2CPort
, timeout
))
{
u8 TxBytes
= 0;
u8 TxData
[3];
// set register pointer
TxData
[TxBytes
++] = REG_ACC_X_LSB
|REG_ACC_MASK_AUTOINCREMENT
;
// initiate transmission
I2CBus_Transmission
(Compass_I2CPort
, ACC_SLAVE_ADDRESS
, TxData
, TxBytes
, &NCMAG_UpdateAccVector
, sizeof(AccRawVector
));
}
}
//----------------------------------------------------------------
u8 NCMAG_ConfigureSensor
(void)
{
u8 crb_gain
, cra_rate
;
switch(NCMAG_SensorType
)
{
case TYPE_HMC5843
:
crb_gain
= HMC5843_CRB_GAIN_15GA
;
cra_rate
= HMC5843_CRA_RATE_50HZ
;
break;
case TYPE_LSM303DLH
:
case TYPE_LSM303DLM
:
// crb_gain = LSM303DLH_CRB_GAIN_19GA;
crb_gain
= LSM303DLH_CRB_GAIN_40GA
; // seit 2.03 -> 2.2014
cra_rate
= LSM303DLH_CRA_RATE_75HZ
;
break;
default:
return(0);
}
MagConfig.
cra = cra_rate
|CRA_MODE_NORMAL
;
MagConfig.
crb = crb_gain
;
MagConfig.
mode = MODE_CONTINUOUS
;
return(NCMAG_SetMagConfig
());
}
//----------------------------------------------------------------
u8 NCMAG_Init_ACCSensor
(void)
{
AccConfig.
ctrl_1 = ACC_CRTL1_PM_NORMAL
|ACC_CRTL1_DR_50HZ
|ACC_CRTL1_XEN
|ACC_CRTL1_YEN
|ACC_CRTL1_ZEN
;
AccConfig.
ctrl_2 = 0;
AccConfig.
ctrl_3 = 0x00;
AccConfig.
ctrl_4 = ACC_CTRL4_BDU
| ACC_CTRL4_FS_8G
;
AccConfig.
ctrl_5 = ACC_CTRL5_STW_OFF
;
return(NCMAG_SetAccConfig
());
}
// --------------------------------------------------------
void NCMAG_Update
(u8 init
)
{
static u32 TimerUpdate
= 0;
static s8 send_config
= 0;
u32 delay
= 20;
if(init
) TimerUpdate
= SetDelay
(10);
if( (I2CBus
(Compass_I2CPort
)->State
== I2C_STATE_UNDEF
) /*|| !NCMAG_Present*/ )
{
Compass_Heading
= -1;
DebugOut.
Analog[14]++; // count I2C error
TimerUpdate
= SetDelay
(10);
return;
}
if(CheckDelay
(TimerUpdate
))
{
if(Compass_Heading
!= -1) send_config
= 0; // no re-configuration if value is valid
if(++send_config
== 25) // 500ms
{
send_config
= -25; // next try after 1 second
NCMAG_ConfigureSensor
();
TimerUpdate
= SetDelay
(20); // back into the old time-slot
}
else
{
static u8 s
= 0;
// check for new calibration state
Compass_UpdateCalState
();
if(Compass_CalState
) NCMAG_Calibrate
();
// in case of LSM303 type
switch(NCMAG_SensorType
)
{
case TYPE_HMC5843
:
delay
= 20; // next cycle after 20 ms
NCMAG_GetMagVector
(5);
break;
case TYPE_LSM303DLH
:
case TYPE_LSM303DLM
:
delay
= 20; // next cycle after 20 ms
if(s
-- || (Compass_I2CPort
== NCMAG_PORT_INTERN
))
{
NCMAG_GetMagVector
(5);
}
else // having an external compass, read every 50th cycle the ACC vec
{ // try to initialize if no data are there
if((AccRawVector.
X + AccRawVector.
Y + AccRawVector.
Z) == 0) NCMAG_Init_ACCSensor
();
// get new data
NCMAG_GetAccVector
(5);
delay
= 10; // next cycle after 10 ms
s
= 40; //reset downconter about 0,8 sec
}
break;
}
if(send_config
== 24) TimerUpdate
= SetDelay
(15); // next event is the re-configuration
else TimerUpdate
= SetDelay
(delay
); // every 20 ms are 50 Hz
}
}
}
// --------------------------------------------------------
u8 NCMAG_SelfTest
(void)
{
u8 msg
[64];
static u8 done
= 0;
if(done
) return(1); // just make it once
#define LIMITS(value, min, max) {min = (80 * value)/100; max = (120 * value)/100;}
u32
time;
s32 XMin
= 0, XMax
= 0, YMin
= 0, YMax
= 0, ZMin
= 0, ZMax
= 0;
s16 xscale
, yscale
, zscale
, scale_min
, scale_max
;
u8 crb_gain
, cra_rate
;
u8 i
= 0, retval
= 1;
switch(NCMAG_SensorType
)
{
case TYPE_HMC5843
:
crb_gain
= HMC5843_CRB_GAIN_15GA
;
cra_rate
= HMC5843_CRA_RATE_50HZ
;
xscale
= HMC5843_TEST_XSCALE
;
yscale
= HMC5843_TEST_YSCALE
;
zscale
= HMC5843_TEST_ZSCALE
;
break;
case TYPE_LSM303DLH
:
// crb_gain = LSM303DLH_CRB_GAIN_19GA;
crb_gain
= LSM303DLH_CRB_GAIN_40GA
; // seit 2.03 -> 2.2014
cra_rate
= LSM303DLH_CRA_RATE_75HZ
;
xscale
= LSM303DLH_TEST_XSCALE
;
yscale
= LSM303DLH_TEST_YSCALE
;
zscale
= LSM303DLH_TEST_ZSCALE
;
break;
case TYPE_LSM303DLM
:
// does not support self test feature
done
= 1;
return(1); // always return success
break;
default:
return(0);
}
MagConfig.
cra = cra_rate
|CRA_MODE_POSBIAS
;
MagConfig.
crb = crb_gain
;
MagConfig.
mode = MODE_CONTINUOUS
;
// activate positive bias field
NCMAG_SetMagConfig
();
// wait for stable readings
time = SetDelay
(50);
while(!CheckDelay
(time));
// averaging
#define AVERAGE 20
for(i
= 0; i
<AVERAGE
; i
++)
{
NCMAG_GetMagVector
(5);
time = SetDelay
(20);
while(!CheckDelay
(time));
XMax
+= MagRawVector.
X;
YMax
+= MagRawVector.
Y;
ZMax
+= MagRawVector.
Z;
}
MagConfig.
cra = cra_rate
|CRA_MODE_NEGBIAS
;
// activate positive bias field
NCMAG_SetMagConfig
();
// wait for stable readings
time = SetDelay
(50);
while(!CheckDelay
(time));
// averaging
for(i
= 0; i
< AVERAGE
; i
++)
{
NCMAG_GetMagVector
(5);
time = SetDelay
(20);
while(!CheckDelay
(time));
XMin
+= MagRawVector.
X;
YMin
+= MagRawVector.
Y;
ZMin
+= MagRawVector.
Z;
}
// setup final configuration
MagConfig.
cra = cra_rate
|CRA_MODE_NORMAL
;
// activate positive bias field
NCMAG_SetMagConfig
();
// check scale for all axes
// prepare scale limits
LIMITS
(xscale
, scale_min
, scale_max
);
xscale
= (XMax
- XMin
)/(2*AVERAGE
);
if((xscale
> scale_max
) || (xscale
< scale_min
))
{
retval
= 0;
sprintf(msg
, "\r\n Value X: %d not %d-%d !", xscale
, scale_min
,scale_max
);
UART1_PutString
(msg
);
}
LIMITS
(yscale
, scale_min
, scale_max
);
yscale
= (YMax
- YMin
)/(2*AVERAGE
);
if((yscale
> scale_max
) || (yscale
< scale_min
))
{
retval
= 0;
sprintf(msg
, "\r\n Value Y: %d not %d-%d !", yscale
, scale_min
,scale_max
);
UART1_PutString
(msg
);
}
LIMITS
(zscale
, scale_min
, scale_max
);
zscale
= (ZMax
- ZMin
)/(2*AVERAGE
);
if((zscale
> scale_max
) || (zscale
< scale_min
))
{
retval
= 0;
sprintf(msg
, "\r\n Value Z: %d not %d-%d !", zscale
, scale_min
,scale_max
);
UART1_PutString
(msg
);
}
done
= retval
;
return(retval
);
}
void NCMAG_CheckOrientation
(void)
{ // only for external sensor
if(Compass_I2CPort
== NCMAG_PORT_EXTERN
)
{
NCMAG_Orientation
= NCMAG_GetOrientationFromAcc
();
if(NCMAG_Orientation
!= (Calibration.
Version>>4)) NCMAG_IsCalibrated
= 0;
else NCMAG_IsCalibrated
= 1;
}
}
//----------------------------------------------------------------
u8 NCMAG_Init
(void)
{
MagRawVector.
X = 0;
MagRawVector.
Y = 0;
MagRawVector.
Z = 0;
AccRawVector.
X = 0;
AccRawVector.
Y = 0;
AccRawVector.
Z = 0;
if(NCMAG_Present
) // do only short init ! , full init was called before
{
// reset I2C Bus
//I2CBus_Deinit(Compass_I2CPort);
//I2CBus_Init(Compass_I2CPort);
// try to reconfigure senor
NCMAG_ConfigureSensor
();
//NCMAG_Update(1);
}
else // full init
{
u8 msg
[64];
u8 retval
= 0;
u8 repeat
= 0;
//--------------------------------------------
// search external sensor first
//--------------------------------------------
Compass_I2CPort
= NCMAG_PORT_EXTERN
;
// get id bytes
retval
= 0;
for(repeat
= 0; repeat
< 5; repeat
++)
{
//retval = NCMAG_GetIdentification();
retval
= NCMAG_GetAccConfig
(); // only the external sensor with ACC is supported
if(retval
) break; // break loop on success
UART1_PutString
("_");
}
// Extenal sensor not found?
if(!retval
)
{
// search internal sensor afterwards
UART1_PutString
(" internal sensor");
Compass_I2CPort
= NCMAG_PORT_INTERN
;
}
else
{
UART1_PutString
(" external sensor");
Compass_I2CPort
= NCMAG_PORT_EXTERN
;
}
//-------------------------------------------
NCMAG_Present
= 0;
NCMAG_SensorType
= TYPE_HMC5843
; // assuming having an HMC5843
// polling for LSM302DLH/DLM option by ACC address ack
repeat
= 0;
for(repeat
= 0; repeat
< 3; repeat
++)
{
retval
= NCMAG_GetAccConfig
();
if(retval
) break; // break loop on success
}
if(retval
)
{
// initialize ACC sensor
NCMAG_Init_ACCSensor
();
NCMAG_SensorType
= TYPE_LSM303DLH
;
// polling of sub identification
repeat
= 0;
for(repeat
= 0; repeat
< 12; repeat
++)
{
retval
= NCMAG_GetIdentification_Sub
();
if(retval
) break; // break loop on success
}
if(retval
)
{
if(NCMAG_Identification2.
Sub == MAG_IDF_LSM303DLM
) NCMAG_SensorType
= TYPE_LSM303DLM
;
}
}
// get id bytes
retval
= 0;
for(repeat
= 0; repeat
< 3; repeat
++)
{
retval
= NCMAG_GetIdentification
();
if(retval
) break; // break loop on success
}
// if we got an answer to id request
if(retval
)
{
u8 n1
[] = "\n\r HMC5843";
u8 n2
[] = "\n\r LSM303DLH";
u8 n3
[] = "\n\r LSM303DLM";
u8
* pn
= n1
;
switch(NCMAG_SensorType
)
{
case TYPE_HMC5843
:
pn
= n1
;
break;
case TYPE_LSM303DLH
:
pn
= n2
;
break;
case TYPE_LSM303DLM
:
pn
= n3
;
break;
}
sprintf(msg
, " %s ID 0x%02x/%02x/%02x-%02x", pn
, NCMAG_Identification.
A, NCMAG_Identification.
B, NCMAG_Identification.
C,NCMAG_Identification2.
Sub);
UART1_PutString
(msg
);
if ( (NCMAG_Identification.
A == MAG_IDA
)
&& (NCMAG_Identification.
B == MAG_IDB
)
&& (NCMAG_Identification.
C == MAG_IDC
))
{
NCMAG_Present
= 1;
if(EEPROM_Init
())
{
NCMAG_IsCalibrated
= NCMag_CalibrationRead
(Compass_I2CPort
);
if(!NCMAG_IsCalibrated
) UART1_PutString
("\r\n Not calibrated!");
}
else UART1_PutString
("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
// in case of an external sensor, try to get the orientation by acc readings
if(Compass_I2CPort
== NCMAG_PORT_EXTERN
)
{
// try to get orientation by acc sensor values
for(repeat
= 0; repeat
< 100; repeat
++)
{
NCMAG_GetAccVector
(10); // only the sensor with ACC is supported
NCMAG_Orientation
= NCMAG_GetOrientationFromAcc
();
if(NCMAG_Orientation
&& (NCMAG_Orientation
== Calibration.
Version >> 4)) break;
}
// check orientation result if available
sprintf(msg
, "\r\n Orientation: ");
UART1_PutString
(msg
);
if(NCMAG_Orientation
)
{
sprintf(msg
, "%d ", NCMAG_Orientation
);
UART1_PutString
(msg
);
if(NCMAG_IsCalibrated
) // check against calibration data orientation
{
if(NCMAG_Orientation
!= Calibration.
Version >> 4)
{
sprintf(msg
, "\n\r Warning: calibrated orientation was %d !",Calibration.
Version >> 4);
UART1_PutString
(msg
);
}
}
}
else
{
UART1_PutString
("unknown!");
}
}
// perform self test
if(!NCMAG_SelfTest
())
{
UART1_PutString
("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
LED_RED_ON
;
//NCMAG_IsCalibrated = 0;
}
else UART1_PutString
("\r\n Selftest ok");
// initialize magnetic sensor configuration
NCMAG_ConfigureSensor
();
}
else
{
UART1_PutString
("\n\r Not compatible!");
UART_VersionInfo.
HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE
;
LED_RED_ON
;
}
}
else // nothing found
{
NCMAG_SensorType
= TYPE_NONE
;
UART1_PutString
("not found!");
}
}
return(NCMAG_Present
);
}