Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 589 → Rev 590

/trunk/ncmag.c
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;