119,6 → 119,15 |
#define REG_MAG_IDC 0x0C |
#define REG_MAG_IDF 0x0F // WHO_AM_I _M = 0x03c when LSM303DLM is connected |
|
#define ADR_LSM303D_MAG 0x08 |
#define REG_303D_CTRL0 0x1F // Fifo + Interrupts |
#define REG_303D_CTRL1 0x20 // ACC Update Speed |
#define REG_303D_CTRL2 0x21 // ACC Gain + Filter |
#define REG_303D_CTRL3 0x22 // Fifo + Interrupts |
#define REG_303D_CTRL4 0x23 // Fifo + Interrupts |
#define REG_303D_CTRL5 0x24 // Temperature Enable + Magn.Resolution + Magn.Rate |
#define REG_303D_CTRL6 0x25 // Magn. Gain |
#define REG_303D_CTRL7 0x26 // Magn. PowerDown + Filter |
// bit mask for configuration mode |
#define CRA_MODE_MASK 0x03 |
#define CRA_MODE_NORMAL 0x00 //default |
144,6 → 153,8 |
#define MAG_IDB 0x34 |
#define MAG_IDC 0x33 |
#define MAG_IDF_LSM303DLM 0x3C |
#define MAG_IDF_LSM303D 0x49 |
#define MAG_IDF_LSM303DLH 0x00 |
|
// the special HMC5843 interface |
// bit mask for rate |
153,7 → 164,7 |
#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 |
#define HMC5843_CRA_RATE_50HZ 0x18 // <-- we use this |
// bit mask for gain |
#define HMC5843_CRB_GAIN_07GA 0x00 |
#define HMC5843_CRB_GAIN_10GA 0x20 //default |
178,8 → 189,45 |
#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 |
#define LSM303DLH_CRA_RATE_75HZ 0x18 // <-- we use this |
|
// LSM303D CTRL1 |
#define LSM303D_ACC_RATE_0HZ 0x0F |
#define LSM303D_ACC_RATE_3HZ 0x1F |
#define LSM303D_ACC_RATE_6HZ 0x2F |
#define LSM303D_ACC_RATE_12HZ 0x3F |
#define LSM303D_ACC_RATE_25HZ 0x4F |
#define LSM303D_ACC_RATE_50HZ 0x5F |
#define LSM303D_ACC_RATE_100HZ 0x6F |
#define LSM303D_ACC_RATE_200HZ 0x7F |
#define LSM303D_ACC_RATE_400HZ 0x8F |
#define LSM303D_ACC_RATE_800HZ 0x9F |
#define LSM303D_ACC_RATE_1600HZ 0xAF |
|
// LSM303D CTRL1 |
#define LSM303D_ACC_SCALE_2g 0x00 |
#define LSM303D_ACC_SCALE_4g 0x08 |
#define LSM303D_ACC_SCALE_6g 0x10 |
#define LSM303D_ACC_SCALE_8g 0x18 |
#define LSM303D_ACC_SCALE_16g 0x20 |
|
// LSM303D CTRL5 |
#define LSM303D_ODR_RATE_3HZ 0x00 |
#define LSM303D_ODR_RATE_6HZ 0x04 |
#define LSM303D_ODR_RATE_12HZ 0x08 |
#define LSM303D_ODR_RATE_25HZ 0x0C |
#define LSM303D_ODR_RATE_50HZ 0x10 |
#define LSM303D_ODR_RATE_100HZ 0x14 // attention: ACC Rate must be >50Hz to use this |
#define LSM303D_HIGH_RESULUTION 0x60 |
#define LSM303D_LOW_RESULUTION 0x00 |
#define LSM303D_TEMP_ENABLE 0x80 |
|
// LSM303D CTRL6 |
#define LSM303D_MAG_SCALE_2GA 0x00 |
#define LSM303D_MAG_SCALE_4GA 0x20 |
#define LSM303D_MAG_SCALE_8GA 0x40 // <-- we use this (Achtung: der LSM303D hat eine höhere Auflösung) |
#define LSM303D_MAG_SCALE_12GA 0x60 |
|
// bit mask for gain |
#define LSM303DLH_CRB_GAIN_XXGA 0x00 |
#define LSM303DLH_CRB_GAIN_13GA 0x20 //default |
224,7 → 272,7 |
#define ACC_SLAVE_ADDRESS 0x30 // i2c slave for acc. sensor registers |
|
// multiple byte read/write mask |
#define REG_ACC_MASK_AUTOINCREMENT 0x80 |
#define REG_MASK_AUTOINCREMENT 0x80 |
|
// register mapping |
#define REG_ACC_CTRL1 0x20 |
427,6 → 475,8 |
|
case TYPE_LSM303DLH: |
case TYPE_LSM303DLM: |
case TYPE_LSM303D: |
default: |
UART1_PutString("\r\nFinished: LSM303 calibration\n\r"); |
MinCalibration = LSM303_CALIBRATION_RANGE; |
break; |
511,17 → 561,16 |
|
// 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) ) |
{ |
u8 endian = 1; |
if(NCMAG_SensorType == TYPE_LSM303D) endian = 0; // here the low and high-bytes are mixed |
if(RxBufferSize == sizeof(MagRawVector) ) // if number of bytes are matching |
{ // byte order from big to little endian |
s16 raw, X = 0, Y = 0, Z = 0; |
raw = pRxBuffer[0]<<8; |
raw+= pRxBuffer[1]; |
if(endian) {raw = pRxBuffer[0]<<8; raw+= pRxBuffer[1];} else {raw = (pRxBuffer[1] << 8) + pRxBuffer[0]; raw >>= 3;} |
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(endian) {raw = pRxBuffer[2]<<8; raw+= pRxBuffer[3];} else {raw = (pRxBuffer[3] << 8) + pRxBuffer[2]; raw >>= 3;} |
if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
{ |
if(NCMAG_SensorType == TYPE_LSM303DLM) Z = raw; // here Z and Y are exchanged |
528,9 → 577,7 |
else Y = raw; |
} |
else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data |
|
raw = pRxBuffer[4]<<8; |
raw+= pRxBuffer[5]; |
if(endian) {raw = pRxBuffer[4]<<8; raw+= pRxBuffer[5];} else {raw = (pRxBuffer[5] << 8) + pRxBuffer[4]; raw >>= 3;} |
if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
{ |
if(NCMAG_SensorType == TYPE_LSM303DLM) Y = raw; // here Z and Y are exchanged |
537,7 → 584,6 |
else Z = raw; |
} |
else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data |
|
// correct compass orientation |
switch(NCMAG_Orientation) |
{ |
654,7 → 700,33 |
// try to catch the i2c buffer within 100 ms timeout |
if(I2CBus_LockBuffer(Compass_I2CPort, 100)) |
{ |
if(NCMAG_SensorType == TYPE_LSM303D) |
{ |
u8 TxBytes = 0; |
u8 CfgData[] = { REG_303D_CTRL0 | REG_MASK_AUTOINCREMENT, |
0, // Ctrl 0 -> Fifo |
LSM303D_ACC_RATE_6HZ, // Ctrl 1 -> ACC Update Speed |
LSM303D_ACC_SCALE_8g, // Ctrl 2 -> ACC Gain + Filter (0 = 773Hz) |
0, // Ctrl 3 -> Interrupts |
0, // Ctrl 4 -> Interrupts |
LSM303D_ODR_RATE_50HZ | LSM303D_TEMP_ENABLE | LSM303D_LOW_RESULUTION,// Ctrl 5 -> Temperature Enable + Magn.Resolution + Magn.Rate |
LSM303D_MAG_SCALE_8GA, // Ctrl 6 -> Magn. Scale |
0x80 // Ctrl 7 -> Magn. PowerDown + Filter |
}; |
|
TxBytes = sizeof(CfgData); |
DebugOut.Analog[16]++; |
if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, CfgData, TxBytes, 0, 0)) |
{ |
if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100)) |
{ |
if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1; |
} |
} |
} |
else |
{ |
u8 TxBytes = 0; |
u8 TxData[sizeof(MagConfig) + 3]; |
|
TxData[TxBytes++] = REG_MAG_CRA; |
667,11 → 739,13 |
if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1; |
} |
} |
} |
} |
return(retval); |
} |
|
// ---------------------------------------------------------------------------------------- |
/* |
u8 NCMAG_GetMagConfig(void) |
{ |
u8 retval = 0; |
691,7 → 765,7 |
} |
return(retval); |
} |
|
*/ |
// ---------------------------------------------------------------------------------------- |
u8 NCMAG_SetAccConfig(void) |
{ |
701,7 → 775,7 |
{ |
u8 TxBytes = 0; |
u8 TxData[sizeof(AccConfig) + 3]; |
TxData[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT; |
TxData[TxBytes++] = REG_ACC_CTRL1|REG_MASK_AUTOINCREMENT; |
memcpy(&TxData[TxBytes], (u8*)&AccConfig, sizeof(AccConfig)); |
TxBytes += sizeof(AccConfig); |
if(I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, 0, 0)) |
724,7 → 798,7 |
{ |
u8 TxBytes = 0; |
u8 TxData[3]; |
TxData[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT; |
TxData[TxBytes++] = REG_ACC_CTRL1|REG_MASK_AUTOINCREMENT; |
if(I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig))) |
{ |
if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100)) |
740,6 → 814,7 |
u8 NCMAG_GetIdentification(void) |
{ |
u8 retval = 0; |
if(NCMAG_SensorType == TYPE_LSM303D) return(1); |
// try to catch the i2c buffer within 100 ms timeout |
if(I2CBus_LockBuffer(Compass_I2CPort, 100)) |
{ |
793,7 → 868,8 |
u8 TxBytes = 0; |
u8 TxData[3]; |
// set register pointer |
TxData[TxBytes++] = REG_MAG_DATAX_MSB; |
if(NCMAG_SensorType == TYPE_LSM303D) TxData[TxBytes++] = ADR_LSM303D_MAG | REG_MASK_AUTOINCREMENT; |
else TxData[TxBytes++] = REG_MAG_DATAX_MSB; |
// initiate transmission |
I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector)); |
} |
808,9 → 884,17 |
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)); |
if(NCMAG_SensorType == TYPE_LSM303D) |
{ |
TxData[TxBytes++] = REG_ACC_X_LSB | REG_MASK_AUTOINCREMENT; |
I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
} |
else |
{ |
TxData[TxBytes++] = REG_ACC_X_LSB | REG_MASK_AUTOINCREMENT; |
// initiate transmission |
I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
} |
} |
} |
|
817,28 → 901,27 |
//---------------------------------------------------------------- |
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; |
MagConfig.cra = HMC5843_CRA_RATE_50HZ|CRA_MODE_NORMAL; |
MagConfig.crb = HMC5843_CRB_GAIN_15GA; |
MagConfig.mode = MODE_CONTINUOUS; |
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; |
// MagConfig.crb = LSM303DLH_CRB_GAIN_19GA; |
MagConfig.crb = LSM303DLH_CRB_GAIN_40GA; // seit 2.03 -> 2.2014 |
MagConfig.cra = LSM303DLH_CRA_RATE_75HZ|CRA_MODE_NORMAL;; |
MagConfig.mode = MODE_CONTINUOUS; |
break; |
|
case TYPE_LSM303D: |
// -> see defined data in NCMAG_SetMagConfig(); |
break; |
default: |
return(0); |
} |
|
MagConfig.cra = cra_rate|CRA_MODE_NORMAL; |
MagConfig.crb = crb_gain; |
MagConfig.mode = MODE_CONTINUOUS; |
return(NCMAG_SetMagConfig()); |
} |
|
846,6 → 929,12 |
//---------------------------------------------------------------- |
u8 NCMAG_Init_ACCSensor(void) |
{ |
if(NCMAG_SensorType == TYPE_LSM303D) |
{ |
return(NCMAG_SetMagConfig()); |
} |
else |
{ |
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; |
852,6 → 941,7 |
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) |
894,6 → 984,7 |
break; |
case TYPE_LSM303DLH: |
case TYPE_LSM303DLM: |
case TYPE_LSM303D: |
delay = 20; // next cycle after 20 ms |
if(s-- || (Compass_I2CPort == NCMAG_PORT_INTERN)) |
{ |
906,6 → 997,12 |
NCMAG_GetAccVector(5); |
delay = 10; // next cycle after 10 ms |
s = 40; //reset downconter about 0,8 sec |
/* |
DebugOut.Analog[17] = AccVector.X; |
DebugOut.Analog[18] = AccVector.Y; |
DebugOut.Analog[19] = AccVector.Z; |
s = 2; |
*/ |
} |
break; |
} |
955,7 → 1052,10 |
done = 1; |
return(1); // always return success |
break; |
|
case TYPE_LSM303D: |
done = 1; |
return(1); // always return success |
break; |
default: |
return(0); |
} |
1068,11 → 1168,25 |
// search external sensor first |
//-------------------------------------------- |
Compass_I2CPort = NCMAG_PORT_EXTERN; |
retval = 0; |
// get id bytes |
retval = 0; |
for(repeat = 0; repeat < 5; repeat++) |
{ |
//retval = NCMAG_GetIdentification(); |
if(NCMAG_GetIdentification_Sub()) break; |
UART1_PutString("="); |
} |
//sprintf(msg, "(WhoAmI=%02x)", NCMAG_Identification2.Sub); |
//UART1_PutString(msg); |
if(NCMAG_Identification2.Sub == MAG_IDF_LSM303D) |
{ |
NCMAG_SensorType = TYPE_LSM303D; |
NCMAG_ConfigureSensor(); |
retval = 1; |
} |
else |
{ // External Sensor not LSM303D |
for(repeat = 0; repeat < 3; repeat++) // does it have ACC? |
{ |
retval = NCMAG_GetAccConfig(); // only the external sensor with ACC is supported |
if(retval) break; // break loop on success |
UART1_PutString("_"); |
1090,33 → 1204,39 |
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++) |
{ |
if(NCMAG_GetIdentification_Sub()) break; |
UART1_PutString("="); |
} |
if(NCMAG_Identification2.Sub == MAG_IDF_LSM303D) // internal Sensor the LSM303D? |
{ |
NCMAG_SensorType = TYPE_LSM303D; |
NCMAG_ConfigureSensor(); |
retval = 1; |
} |
else |
for(repeat = 0; repeat < 3; repeat++) // polling for LSM303DLH/DLM option by ACC address ack |
{ |
retval = NCMAG_GetAccConfig(); |
if(retval) break; // break loop on success |
} |
if(retval) |
if(retval) // it is a sensor with ACC |
{ |
// 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; |
} |
switch(NCMAG_Identification2.Sub) |
{ |
case MAG_IDF_LSM303DLM: NCMAG_SensorType = TYPE_LSM303DLM; |
NCMAG_Init_ACCSensor(); |
break; |
case MAG_IDF_LSM303DLH: NCMAG_SensorType = TYPE_LSM303DLH; |
NCMAG_Init_ACCSensor(); |
break; |
case MAG_IDF_LSM303D: NCMAG_SensorType = TYPE_LSM303D; |
break; |
} |
} |
// get id bytes |
retval = 0; |
1125,7 → 1245,7 |
retval = NCMAG_GetIdentification(); |
if(retval) break; // break loop on success |
} |
|
} |
// if we got an answer to id request |
if(retval) |
{ |
1132,7 → 1252,9 |
u8 n1[] = "\n\r HMC5843"; |
u8 n2[] = "\n\r LSM303DLH"; |
u8 n3[] = "\n\r LSM303DLM"; |
u8* pn = n1; |
u8 n4[] = "\n\r LSM303D"; |
u8 n5[] = "\n\r ??"; |
u8* pn = n5; |
|
switch(NCMAG_SensorType) |
{ |
1145,13 → 1267,17 |
case TYPE_LSM303DLM: |
pn = n3; |
break; |
case TYPE_LSM303D: |
pn = n4; |
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) |
if (( (NCMAG_Identification.A == MAG_IDA) |
&& (NCMAG_Identification.B == MAG_IDB) |
&& (NCMAG_Identification.C == MAG_IDC)) |
&& (NCMAG_Identification.C == MAG_IDC)) |
|| NCMAG_SensorType == TYPE_LSM303D) |
{ |
NCMAG_Present = 1; |
|