//______________________________________________________________________________________
int writeI2C( _i2c *p, char *dBuffer, int length) {
static
int nrpt=3;
int to=__time__+25;
if(p) {
memcpy(p->txbuf,dBuffer,length);
p->ntx=length;
I2C_ITConfig(I2C1, (I2C_IT_EVT | I2C_IT_BUF), ENABLE);
I2C_GenerateSTART(I2C1, ENABLE);
while(p->ntx && __time__< to)
_proc_loop();
if(p->ntx) {
Reset_I2C(p);
if(--nrpt)
return writeI2C(p, dBuffer, length);
}
nrpt=3;
if(p->ntx) {
return(0);
}
}
_CLEAR_ERROR(pfm,PFM_I2C_ERR);
if(_DBG(pfm,_DBG_I2C_TX)) {
_io *io=_stdio(__dbug);
int i;
__print(":%04d",__time__ % 10000);
for(i=0;i<length;++i)
__print(" %02X",p->txbuf[i]);
__print("\r\n>");
_stdio(io);
}
return(-1);
}
int main()
{
i2cBus = open("/dev/i2c-1", O_RDWR);
if (i2cBus <= 0)
{
printf("Error opening the i2c bus\n");
return -1;
}
selectI2CDevice(ACCEL_BUS_ADDRESS);
// writeI2C(CTRL_REG5_XM, 0xf0);
// writeI2C(CTRL_REG6_XM, 0x60);
// writeI2C(CTRL_REG7_XM, 0);
writeI2C(0x2D, 0x08); // enable accel
while(1)
{
short block[3];
readI2C(ACCEL_X, 2, (unsigned char*)&block[0]);
readI2C(ACCEL_Y, 2, (unsigned char*)&block[1]);
readI2C(ACCEL_Z, 2, (unsigned char*)&block[2]);
printf("gyro reads: %d,%d,%d\n", (int)block[0], (int)block[1], (int)block[2]);
}
return 0;
}
bool EepromDev::saveToDevice(void)
{
bool ret_value = false;
int fd;
if (openDevice(fd)) {
const char *buffer = (const char *)&data_;
unsigned int buffer_offset = 0;
int page_size = data_.page_size;
bool ret;
while (buffer_offset < sizeof(data_)) {
if ((buffer_offset + page_size) > sizeof(data_))
page_size = sizeof(data_) - buffer_offset;
ret = writeI2C(fd, buffer_offset, (void *)(buffer + buffer_offset),
page_size);
if (!ret) {
break;
}
buffer_offset += page_size;
}
::close(fd);
}
else
ret_value = -1;
return ret_value;
}
ITG3200::ITG3200() {
readI2C(GYRADDR, 0x00, 1, buffer); // Who am I?
sp("ITG-3200 ID = ");
spln((int) buffer[0]);
readI2C(GYRADDR, 0x15, 2, buffer);
// Sample rate divider is 1 + 1 = 2, so 1000 Hz / 2 = 500 Hz
buffer[0] = 1;
// Set FS_SEL = 3 as recommended on DS p. 24.
// Set DLPF_CFG = 3. This signifies a 1 kHz internal sample rate with a 42
// Hz LPF bandwidth, which should be low enough to filter out the motor
// vibrations.
buffer[1] = (3 << 3) | 3; // FS_SEL is on bits 4 and 3.
writeI2C(GYRADDR, 0x15, 2, buffer);
// Set to use X gyro as clock reference as recommended on DS p. 27.
buffer[0] = 1;
writeI2C(GYRADDR, 0x3e, 1, buffer);
spln("ITG-3200 configured!");
// Zero buffer.
for (int i=0; i<6; i++) {
buffer[i] = 0;
}
// Low-pass filter.
#ifdef GYRO_LPF_DEPTH
lpfIndex = 0;
for (int i=0; i<3; i++)
for (int j=0; j<GYRO_LPF_DEPTH; j++)
lpfVal[i][j] = 0;
#endif // GYRO_LPF_DEPTH
for (int i=0; i<3; i++) {
gZero[i] = 0;
gVec[i] = 0;
angle[i] = 0;
}
calibrated = false;
}
void setOutputBit(unsigned char state){
unsigned char temp;
readI2C(SlaveAddress,CTRL_REG1,&temp,1);
if(state)
temp &= ~CTRL_REG1_FREAD;
else
temp |= CTRL_REG1_FREAD;
writeI2C(SlaveAddress,CTRL_REG1,&temp,1);
}
void executeMMA8451Q(unsigned char* buffer, unsigned int length) {
enableI2C(12);
setPwrBusStatus(PM_SYSPWR, PM_ENABLE);
initMMA8451Q();
attachInterrupt(mma8451qVector, 1,6, 1);
setActiveMMA8451Q(1);
LPM3;
detachInterrupt(1,6);
unsigned char statreg[2] = {0x00, 0x01};
writeI2C(0x1c, statreg, 1, 0);
readI2C(0x1c, statreg , 1, 1);
writeI2C(0x1c, statreg+1, 1, 0);
readI2C(0x1c, buffer , 96, 1);
setActiveMMA8451Q(0);
disableI2C();
setPwrBusStatus(PM_SYSPWR, PM_DISABLE);
return;
}
IOReturn AppleLM8x::restoreRegisters()
{
IOReturn status;
DLOG("AppleLM8x::restoreRegisters(0x%x) entered.\n", kLM8xAddr<<1);
status = openI2C(kLM8xBus);
if(status != kIOReturnSuccess)
{
DLOG("AppleLM8x::restoreRegisters(0x%x) failed to open I2C bus.\n", kLM8xAddr<<1);
return status;
}
status = writeI2C((UInt8)kChannelModeRegister, (UInt8 *)&savedRegisters.ChannelMode, 1);
if(status != kIOReturnSuccess)
{
DLOG("AppleLM8x::restoreRegisters(0x%x) writeI2C failed.\n", kLM8xAddr<<1);
closeI2C();
return status;
}
status = writeI2C((UInt8)kConfReg2, (UInt8 *)&savedRegisters.Configuration2, 1);
if(status != kIOReturnSuccess)
{
DLOG("AppleLM8x::restoreRegisters(0x%x) writeI2C failed.\n", kLM8xAddr<<1);
closeI2C();
return status;
}
// restore Configuration Register 1 last, since it starts polling
status = writeI2C((UInt8)kConfReg1, (UInt8 *)&savedRegisters.Configuration1, 1);
if(status != kIOReturnSuccess)
{
DLOG("AppleLM8x::restoreRegisters(0x%x) writeI2C failed.\n", kLM8xAddr<<1);
closeI2C();
return status;
}
closeI2C();
return status;
}
bool HTSPBwriteLED(tSensors link, ubyte mask) {
memset(HTSPB_I2CRequest, 0, sizeof(tByteArray));
HTSPB_I2CRequest[0] = 3; // Message size
HTSPB_I2CRequest[1] = HTSPB_I2C_ADDR; // I2C Address
HTSPB_I2CRequest[2] = HTSPB_OFFSET + HTSPB_LED; // Start LED read address
HTSPB_I2CRequest[3] = mask; // The specified digital ports
return writeI2C(link, HTSPB_I2CRequest);
}
uint16_t getGyroSample(uint16_t index, uint8_t *array){
startI2C(MPU9150ADDR, WRITE);
writeI2C(0x43);
stopI2C();
startI2C(MPU9150ADDR, READ);
for(uint8_t k=0; k<6; k++){
if(index > 54-1) break;
uint8_t ackType = (k < (6-1))? ACK : NACK ;
array[index++] = readI2C(ackType);
}
stopI2C();
return index;
}
//______________________________________________________________________________________
int readI2C(_i2c *p, char* dBuffer, int length) {
static
int nrpt=3;
int to=__time__+25;
if(p) {
p->nrx=length;
if(dBuffer[1])
writeI2C(p,dBuffer,2);
else
writeI2C(p,dBuffer,1);
while(p->nrx && __time__< to)
_proc_loop();
if(p->nrx) {
Reset_I2C(p);
if(--nrpt)
return readI2C(p, dBuffer, length);
}
nrpt=3;
memcpy(dBuffer,p->rxbuf,length);
if(p->nrx) {
return(0);
}
} else
while(length--)
dBuffer[length]=0;
_CLEAR_ERROR(pfm,PFM_I2C_ERR);
if(_DBG(pfm,_DBG_I2C_RX)) {
_io *io=_stdio(__dbug);
int i;
__print(":%04d",__time__ % 10000);
for(i=0;i<length;++i)
__print(" %02X",p->rxbuf[i]);
__print("\r\n>");
_stdio(io);
}
return(-1);
}
//=========================================
//功能：初始化MMA8451
//输入：无
//输出：无
//返回：无
//=========================================
void Init_MMA8452(void)
{
unsigned char temp;
//设置休眠
MMA845x_Standby();
/* 设置采样频率为100Hz */
MMA845x_Output_Data_Rates(Output_Data_Rates_400);
setOutput_8Bit;
readI2C(SlaveAddress,CTRL_REG4,&temp,1);
temp |= CTRL_REG4_INT_EN_DRDY;
writeI2C(SlaveAddress,CTRL_REG4,&temp,1);
readI2C(SlaveAddress,CTRL_REG5,&temp,1);
temp |= CTRL_REG5_INT_CFG_DRDY;
writeI2C(SlaveAddress,CTRL_REG5,&temp,1);
/* 将设备切换到主动模式 */
MMA845x_Active();
}
void initMMA8451Q() {
writeI2C(0x1c, smode, 2, 1); //0x09 1010 0000 A0 samples 10XX XXXX
writeI2C(0x1c, srnge, 2, 1); //0x0E 0000 0000 00 range selection 0000 00 XX
writeI2C(0x1c, sctl1, 2, 1); //0x2A 0011 1010 3A
writeI2C(0x1c, sctl2, 2, 1); //0x2B 0001 1011 1B
writeI2C(0x1c, sctl4, 2, 1); //0x2D 0100 0000 40
writeI2C(0x1c, sctl5, 2, 1); //0x2E 0100 0000 40
}
void MPU9150Mode(int8_t mode){
if(mode == SLEEP){ // || mode == ARMED){
startI2C(MPU9150ADDR, WRITE);
writeI2C(0x6B); // PWR_MGMT_1
writeI2C(1<<6); // SLEEP bit set
stopI2C();
}
else{ //if(mode == ACTIVE){
startI2C(MPU9150ADDR, WRITE);
writeI2C(0x6B); // PWR_MGMT_1
writeI2C(0x03); // Z-Axis Gyro Reference
stopI2C();
_delay_ms(1);
startI2C(MPU9150ADDR, WRITE);
writeI2C(0x19); // SMPRT_DIV
writeI2C(0x00); // SMPRT_DIV
writeI2C(0x00); // CONFIG
writeI2C(3<<3); // GYRO_CONFIG
writeI2C((3<<3)|(0)); // ACCEL_CONFIG
stopI2C();
}
}
uint8_t* HMC5883L::Read(int address, int length)
{
writeI2C( address );
Wire.beginTransmission(HMC5883L_Address);
Wire.requestFrom(HMC5883L_Address, length);
uint8_t buffer[length];
if(Wire.available() == length)
{
for(uint8_t i = 0; i < length; i++)
{
buffer[i] = Wire.read();
}
}
Wire.endTransmission();
return buffer;
}
IOReturn
IOI2CDevice::writeI2C(
UInt32 subAddress,
UInt8 *data,
UInt32 count,
UInt32 clientKey,
UInt32 mode,
UInt32 retries,
UInt32 timeout_uS,
UInt32 options)
{
IOI2CCommand cmd = {0};
cmd.command = kI2CCommand_Write;
cmd.mode = mode;
cmd.subAddress = subAddress;
cmd.count = count;
cmd.buffer = data;
cmd.retries = retries;
cmd.timeout_uS = timeout_uS;
cmd.options = options;
return writeI2C(&cmd, clientKey);
}
IOReturn AppleLM8x::initHW(IOService *provider)
{
IOReturn status;
UInt8 cfgReg = 0;
UInt8 attempts = 0;
DLOG("AppleLM8x::initHW(0x%x) entered.\n", kLM8xAddr<<1);
status = openI2C(kLM8xBus);
if(status != kIOReturnSuccess)
{
DLOG("AppleLM8x::initHW(0x%x) failed to open I2C bus.\n", kLM8xAddr<<1);
return status;
}
// Attempt to start LM87 by setting bit 0 in Configuration Register 1. If we detect the RESET
// bit, then make a second attempt.
for ( ; attempts < 2; attempts++ )
{
// Read Configuration Register 1
status = readI2C((UInt8)kConfReg1, (UInt8 *) &cfgReg, 1);
if(status != kIOReturnSuccess)
{
DLOG("AppleLM8x::initHW(0x%x) readI2C failed.\n", kLM8xAddr<<1);
break;
}
DLOG("AppleLM8x::readI2C(0x%x) retrieved data = 0x%x\n", kLM8xAddr<<1, cfgReg);
// If we detect RESET, wait at least 45ms for it to clear.
if ( cfgReg & kConfRegRESET )
{
IOSleep(50);
status = kIOReturnError;
}
if(status != kIOReturnSuccess)
{
IOLog("AppleLM8x::initHW(0x%x) readI2C detected RESET bit.\n", kLM8xAddr<<1);
}
// Start monitoring operations
cfgReg = 0x1;
// Failure of this write operation is fatal
status = writeI2C((UInt8)kConfReg1, (UInt8 *)&cfgReg, 1);
// Read Configuration Register 1
status = readI2C((UInt8)kConfReg1, (UInt8 *) &cfgReg, 1);
if(status != kIOReturnSuccess)
{
DLOG("AppleLM8x::initHW(0x%x) readI2C failed.\n", kLM8xAddr<<1);
break;
}
if ( cfgReg == kConfRegStart )
{
break;
}
else
{
IOLog("AppleLM8x::readI2C(0x%x) retrieved configuration register 1 = 0x%x\n", kLM8xAddr<<1, cfgReg);
}
}
closeI2C();
return status;
}
IOReturn
IOI2CDevice::callPlatformFunction(
const OSSymbol *functionName,
bool waitForFunction,
void *param1, void *param2,
void *param3, void *param4 )
{
IOReturn status;
if (0 == functionName)
return kIOReturnBadArgument;
if (0 == (fStateFlags & kStateFlags_TEARDOWN))
{
if (symReadI2CBus->isEqualTo(functionName))
return readI2C((IOI2CCommand *)param1, (UInt32)param2);
else
if (symWriteI2CBus->isEqualTo(functionName))
return writeI2C((IOI2CCommand *)param1, (UInt32)param2);
else
if (symLockI2CBus->isEqualTo(functionName))
return lockI2CBus((UInt32 *)param2);
else
if (symUnlockI2CBus->isEqualTo(functionName))
return unlockI2CBus((UInt32)param2);
else
if (symClientRead->isEqualTo(functionName))
return readI2C((UInt32)param1, (UInt8 *)param2, (UInt32)param3, (UInt32)param4);
else
if (symClientWrite->isEqualTo(functionName))
return writeI2C((UInt32)param1, (UInt8 *)param2, (UInt32)param3, (UInt32)param4);
else
if (functionName->isEqualTo("IOI2CSetDebugFlags"))
{
UInt32 flags = ( (UInt32)param1 & ( kStateFlags_IOLog | kStateFlags_kprintf ) );
DLOG("IOI2CDevice@%lx IOI2CSetDebugFlags:%lx %s\n", (unsigned long int)getI2CAddress(), (unsigned long int)flags, ((UInt32)param2 == true)?"TRUE":"FALSE");
if ((UInt32)param2 == true)
fStateFlags |= flags; // set the debug flags
else
fStateFlags &= ~flags; // clear the debug flags
return kIOReturnSuccess;
}
// If no other symbol matched - check for OnDemand platform function.
if (fEnableOnDemandPlatformFunctions)
{
const char *cstr;
if ((cstr = functionName->getCStringNoCopy()) && (0 == strncmp("platform-do-", cstr, strlen("platform-do-"))))
{
IOPlatformFunction *pfFunc;
if (kIOReturnSuccess == (status = getPlatformFunction(functionName, &pfFunc, kIOPFFlagOnDemand)))
return performFunction (pfFunc, param1, param2, param3, param4);
// If the function wasn't found then forward the request to our provider...
// But if some other error occurred then return the status now.
if (kIOReturnNotFound != status)
return status;
}
}
} // kStateFlags_TEARDOWN
return super::callPlatformFunction(functionName, waitForFunction, param1, param2, param3, param4);
}
void write2I2C(unsigned char addr, unsigned char val1, unsigned char val2) {
startI2C(addr, I2C_WRITE);
writeI2C(val1);
writeI2C(val2);
stopI2C();
}
void write1I2C(unsigned char addr, unsigned char val) {
startI2C(addr, I2C_WRITE);
writeI2C(val);
stopI2C();
}
IOReturn
IOI2CDevice::performFunction(
IOPlatformFunction *func,
void *pfParam1,
void *pfParam2,
void *pfParam3,
void *pfParam4)
{
IOReturn status = kIOReturnSuccess;
IOPlatformFunctionIterator *iter;
UInt8 scratchBuffer[kI2CPF_READ_BUFFER_LEN] = {0};
UInt8 readBuffer[kI2CPF_READ_BUFFER_LEN] = {0};
UInt32 mode = kI2CMode_Unspecified;
UInt8 *maskBytes, *valueBytes;
unsigned delayMS;
UInt32 cmd, cmdLen, param1, param2, param3, param4, param5,
param6, param7, param8, param9, param10;
UInt32 key = 0;
bool i2cIsLocked = FALSE;
bool isI2CFunction = FALSE;
DLOG ("IOI2CDevice::performFunction(%lx) - entered\n", fI2CAddress);
if (!func)
return kIOReturnBadArgument;
if (!(iter = func->getCommandIterator()))
return kIOReturnNotFound;
// Check for I2C function...
while (iter->getNextCommand (&cmd, &cmdLen, ¶m1, ¶m2, ¶m3, ¶m4,
¶m5, ¶m6, ¶m7, ¶m8, ¶m9, ¶m10, (UInt32 *)&status)
&& (status != kIOPFNoError))
{
if ((cmd == kCommandReadI2CSubAddr) || (cmd == kCommandWriteI2CSubAddr))
{
isI2CFunction = TRUE;
break;
}
}
if (status == kIOReturnSuccess)
{
if (isI2CFunction)
{
if (kIOReturnSuccess == (status = lockI2CBus(&key)))
i2cIsLocked = TRUE;
}
}
if (status == kIOReturnSuccess)
{
iter->reset();
while (iter->getNextCommand (&cmd, &cmdLen, ¶m1, ¶m2, ¶m3, ¶m4,
¶m5, ¶m6, ¶m7, ¶m8, ¶m9, ¶m10, (UInt32 *)&status)
&& (status != kIOPFNoError))
{
DLOG ("IOI2CDevice::performFunction(%lx) - 1)0x%lx, 2)0x%lx, 3)0x%lx, 4)0x%lx, 5)0x%lx,"
"6)0x%lx, 7)0x%lx, 8)0x%lx, 9)0x%lx, 10)0x%lx\n", getI2CAddress(), param1, param2, param3,
param4, param5, param6, param7, param8, param9, param10);
switch (cmd)
{
case kCommandDelay:
delayMS = param1 / 1000; // convert param1 from uS to mS.
DLOG("IOI2CDevice::performFunction(%lx) delay %u\n", getI2CAddress(), delayMS);
if (delayMS != 0)
IOSleep(delayMS);
break;
case kCommandReadI2CSubAddr:
if (param2 > kI2CPF_READ_BUFFER_LEN)
{
status = kIOPFBadCmdLength;
ERRLOG("IOI2CDevice::performFunction(%lx) r-sub operation too big!\n", getI2CAddress());
break;
}
if (mode == kI2CMode_Unspecified)
{
status = kIOReturnUnsupportedMode;
ERRLOG("IOI2CDevice::performFunction(%lx) Rd Unspecified I2C mode\n", getI2CAddress());
break;
}
status = readI2C(param1, readBuffer, param2, key, mode);
break;
case kCommandWriteI2CSubAddr:
if (mode == kI2CMode_Unspecified)
{
status = kIOReturnUnsupportedMode;
ERRLOG("IOI2CDevice::performFunction(%lx) Wt Unspecified I2C mode\n", getI2CAddress());
break;
}
DLOG("IOI2CDevice::performFunction(%lx) w-sub %lx len %lx data", getI2CAddress(), param1, param2);
status = writeI2C((UInt32) param1, (UInt8 *) param3, (UInt32) param2, key, mode);
break;
case kCommandI2CMode:
switch (param1)
{
default:
case kPFMode_Dumb: status = kIOReturnUnsupportedMode; break;
case kPFMode_Standard: mode = kI2CMode_Standard; break;
case kPFMode_Subaddress: mode = kI2CMode_StandardSub; break;
case kPFMode_Combined: mode = kI2CMode_Combined; break;
}
DLOG("IOI2CDevice::performFunction(%lx) PF mode %lx\n", getI2CAddress(), mode);
break;
case kCommandRMWI2CSubAddr:
// check parameters
if ((param2 > kI2CPF_READ_BUFFER_LEN) || // number of mask bytes
(param3 > kI2CPF_READ_BUFFER_LEN) || // number of value bytes
(param4 > kI2CPF_READ_BUFFER_LEN) || // number of transfer bytes
(param3 > param2)) // param3 is not actually used, we assume that
// any byte that is masked also gets a value OR'ed in.
{
ERRLOG("IOI2CDevice::performFunction(%lx) invalid mw-sub cycle\n", getI2CAddress());
status = kIOReturnAborted;
break;
}
if (mode == kI2CMode_Unspecified)
{
status = kIOReturnUnsupportedMode;
ERRLOG("IOI2CDevice::performFunction(%lx) RMW Unspecified I2C mode\n", getI2CAddress());
break;
}
maskBytes = (UInt8 *) param5;
valueBytes = (UInt8 *) param6;
// Do the modify write operation on the previously read data buffer.
for (unsigned int index = 0; index < param2; index++) // param2 = number of mask bytes
{
scratchBuffer[index] = ((valueBytes[index] & maskBytes[index]) |
(readBuffer[index] & ~maskBytes[index]));
}
DLOG("IOI2CDevice::performFunction(%lx) mw-sub %lx len %lx data", getI2CAddress(), param1, param4);
status = writeI2C((UInt8) param1, scratchBuffer, (UInt16) param4, key, mode);
break;
default:
ERRLOG ("IOI2CDevice::performFunction - bad command %ld\n", cmd);
status = kIOReturnAborted;
break;
}
if (status != kIOReturnSuccess)
break;
}
}
if (iter)
iter->release();
if (i2cIsLocked)
unlockI2CBus(key);
DLOG ("IOI2CDevice::performFunction - done status: %x\n", status);
return status;
}
////========得到X轴的数据========================================
//int Get_x(void)
//{
// dis_data = (BUF[0]<<8)|BUF[1]; //合成数据
// dis_data >>= 4; //数据合成，OUT_X_MSB :[7-0],OUT_X_LSB:[7-2],故合成后的数据需要右移两位
// /* if(dis_data&0x800){
// dis_data |= 0xf800;
//}
// x = (float)dis_data;
// x /= 1024;
// x *= 9.8;
// dis_data = x;*/
//
// return dis_data;
//}
//
////========得到Y轴的数据====================================
//int Get_y(void)
//{
// dis_data = (BUF[2]<<8)|BUF[3]; //合成数据
// dis_data >>= 4; //数据合成，OUT_X_MSB :[7-0],OUT_X_LSB:[7-2],故合成后的数据需要右移两位
// /* if(dis_data&0x800){
// dis_data |= 0xf800;
//}
// x = (float)dis_data;
// x /= 1024;
// x *= 9.8;
// //dis_data = x;*/
//
// return dis_data;
//}
////========得到Z轴的数据====================================
//int Get_z(void)
//{
// dis_data = (BUF[4]<<8)|BUF[5]; //合成数据
// dis_data >>= 4; //数据合成，OUT_X_MSB :[7-0],OUT_X_LSB:[7-2],故合成后的数据需要右移两位
// if(dis_data&0x800){
// dis_data |= 0xf800;
// }
// /*x = (float)dis_data;
// x /= 1024;
// x *= 9.8;
// //dis_data = x;*/
//
// return dis_data;
//}
//==============END OF FILE=================================
void MMA845x_Standby(void){
unsigned char temp;
temp = Single_Read_MMA8452(CTRL_REG1)&~CTRL_REG1_ACTIVE;
writeI2C(SlaveAddress,CTRL_REG1,&temp,1);
//Single_Write_MMA8452(CTRL_REG1,temp&~CTRL_REG1_ACTIVE);
}
bool IOI2CMaxim1631::start( IOService *nub )
{
IOReturn status;
UInt8 configReg, cmdByte;
DLOG("IOI2CMaxim1631::start - entered\n");
if (false == super::start(nub))
{
DLOG( "IOI2CMaxim1631::start - super::start failed. Exiting...\n" );
return false;
}
if (!fGetSensorValueSym)
fGetSensorValueSym = OSSymbol::withCString("getSensorValue");
// According to the I2C gurus, accessing the config register is done by a COMBINED mode
// (default for readI2C) I2C access with the 'kAccessConfigurationByte' command as the
// "sub-addr" value.
if (kIOReturnSuccess != (status = readI2C( kAccessConfigurationByte, &configReg, 1 )))
{
IOLog("IOI2CMaxim1631@%lx::start unable to read config reg\n", getI2CAddress());
freeI2CResources();
return false;
}
DLOG( "IOI2CMaxim1631::start - read config register- value = 0x%02X\n", configReg );
if ( (configReg & 0x01) != 0 ) // not in continuous conversion mode (bit 0 == 1 is 1SHOT mode)
{
IOLog( "IOI2CMaxim1631::start - Note: not in continuous conversion mode. Setting mode.\n" );
// should we also be setting the resolution bits here? -- bg
configReg &= ~0x01; // set 1SHOT bit to zero for continuous conversion mode
// to write to the config register, you perform a Standard SubAddr I2C transaction
// (which is the default mode for writeI2C()), specifying 'kAccessConfigurationByte'
// command as the sub-addr.
if ( kIOReturnSuccess != ( status = writeI2C( kAccessConfigurationByte, &configReg, 1 )) )
{
IOLog( "IOI2CMaxim1631::start - unable to turn off 1SHOT mode! Cannot provide temperature values.\n" );
freeI2CResources();
return false;
}
}
// Tell the sensor to go into continuous temp. conversion mode.
// Talked to the I2C gurus and according to the diagram for the interface diagram
// for issuing the 'kStartConvertT' command, one needs to issue a Standard I2C write
// transaction. The data buffer contains one (1) byte, which is the command byte,
// and we override the default "mode" so that IOI2CFamily issues the correct type
// of transaction. [rdar://problem/4118773]
cmdByte = kStartConvertT;
if ( kIOReturnSuccess != ( status = writeI2C( 0 /* no subaddr */, &cmdByte, 1, kIOI2C_CLIENT_KEY_DEFAULT, kI2CMode_Standard ) ) )
{
IOLog( "IOI2CMaxim1631::start - unable to start sensor (status = 0x%08X)\n", status );
}
// tell the world i'm here
registerService();
// Publish any child nubs under the max1631 node...
publishChildren(nub);
return(true);
}
int main()
{
system("stty -F /dev/ttyACM0 115200 cs8 ignbrk -brkint -icrnl -imaxbel -opost -onlcr -isig -icanon -iexten -echo -echoe -echok -echoctl -echoke noflsh -ixon -crtscts");
pSerialOut = fopen("/dev/ttyACM0","w+");
if (!pSerialOut)
{
printf("Couldn't open the serial port comm :(\n");
return 1;
}
i2cBus = open("/dev/i2c-1", O_RDWR);
if (i2cBus > 0)
{
selectI2CDevice(ACCEL_BUS_ADDRESS);
sleep(1);
writeI2C(0x2D, 0x08); // enable accel
printf("Accel and gyro up?\n");
}
sleep(2);
char buffer[512];
printf("received: %s\n", fgets(buffer, 512, pSerialOut));
fprintf(pSerialOut, "%c00\n", 253); // turn motors on
leg leftLeg(5,4,3,2, 1, -1, 1, 1);
leftLeg.servoOffsets[0] = -0.5f;
leftLeg.servoOffsets[1] = -0.25f;
leftLeg.servoOffsets[2] = -0.75f;
leg rightLeg(32,33,34,35, 1, -1, 1, 1);
rightLeg.servoOffsets[0] = -1.0f;
rightLeg.servoOffsets[1] = 0.f;
rightLeg.servoOffsets[2] = -0.5f;
setServo(2,0);
setServo(35,0);
leftLeg.Update();
rightLeg.Update();
float t = 0;
while(1)
{
t += 0.1f;
Vector orientation = getOrientation();
orientation.x = cosf(t/2);
orientation.x = ( (orientation.x*fabs(orientation.x)) * 1.0f ) + 0.25f;
printf("or: %f\n", orientation.x);
setServo(2, orientation.x+0.25f);
setServo(35, -orientation.x);
float stepSize = 0.3f;
float stepHeight = 0.75f;
for (int iter = 0; iter < 100; iter++)
{
leftLeg.hip.position = Vector(0,6.0f,-0.01f);
leftLeg.hip.UpdateChildren();
Vector footTarget(0,stepHeight*(cosf(t)+3.1415f), cosf(t/2)*stepSize);
leftLeg.ankle.IKSetEndPosition(footTarget, Vector(0,-1,0));
rightLeg.hip.position = Vector(0,5.0f,-0.01f);
rightLeg.hip.UpdateChildren();
Vector rightFootTarget(0, stepHeight*(cosf(t+1.553f)+3.1415), cosf(t/2)*stepSize);
rightLeg.ankle.IKSetEndPosition(rightFootTarget, Vector(0,-1,0));
}
leftLeg.Update();
rightLeg.Update();
usleep(70000);
}
return 0;
}
BMA180::BMA180() {
readI2C(ACCADDR, 0x00, 1, buffer);
sp("BMA180 ID = ");
spln((int) buffer[0]);
// Set ee_w bit
readI2C(ACCADDR, CTRLREG0, 1, buffer);
buffer[0] |= 0x10; // Bitwise OR operator to set ee_w bit.
writeI2C(ACCADDR, CTRLREG0, 1, buffer); // Have to set ee_w to write any other registers.
// Set range.
readI2C(ACCADDR, OLSB1, 1, buffer);
buffer[0] &= (~0x0e); // Clear old ACC_RANGE bits.
buffer[0] |= (ACC_RANGE << 1); // Need to shift left one bit; refer to DS p. 21.
writeI2C(ACCADDR, OLSB1, 1, buffer); // Write new ACC_RANGE data, keep other bits the same.
// Set ADC resolution (DS p. 8).
res = 8000; // == 1/0.000125 // [ -1, 1] g
if (ACC_RANGE == 1) res /= 1.5; // [ -1.5, 1.5] g
else if (ACC_RANGE == 2) res /= 2; // [ -2, 2] g
else if (ACC_RANGE == 3) res /= 3; // [ -3, 3] g
else if (ACC_RANGE == 4) res /= 4; // [ -4, 4] g
else if (ACC_RANGE == 5) res /= 8; // [ -8, 8] g
else if (ACC_RANGE == 6) res /= 16; // [ -16, 16] g
// Set bandwidth.
// ACC_BW bandwidth (Hz)
// 0 10
// 1 20
// 2 40
// 3 75
// 4 150
// 5 300
// 6 600
// 7 1200
readI2C(ACCADDR, BWTCS, 1, buffer);
buffer[0] &= (~0xf0); // Clear bandwidth bits <7:4>.
buffer[0] |= (ACC_BW << 4); // Need to shift left four bits; refer to DS p. 21.
writeI2C(ACCADDR, BWTCS, 1, buffer); // Keep tcs<3:0> in BWTCS, but write new ACC_BW.
// Set mode_config to 0x01 (ultra low noise mode, DS p. 28).
//readI2C(ACCADDR, 0x30, 1, buffer);
//buffer[0] &= (~0x03); // Clear mode_config bits <1:0>.
//buffer[0] |= 0x01;
//writeI2C(ACCADDR, 0x30, 1, buffer);
spln("BMA180 configured!");
for (int i=0; i<3; i++) {
aRaw[i] = 0;
aVec[i] = 0;
}
// Zero buffer.
for (int i=0; i<6; i++) {
buffer[i] = 0;
}
// Low-pass filter.
#ifdef ACC_LPF_DEPTH
lpfIndex = 0;
for (int i=0; i<3; i++)
for (int j=0; j<ACC_LPF_DEPTH; j++)
lpfVal[i][j] = 0;
#endif // ACC_LPF_DEPTH
}
