Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 393 → Rev 394

/trunk/compass.c
68,7 → 68,7
fifo_t CompassCalcStateFiFo;
 
volatile s16vec_t MagVector; // is written by mk3mag or ncmag implementation
volatile s16 Compass_Heading; // is written by mk3mag or ncmag implementation
volatile s16 Compass_Heading; // is written by mk3mag or ncmag implementation
volatile u8 Compass_CalState; // is written by mk3mag or ncmag implementation
s16 Hx = 0, Hy = 0;
s32 EarthMagneticField = 100;
/trunk/ncmag.c
69,14 → 69,17
u8 NCMAG_Present = 0;
u8 NCMAG_IsCalibrated = 0;
 
#define MAG_TYPE_NONE 0
#define MAG_TYPE_HMC5843 1
#define MAG_TYPE_LSM303DLH 2
u8 NCMAG_MagType = MAG_TYPE_NONE;
// 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;
 
#define EEPROM_ADR_MAG_CALIBRATION 50
#define CALIBRATION_VERSION 1
#define EEPROM_ADR_MAG_CALIBRATION 50
#define MAG_CALIBRATION_COMPATIBEL 0xA2
#define MAG_CALIBRATION_COMPATIBLE 0xA2
 
#define NCMAG_MIN_RAWVALUE -2047
#define NCMAG_MAX_RAWVALUE 2047
119,7 → 122,7
#define REG_MAG_IDA 0x0A
#define REG_MAG_IDB 0x0B
#define REG_MAG_IDC 0x0C
#define REG_MAG_IDF 0x0F
#define REG_MAG_IDF 0x0F // WHO_AM_I _M = 0x03c when LSM303DLM is connected
 
// bit mask for configuration mode
#define CRA_MODE_MASK 0x03
145,6 → 148,7
#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
168,7 → 172,7
#define HMC5843_TEST_XSCALE 555
#define HMC5843_TEST_YSCALE 555
#define HMC5843_TEST_ZSCALE 555
// clibration range
// calibration range
#define HMC5843_CALIBRATION_RANGE 600
 
// the special LSM302DLH interface
190,6 → 194,31
#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 495
#define LSM303DLH_TEST_YSCALE 495
199,6 → 228,10
 
// 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
215,33 → 248,37
#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_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
 
typedef struct
{
u8 A;
u8 B;
u8 C;
} __attribute__((packed)) Identification_t;
volatile Identification_t NCMAG_Identification;
#define ACC_CTRL5_STW_ON 0x03
#define ACC_CTRL5_STW_OFF 0x00
 
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;
 
typedef struct
{
u8 ctrl_1;
u8 ctrl_2;
u8 ctrl_3;
253,7 → 290,7
 
u8 NCMag_CalibrationWrite(void)
{
u8 i, crc = MAG_CALIBRATION_COMPATIBEL;
u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
EEPROM_Result_t eres;
u8 *pBuff = (u8*)&Calibration;
 
271,7 → 308,7
 
u8 NCMag_CalibrationRead(void)
{
u8 i, crc = MAG_CALIBRATION_COMPATIBEL;
u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
u8 *pBuff = (u8*)&Calibration;
 
if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)))
294,7 → 331,7
static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
static s16 X = 0, Y = 0, Z = 0;
static u8 OldCalState = 0;
s16 MinCaclibration = 450;
s16 MinCalibration = 450;
 
X = (4*X + MagRawVector.X + 3)/5;
Y = (4*Y + MagRawVector.Y + 3)/5;
336,20 → 373,22
// Save values
if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
{
// #define MIN_CALIBRATION 256
if(NCMAG_MagType == MAG_TYPE_HMC5843)
{
UART1_PutString("\r\nHMC5843 calibration\n\r");
MinCaclibration = HMC5843_CALIBRATION_RANGE;
}
if(NCMAG_MagType == MAG_TYPE_LSM303DLH)
{
UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
MinCaclibration =LSM303_CALIBRATION_RANGE;
}
switch(NCMAG_SensorType)
{
case TYPE_HMC5843:
UART1_PutString("\r\nHMC5843 calibration\n\r");
MinCalibration = HMC5843_CALIBRATION_RANGE;
break;
 
case TYPE_LSM303DLH:
case TYPE_LSM303DLM:
UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
MinCalibration = LSM303_CALIBRATION_RANGE;
break;
}
if(EarthMagneticStrengthTheoretic)
{
MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50;
MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
UART1_PutString(msg);
}
361,7 → 400,7
Calibration.MagY.Offset = (Ymin + Ymax) / 2;
Calibration.MagZ.Range = Zmax - Zmin;
Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
if((Calibration.MagX.Range > MinCaclibration) && (Calibration.MagY.Range > MinCaclibration) && (Calibration.MagZ.Range > MinCaclibration))
if((Calibration.MagX.Range > MinCalibration) && (Calibration.MagY.Range > MinCalibration) && (Calibration.MagZ.Range > MinCalibration))
{
NCMAG_IsCalibrated = NCMag_CalibrationWrite();
BeepTime = 2500;
370,9 → 409,9
else
{
UART1_PutString("\r\nCalibration FAILED - Values too low: ");
if(Calibration.MagX.Range < MinCaclibration) UART1_PutString("X! ");
if(Calibration.MagY.Range < MinCaclibration) UART1_PutString("Y! ");
if(Calibration.MagZ.Range < MinCaclibration) UART1_PutString("Z! ");
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
384,7 → 423,7
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",MinCaclibration);
sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCalibration);
UART1_PutString(msg);
}
break;
427,17 → 466,16
raw+= pRxBuffer[3];
if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
{
if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Z = raw; // here Z and Y are exchanged
else MagRawVector.Y = raw;
if(NCMAG_SensorType == TYPE_LSM303DLM) MagRawVector.Z = raw; // here Z and Y are exchanged
else MagRawVector.Y = raw;
}
raw = pRxBuffer[4]<<8;
raw+= pRxBuffer[5];
if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
{
if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Y = raw; // here Z and Y are exchanged
else MagRawVector.Z = raw;
if(NCMAG_SensorType == TYPE_LSM303DLM) MagRawVector.Y = raw; // here Z and Y are exchanged
else MagRawVector.Z = raw;
}
//MagRawVector.X += 2 * ((s32) FC.Poti[7] - 128);
}
if(Compass_CalState || !NCMAG_IsCalibrated)
{ // mark out data invalid
462,6 → 500,9
{
memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
}
DebugOut.Analog[16] = AccRawVector.X;
DebugOut.Analog[17] = AccRawVector.Y;
DebugOut.Analog[18] = AccRawVector.Z;
}
// rx data handler for reading magnetic sensor configuration
void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
532,7 → 573,7
if(I2C_LockBuffer(100))
{
u8 TxBytes = 0;
I2C_Buffer[TxBytes++] = REG_ACC_CTRL1;
I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
TxBytes += sizeof(AccConfig);
if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
554,7 → 595,7
if(I2C_LockBuffer(100))
{
u8 TxBytes = 0;
I2C_Buffer[TxBytes++] = REG_ACC_CTRL1;
I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
{
if(I2C_WaitForEndOfTransmission(100))
618,7 → 659,6
// try to catch the I2C buffer within 0 ms
if(I2C_LockBuffer(0))
{
// s16 tmp;
u16 TxBytes = 0;
// set register pointer
I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB;
635,7 → 675,7
{
u16 TxBytes = 0;
// set register pointer
I2C_Buffer[TxBytes++] = REG_ACC_X_LSB;
I2C_Buffer[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT;
// initiate transmission
I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
}
642,46 → 682,49
}
 
//----------------------------------------------------------------
void InitNC_MagnetSensor(void)
u8 InitNC_MagnetSensor(void)
{
s16 xscale, yscale, zscale;
u8 crb_gain, cra_rate;
// u8 retval = 1;
 
switch(NCMAG_MagType)
switch(NCMAG_SensorType)
{
case MAG_TYPE_HMC5843:
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 MAG_TYPE_LSM303DLH:
case TYPE_LSM303DLH:
case TYPE_LSM303DLM:
crb_gain = LSM303DLH_CRB_GAIN_19GA;
cra_rate = LSM303DLH_CRA_RATE_75HZ;
xscale = LSM303DLH_TEST_XSCALE;
yscale = LSM303DLH_TEST_YSCALE;
zscale = LSM303DLH_TEST_ZSCALE;
break;
 
default:
return;
return(0);
}
 
MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
MagConfig.crb = crb_gain;
MagConfig.mode = MODE_CONTINUOUS;
NCMAG_SetMagConfig();
return(NCMAG_SetMagConfig());
}
 
 
//----------------------------------------------------------------
u8 NCMAG_Init_ACCSensor(void)
{
AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_400HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN;
AccConfig.ctrl_2 = 0x00;
AccConfig.ctrl_3 = 0x00;
AccConfig.ctrl_4 = ACC_CTRL4_BDU|ACC_CTRL4_FS_2G;
AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF;
return(NCMAG_SetAccConfig());
}
// --------------------------------------------------------
void NCMAG_Update(void)
{
static u32 TimerUpdate = 0;
static u8 send_config = 0;
u32 delay = 20;
 
if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
{
691,21 → 734,36
}
if(CheckDelay(TimerUpdate))
{
if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
if(++send_config == 25) // 500ms
{
send_config = 0;
InitNC_MagnetSensor();
TimerUpdate = SetDelay(15); // back into the old time-slot
}
if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
if(++send_config == 25) // 500ms
{
send_config = 0;
InitNC_MagnetSensor();
TimerUpdate = SetDelay(15); // back into the old time-slot
}
else
{
// check for new calibration state
Compass_UpdateCalState();
if(Compass_CalState) NCMAG_Calibrate();
NCMAG_GetMagVector(); //Get new data;
if(send_config == 24) TimerUpdate = SetDelay(5); // next event is the re-configuration
else TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz
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:
NCMAG_GetMagVector();
delay = 20;
break;
case TYPE_LSM303DLH:
case TYPE_LSM303DLM:
if(s){ NCMAG_GetMagVector(); s = 0;}
else { NCMAG_GetAccVector(); s = 1;}
delay = 10;
break;
}
if(send_config == 24) TimerUpdate = SetDelay(5); // next event is the re-configuration
else TimerUpdate = SetDelay(delay); // every 20 ms are 50 Hz
}
}
}
726,9 → 784,9
u8 crb_gain, cra_rate;
u8 i = 0, retval = 1;
 
switch(NCMAG_MagType)
switch(NCMAG_SensorType)
{
case MAG_TYPE_HMC5843:
case TYPE_HMC5843:
crb_gain = HMC5843_CRB_GAIN_15GA;
cra_rate = HMC5843_CRA_RATE_50HZ;
xscale = HMC5843_TEST_XSCALE;
736,7 → 794,7
zscale = HMC5843_TEST_ZSCALE;
break;
 
case MAG_TYPE_LSM303DLH:
case TYPE_LSM303DLH:
crb_gain = LSM303DLH_CRB_GAIN_19GA;
cra_rate = LSM303DLH_CRA_RATE_75HZ;
xscale = LSM303DLH_TEST_XSCALE;
744,8 → 802,14
zscale = LSM303DLH_TEST_ZSCALE;
break;
 
case TYPE_LSM303DLM:
// does not support self test feature
done = retval;
return(retval);
break;
 
default:
return(0);
return(0);
}
 
MagConfig.cra = cra_rate|CRA_MODE_POSBIAS;
792,27 → 856,27
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);
}
{
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);
}
{
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);
}
{
retval = 0;
sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
UART1_PutString(msg);
}
done = retval;
return(retval);
}
826,8 → 890,8
u8 repeat;
 
NCMAG_Present = 0;
NCMAG_MagType = MAG_TYPE_HMC5843; // assuming having an HMC5843
// polling for LSM302DLH option
NCMAG_SensorType = TYPE_HMC5843; // assuming having an HMC5843
// polling for LSM302DLH/DLM option by ACC address ack
repeat = 0;
do
{
836,16 → 900,27
UART1_PutString(".");
repeat++;
}while(repeat < 3);
if(retval) NCMAG_MagType = MAG_TYPE_LSM303DLH; // must be a LSM303DLH
// polling of identification
repeat = 0;
do
if(retval)
{
retval = NCMAG_GetIdentification_Sub();
if(retval) break; // break loop on success
UART1_PutString(".");
repeat++;
}while(repeat < 12);
// initialize ACC sensor
NCMAG_Init_ACCSensor();
 
NCMAG_SensorType = TYPE_LSM303DLH;
// polling of sub identification
repeat = 0;
do
{
retval = NCMAG_GetIdentification_Sub();
if(retval) break; // break loop on success
UART1_PutString(".");
repeat++;
}while(repeat < 12);
if(retval)
{
if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM) NCMAG_SensorType = TYPE_LSM303DLM;
}
}
// get id bytes
retval = 0;
do
{
861,13 → 936,19
u8 n1[] = "\n\r HMC5843";
u8 n2[] = "\n\r LSM303DLH";
u8 n3[] = "\n\r LSM303DLM";
u8* pn;
u8* pn = n1;
pn = n1;
if(NCMAG_MagType == MAG_TYPE_LSM303DLH)
switch(NCMAG_SensorType)
{
if(NCMAG_Identification2.Sub == 0x3c) pn = n3;
else pn = n2;
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);
879,22 → 960,22
NCMAG_Present = 1;
 
if(EEPROM_Init())
{
NCMAG_IsCalibrated = NCMag_CalibrationRead();
if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
}
{
NCMAG_IsCalibrated = NCMag_CalibrationRead();
if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
}
else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
if(NCMAG_Identification2.Sub == 0x00)
{
if(!NCMAG_SelfTest())
{
// 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");
}
else InitNC_MagnetSensor();
}
else UART1_PutString("\r\n Selftest ok");
 
// initialize magnetic sensor configuration
InitNC_MagnetSensor();
}
else
{
905,7 → 986,7
}
else // nothing found
{
NCMAG_MagType = MAG_TYPE_NONE;
NCMAG_SensorType = TYPE_NONE;
UART1_PutString("not found!");
}
return(NCMAG_Present);