/*I2C读字符串 功能暂时不提供
//input : byIicPort I2C端口号为 0 ~ 1
// bySlave 设备号
// dwAddr 命令地址
// dwCount 命令长度
//output: pbyData 命令值
return: SYS_OK
SYS_ERR
*/
SDWORD I2cReadSeq(BYTE byIicPort, BYTE bySlave, DWORD dwAddr, BYTE *pbyData, DWORD dwCount)
{
volatile BYTE i2c_bw_ctrl; /* I2C write control */
volatile BYTE i2c_br_ctrl; /* I2C read control */
volatile BYTE i2c_addr_00; /* I2C address (0) */
int i;
STATUS semStatus; /*i2c同步信号量*/
if (0 == dwCount)
{
return SYS_ERROR;
}
semStatus = semTake(semI2cLock, WAIT_FOREVER); /*获得信号量*/
if(ERROR == semStatus)
{
logMsg("i2c semTake ERROR\n", 0, 0, 0, 0, 0, 0);
return;
}
if(0 == byIicPort)
{
sCurI2cPort.byClkPin = IICCLK0;
sCurI2cPort.byDataPin = IICDATA0;
}
else if(1 == byIicPort)
{
sCurI2cPort.byClkPin = IICCLK1;
sCurI2cPort.byDataPin = IICDATA1;
}
#ifdef I2C_DEBUG
printf("\n i2cReadSeq() slave address is 0x%x", bySlave);
#endif /* I2C_DEBUG */
i2c_ack_stat=0x0;
i2c_bw_ctrl = bySlave | I2C_WRITE;
i2c_br_ctrl = bySlave | I2C_READ;
/*i2c_addr_01 = I2C_W1_ADDR(dwAddr);*/
i2c_addr_00 = I2C_W0_ADDR(dwAddr);
if(1)
{
i2cStart();
I2cWriteByte(i2c_bw_ctrl);
ack1=I2cReadBit(); /*read ack from slave*/
if(ack1!=0)
i2c_ack_stat |= 0x1;
I2cWriteByte(i2c_addr_00);
ack1=I2cReadBit(); /*read ack from slave*/
if(ack1!=0)
i2c_ack_stat |= 0x2;
i2cStart();
I2cWriteByte(i2c_br_ctrl);
ack1=I2cReadBit(); /*read ack from slave*/
if(ack1!=0)
i2c_ack_stat |= 0x3;
for(i=0;i<dwCount;i++)
{
pbyData[i] = I2cReadByte();
if(i != (dwCount-1))
I2cWriteBit(0x0); /*send ack*/
else
I2cWriteBit(0x80); /*send nack after reading the last byte*/
I2cData(HIGH);
}
I2cStop();
}
if(i2c_ack_stat!=0)
printf("\nError in read, ack not recd, i2c_ack_stat=0x%x\n",i2c_ack_stat);
semGive(semI2cLock); /*释放信号量*/
if(0 != i2c_ack_stat)
{
return SYS_ERROR;
}
return SYS_OK;
}
SDWORD I2cWrite(BYTE byIicPort, BYTE bySlave, DWORD dwAddr, BYTE byData)
{
volatile BYTE i2c_bw_ctrl; /* I2C write control */
volatile BYTE i2c_addr_00; /* I2C address (0) */
STATUS semStatus; /*i2c同步信号量*/
semStatus = semTake(semI2cLock, WAIT_FOREVER); /*获得信号量*/
if(ERROR == semStatus)
{
logMsg("i2c semTake ERROR\n", 0, 0, 0, 0, 0, 0);
return;
}
if(0 == byIicPort)
{
sCurI2cPort.byClkPin = IICCLK0;
sCurI2cPort.byDataPin = IICDATA0;
}
else if(1 == byIicPort)
{
sCurI2cPort.byClkPin = IICCLK1;
sCurI2cPort.byDataPin = IICDATA1;
}
i2c_ack_stat=0x0;
i2c_bw_ctrl = bySlave | I2C_WRITE;
/*i2c_addr_01 = I2C_W1_ADDR(dwAddr);*/
i2c_addr_00 = I2C_W0_ADDR(dwAddr);
if(1)/*0 == I2cDeviceBusy(bySlave))*/
{
i2cStart();
I2cWriteByte(i2c_bw_ctrl);
ack1=I2cReadBit();
if(ack1!=0)
i2c_ack_stat |= 0x2;
I2C_DELAY(CLOCK_LOW_TIME*3);
/*I2cWriteByte(i2c_addr_01);
ack1=I2cReadBit();
if(ack1!=0)
i2c_ack_stat |= 0x2;*/
I2cWriteByte(i2c_addr_00);
ack1=I2cReadBit();
if(ack1!=0)
i2c_ack_stat |= 0x2;
I2C_DELAY(CLOCK_LOW_TIME*3);
I2cWriteByte(byData);
ack2=I2cReadBit();
if(ack2!=0)
i2c_ack_stat |= 0x8;
I2C_DELAY(CLOCK_LOW_TIME*3);
I2cStop();
}
semGive(semI2cLock); /*释放信号量*/
if(0 != i2c_ack_stat)
{
printf("\nError in write, ack not recd, i2c_ack_stat=0x%x\n",i2c_ack_stat);
return SYS_ERROR;
}
return SYS_OK;
}
/*
* Application entry point.
*/
int main(void) {
msg_t status = MSG_OK;
systime_t tmo = MS2ST(4);
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Starts I2C
*/
i2cStart(&I2CD2, &i2cfg2);
/*
* Prepares the Serial driver 2
*/
sdStart(&SD2, NULL); /* Default is 38400-8-N-1.*/
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
/**
* Prepares the accelerometer
*/
txbuf[0] = ACCEL_CTRL_REG1; /* register address */
txbuf[1] = 0x1;
i2cAcquireBus(&I2CD2);
status = i2cMasterTransmitTimeout(&I2CD2, mma8451_addr, txbuf, 2, rxbuf, 0, tmo);
i2cReleaseBus(&I2CD2);
if (status != MSG_OK){
errors = i2cGetErrors(&I2CD2);
}
/*
* Normal main() thread activity, nothing in this test.
*/
while (TRUE) {
palTogglePad(GPIOB, GPIOB_LED_B);
chThdSleepMilliseconds(100);
txbuf[0] = ACCEL_OUT_DATA; /* register address */
i2cAcquireBus(&I2CD2);
status = i2cMasterTransmitTimeout(&I2CD2, mma8451_addr, txbuf, 1, rxbuf, 6, tmo);
i2cReleaseBus(&I2CD2);
if (status != MSG_OK){
errors = i2cGetErrors(&I2CD2);
}
acceleration_x = complement2signed(rxbuf[0], rxbuf[1]);
acceleration_y = complement2signed(rxbuf[2], rxbuf[3]);
acceleration_z = complement2signed(rxbuf[4], rxbuf[5]);
print("x: ");
printn(acceleration_x);
print(" y: ");
printn(acceleration_y);
print(" z: ");
printn(acceleration_z);
println("");
}
}
SDWORD I2cRead1014(BYTE byIicPort, BYTE bySlave, DWORD dwAddr, BYTE *pbyData)
{
BYTE byData = 0;
volatile BYTE i2c_bw_ctrl; /* I2C write control */
volatile BYTE i2c_br_ctrl; /* I2C read control */
volatile BYTE i2c_addr_00; /* I2C address (0) */
STATUS semStatus; /*i2c同步信号量*/
semStatus = semTake(semI2cLock, WAIT_FOREVER); /*获得信号量*/
if(ERROR == semStatus)
{
logMsg("i2c semTake ERROR\n", 0, 0, 0, 0, 0, 0);
return;
}
if(0 == byIicPort)
{
sCurI2cPort.byClkPin = IICCLK0;
sCurI2cPort.byDataPin = IICDATA0;
}
else if(1 == byIicPort)
{
sCurI2cPort.byClkPin = IICCLK1;
sCurI2cPort.byDataPin = IICDATA1;
}
#ifdef I2C_DEBUG
printf("\n i2cRead() slave address is 0x%08x, addr: 0x%08x", bySlave, dwAddr);
#endif /* I2C_DEBUG */
i2c_ack_stat=0x0;
i2c_bw_ctrl = bySlave | I2C_WRITE;
i2c_br_ctrl = bySlave | I2C_READ;
/*i2c_addr_01 = I2C_W1_ADDR(addr);*/
i2c_addr_00 = I2C_W0_ADDR(dwAddr);
if(1)
{
i2cStart();
I2cWriteByte(i2c_bw_ctrl);
ack1=I2cReadBit(); /*read ack from slave*/
if(ack1!=0)
i2c_ack_stat |= 0x1;
I2C_DELAY(CLOCK_LOW_TIME*3);
/*I2cWriteByte(i2c_addr_01);*/
/*ack1=I2cReadBit();*/ /*read ack from slave*/
/*if(ack1!=0)
i2c_ack_stat |= 0x2; */
I2cWriteByte(i2c_addr_00);
ack1=I2cReadBit(); /*read ack from slave*/
if(ack1!=0)
i2c_ack_stat |= 0x2;
I2cStop();
I2C_DELAY(255);
I2C_DELAY(255);
i2cStart();
I2C_DELAY(CLOCK_LOW_TIME*3);
I2cWriteByte(i2c_br_ctrl);
ack1=I2cReadBit(); /*read ack from slave*/
if(ack1!=0)
i2c_ack_stat |= 0x3;
I2C_DELAY(CLOCK_LOW_TIME*3);
byData = I2cReadByte();
I2cStop();
}
#ifdef I2C_DEBUG
if(i2c_ack_stat!=0)
printf("\nError in read, ack not recd, i2c_ack_stat=0x%x\n",i2c_ack_stat);
#endif
*pbyData = byData;
semGive(semI2cLock); /*释放信号量*/
if(0 != i2c_ack_stat)
{
return SYS_ERROR;
}
return SYS_OK;
}
/**
* @brief Configures and activates LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
* @param[in] config pointer to the @p LSM6DS0Config object
*
* @api
*/
void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
uint32_t i;
uint8_t cr[5];
osalDbgCheck((devp != NULL) && (config != NULL) &&
((devp->config->acccfg != NULL) ||
(devp->config->gyrocfg != NULL)));
osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
"lsm6ds0Start(), invalid state");
devp->config = config;
/* Control register 8 configuration block.*/
{
cr[0] = LSM6DS0_AD_CTRL_REG8;
cr[1] = LSM6DS0_CTRL_REG8_IF_ADD_INC;
#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
cr[1] |= devp->config->endianness | devp->config->blockdataupdate;
#endif
}
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
if((devp)->config->acccfg != NULL) {
cr[0] = LSM6DS0_AD_CTRL_REG5_XL;
/* Control register 5 configuration block.*/
{
cr[1] = LSM6DS0_CTRL_REG5_XL_XEN_XL | LSM6DS0_CTRL_REG5_XL_YEN_XL |
LSM6DS0_CTRL_REG5_XL_ZEN_XL;
#if LSM6DS0_ACC_USE_ADVANCED || defined(__DOXYGEN__)
cr[1] |= devp->config->acccfg->decmode;
#endif
}
/* Control register 6 configuration block.*/
{
cr[2] = devp->config->acccfg->outdatarate |
devp->config->acccfg->fullscale;
}
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 2);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
}
if((devp)->config->gyrocfg != NULL) {
cr[0] = LSM6DS0_AD_CTRL_REG1_G;
/* Control register 1 configuration block.*/
{
cr[1] = devp->config->gyrocfg->fullscale |
devp->config->gyrocfg->outdatarate;
#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
cr[1] |= devp->config->acccfg->decmode;
#endif
}
/* Control register 2 configuration block.*/
{
cr[2] = 0;
#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
cr[2] |= devp->config->gyrocfg->outsel;
#endif
}
/* Control register 3 configuration block.*/
{
cr[3] = 0;
#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
cr[3] |= devp->config->gyrocfg->hpfenable |
devp->config->gyrocfg->lowmodecfg |
devp->config->gyrocfg->hpcfg;
#endif
}
/* Control register 4 configuration block.*/
{
cr[4] = LSM6DS0_CTRL_REG4_XEN_G | LSM6DS0_CTRL_REG4_YEN_G |
LSM6DS0_CTRL_REG4_ZEN_G;
}
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 4);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
cr[0] = LSM6DS0_AD_CTRL_REG9;
/* Control register 9 configuration block.*/
{
cr[1] = 0;
}
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
}
/* Storing sensitivity information according to full scale value */
if((devp)->config->acccfg != NULL) {
if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_2G) {
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
if((devp)->config->acccfg->bias == NULL)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
else
devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i];
}
devp->accfullscale = LSM6DS0_ACC_2G;
}
else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_4G) {
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
if((devp)->config->acccfg->bias == NULL)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
else
devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i];
}
devp->accfullscale = LSM6DS0_ACC_4G;
}
else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_8G) {
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
if((devp)->config->acccfg->bias == NULL)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
else
devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i];
}
devp->accfullscale = LSM6DS0_ACC_8G;
}
else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_16G) {
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
if((devp)->config->acccfg->bias == NULL)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
else
devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i];
}
devp->accfullscale = LSM6DS0_ACC_16G;
}
else
osalDbgAssert(FALSE, "lsm6ds0Start(), accelerometer full scale issue");
}
if((devp)->config->gyrocfg != NULL) {
if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DPS) {
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
if((devp)->config->acccfg->bias == NULL)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
else
devp->gyrosensitivity[i] = devp->config->gyrocfg->sensitivity[i];
}
devp->gyrofullscale = LSM6DS0_GYRO_245DPS;
}
else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DPS) {
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
if((devp)->config->acccfg->bias == NULL)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
else
devp->gyrosensitivity[i] = devp->config->gyrocfg->sensitivity[i];
}
devp->gyrofullscale = LSM6DS0_GYRO_500DPS;
}
else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DPS) {
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
if((devp)->config->acccfg->bias == NULL)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
else
devp->gyrosensitivity[i] = devp->config->gyrocfg->sensitivity[i];
}
devp->gyrofullscale = LSM6DS0_GYRO_2000DPS;
}
else
osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");
}
/* This is the Gyroscope transient recovery time */
osalThreadSleepMilliseconds(5);
devp->state = LSM6DS0_READY;
}
/*
* Application entry point.
*/
int main(void) {
Thread *shelltp = NULL;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SERIAL_DRIVER, NULL);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Activates the EXT driver.
*/
extStart(&EXTD1, &extcfg);
/*
* Activates the I2C driver.
*/
i2cStart(&I2C_DRIVER, &i2c1cfg);
/*
* Activates the SPI driver.
*/
spiStart(&SPI_DRIVER, &spi1cfg);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
chThdSleepMilliseconds(200);
if (gyrotp == NULL)
gyrotp = gyroRun(&SPI_DRIVER, NORMALPRIO);
if (acctp == NULL)
acctp = accRun(&I2C_DRIVER, NORMALPRIO);
if (magtp == NULL)
magtp = magRun(&I2C_DRIVER, NORMALPRIO);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state.
*/
while (TRUE) {
if (!shelltp)
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO - 1);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp);
shelltp = NULL;
}
chThdSleepMilliseconds(200);
}
}
/******************************************************************************
*
* Description:
* Start a read
*
* Params:
* [in] devAddr - device address
* [in] address - eeprom address
*
* Returns:
* I2C_CODE_OK or I2C_CODE_ERROR
*
*****************************************************************************/
tS8 eepromStartRead(tU8 devAddr, tU16 address) {
tS8 retCode = 0;
tU8 status = 0;
/* Generate Start condition */
retCode = i2cStart();
/* Transmit address */
if (I2C_CODE_OK == retCode) {
/* Write SLA+W */
retCode = i2cPutChar(devAddr);
while (I2C_CODE_BUSY == retCode) {
retCode = i2cPutChar(devAddr);
}
}
if (I2C_CODE_OK == retCode) {
/* Wait until data transmitted */
while (1) {
/* Get new status */
status = i2cCheckStatus();
if ((0x18 == status) || (0x28 == status)) {
/* Data transmitted and ACK received */
/* Write data */
retCode = i2cPutChar((tU8) (address & 0xff));
while (I2C_CODE_BUSY == retCode) {
retCode = i2cPutChar((tU8) (address & 0xff));
}
break;
} else if (0xf8 != status) {
/* error */
retCode = I2C_CODE_ERROR;
break;
}
}
}
/* Wait until data transmitted */
while (1) {
/* Get new status */
status = i2cCheckStatus();
if (0x28 == status) {
/* Data transmitted and ACK received */
break;
} else if (0xf8 != status) {
/* error */
retCode = I2C_CODE_ERROR;
break;
}
}
/* Generate Start condition */
retCode = i2cRepeatStart();
/* Transmit device address */
if (I2C_CODE_OK == retCode) {
/* Write SLA+R */
retCode = i2cPutChar(devAddr + 0x01);
while (I2C_CODE_BUSY == retCode) {
retCode = i2cPutChar(devAddr + 0x01);
}
}
/* Wait until SLA+R transmitted */
while (1) {
/* Get new status */
status = i2cCheckStatus();
if (0x40 == status) {
/* Data transmitted and ACK received */
break;
} else if (0xf8 != status) {
/* error */
retCode = I2C_CODE_ERROR;
break;
}
}
// Do not generate a stop bit
return retCode;
}
static msg_t gyro_set_full_scale(void *ip, lsm6ds0_gyro_fs_t fs) {
float newfs, scale;
uint8_t i, cr[2];
msg_t msg;
if(fs == LSM6DS0_GYRO_FS_245DPS) {
newfs = LSM6DS0_GYRO_245DPS;
}
else if(fs == LSM6DS0_GYRO_FS_500DPS) {
newfs = LSM6DS0_GYRO_500DPS;
}
else if(fs == LSM6DS0_GYRO_FS_2000DPS) {
newfs = LSM6DS0_GYRO_2000DPS;
}
else {
return MSG_RESET;
}
if(newfs != ((LSM6DS0Driver *)ip)->gyrofullscale) {
scale = newfs / ((LSM6DS0Driver *)ip)->gyrofullscale;
((LSM6DS0Driver *)ip)->gyrofullscale = newfs;
#if LSM6DS0_SHARED_I2C
i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
/* Updating register.*/
msg = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
LSM6DS0_AD_CTRL_REG1_G, &cr[1], 1);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
if(msg != MSG_OK)
return msg;
cr[0] = LSM6DS0_AD_CTRL_REG1_G;
cr[1] &= ~(LSM6DS0_CTRL_REG1_G_FS_MASK);
cr[1] |= fs;
#if LSM6DS0_SHARED_I2C
i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
msg = lsm6ds0I2CWriteRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
cr, 1);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
if(msg != MSG_OK)
return msg;
/* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
((LSM6DS0Driver *)ip)->gyrosensitivity[i] *= scale;
((LSM6DS0Driver *)ip)->gyrobias[i] *= scale;
}
}
return msg;
}
/******************************************************************************
*
* Description:
* Communicate with PCA9532
*
* Params:
* [in] pBuf - buffer of bytes to write
* [in] len - length of pBuf
* [in] pBuf2 - buffer for received data
* [in] len - length of pBuf2
*
* Returns:
* I2C status code
*
*****************************************************************************/
tS8 pca9532(tU8* pBuf, tU16 len, tU8* pBuf2, tU16 len2) {
tS8 retCode = 0;
tU8 status = 0;
tU16 i = 0;
do {
/* generate Start condition */
retCode = i2cStart();
if (I2C_CODE_OK != retCode) {
break;
}
/* write pca9532 address */
retCode = i2cWriteWithWait(0xc0);
if (I2C_CODE_OK != retCode) {
break;
}
/* write data */
for (i = 0; i < len; i++) {
retCode = i2cWriteWithWait(*pBuf);
if (I2C_CODE_OK != retCode) {
break;
}
pBuf++;
}
} while (0);
if (len2 > 0) {
/* Generate Start condition */
retCode = i2cRepeatStart();
/* Transmit device address */
if (I2C_CODE_OK == retCode) {
/* Write SLA+R */
retCode = i2cPutChar(0xc0 + 0x01);
while (I2C_CODE_BUSY == retCode) {
retCode = i2cPutChar(0xc0 + 0x01);
}
}
/* Wait until SLA+R transmitted */
while (1) {
/* Get new status */
status = i2cCheckStatus();
if (0x40 == status) {
/* Data transmitted and ACK received */
break;
} else if (0xf8 != status) {
/* error */
retCode = I2C_CODE_ERROR;
break;
}
}
if (I2C_CODE_OK == retCode) {
/* wait until address transmitted and receive data */
for (i = 1; i <= len2; i++) {
/* wait until data transmitted */
while (1) {
/* Get new status */
status = i2cCheckStatus();
if ((0x40 == status) || (0x48 == status) || (0x50 == status)) {
/* Data received */
if (i == len2) {
/* Set generate NACK */
retCode = i2cGetChar(I2C_MODE_ACK1, pBuf2);
} else {
retCode = i2cGetChar(I2C_MODE_ACK0, pBuf2);
}
/* Read data */
retCode = i2cGetChar(I2C_MODE_READ, pBuf2);
while (I2C_CODE_EMPTY == retCode) {
retCode = i2cGetChar(I2C_MODE_READ, pBuf2);
}
pBuf2++;
break;
} else if (0xf8 != status) {
/* ERROR */
i = len2;
retCode = I2C_CODE_ERROR;
break;
}
}
}
}
}
/* Generate Stop condition */
i2cStop();
return retCode;
}
//------------------------------------------------------------------------------
// An adaptation of a function of the same name in the original Babuino code.
//------------------------------------------------------------------------------
void
Babuino::code_exec()
{
//Serial.println("code_exec()");
if (!_storage.getReadyToRead())
{
// TO DO: What now? How do I report an error?
}
while (_states.getRunRequest() == RUNNING)
{
_storage.readByte(_regs.pc, _regs.opCode);
_regs.pc++;
switch (_regs.opCode)
{
case OP_CONFIG:
withConfig();
break;
case OP_MATH:
withMath();
break;
case OP_BYTE:
case OP_INT8:
case OP_SPAN:
{
//Serial.println("int8 ");
uint8_t value;
_regs.pc = _storage.readByte(_regs.pc, value);
pushUint8(_stack, value);
}
break;
case OP_SHORT:
case OP_UINT16:
{
//Serial.print("int16: ");
uint16_t value;
_regs.pc = _storage.read<uint16_t>(_regs.pc, value);
//Serial.println(value);
pushUint16(_stack, value);
}
break;
case OP_GLOBAL:
{
//Serial.print("global: ");
STACKPTR value;
_regs.pc = _storage.read<STACKPTR>(_regs.pc, value);
//Serial.println(value);
pushStackPtr(_stack, value); //_stack.push(_stack.getBottom() - value);
}
break;
case OP_INT32:
case OP_UINT32:
{
//Serial.print("int32 ");
uint32_t value;
_regs.pc = _storage.read<uint32_t>(_regs.pc, value);
#ifdef SUPPORT_32BIT
pushUint32(_stack, value);
#else
pushUint16(_stack, (value >> 16) & 0xFFFF); // High 16 bits
pushUint16(_stack, value & 0xFFFF); // Low 16 bits
#endif
}
break;
#ifdef SUPPORT_FLOAT
case OP_FLOAT:
{
//Serial.print("float ");
float value;
_regs.pc = _storage.read<float>(_regs.pc, value);
pushFloat(_stack, value);
}
break;
#endif
#ifdef SUPPORT_DOUBLE
case OP_DOUBLE:
{
//Serial.print("double ");
double value;
_regs.pc = _storage.read<double>(_regs.pc, value);
pushDouble(_stack, value);
}
break;
#endif
case OP_BOOL:
{
//Serial.print("bool ");
bool value;
_regs.pc = _storage.read<bool>(_regs.pc, value);
pushUint8(_stack, (uint8_t)value);
}
break;
case OP_CPTR:
{
//Serial.print("cptr ");
PROGPTR value;
_regs.pc = _storage.read<PROGPTR>(_regs.pc, value);
pushProgPtr(_stack, value);
}
break;
#ifdef SUPPORT_STRING
case OP_STRING:
{
//Serial.print("string: \"");
// ***KLUDGE WARNING***
// Borrowing unused (hopefully) stack space as a buffer!
// (To avoid having to allocate another buffer.
uint8_t* psz = (uint8_t*)getTopAddress(_stack);
uint8_t nextChar;
int16_t i = -1;
do
{
i++;
_regs.pc = _storage.readByte(_regs.pc, nextChar);
psz[i] = nextChar;
}
while (nextChar);
//Serial.print((char *)psz);
//Serial.print("\" ");
// Logically this is wrong because I'm apparently
// pushing a string to a location that it already
// occupies. However, the stack implementation
// pushes strings onto a separate stack, then
// pushes the location onto the main stack. So
// the string doesn't get copied over itself, and
// is already copied before the first characters are
// overwritten by pushing the said location onto the
// main stack.
//STACKSTATE state;
//getStackState(_stack, &state);
//Serial.print("[("); Serial.print(state.top); Serial.print(","); Serial.print(state.stringTop);Serial.print(") -> (");
pushString(_stack, psz);
//getStackState(_stack, &state);
//Serial.print(state.top); Serial.print(","); Serial.print(state.stringTop);Serial.println(")]");
}
break;
case OP_ASCII:
{
uint8_t* psz;
topString(_stack, &psz);
uint8_t ascii = psz[0];
popString(_stack);
pushUint8(_stack, ascii);
}
break;
case OP_STRLEN:
{
uint8_t* psz;
topString(_stack, &psz);
uint8_t len = strlen((char*)psz);
popString(_stack);
pushUint8(_stack, len);
}
break;
#endif
case OP_BEEP:
//Serial.println("---beep---");
beep();
break;
case OP_LEDON:
//Serial.println("---LED on---");
userLed(true); // set the correct bit
break;
case OP_LEDOFF:
//Serial.println("---LED off---");
userLed(false);
break;
case OP_WITHINT8:
case OP_WITHUINT8:
case OP_WITHINT16:
case OP_WITHUINT16:
case OP_WITHBOOL:
case OP_WITHPTR:
#ifdef SUPPORT_32BIT
case OP_WITHINT32:
case OP_WITHUINT32:
#endif
#ifdef SUPPORT_FLOAT
case OP_WITHFLOAT:
#endif
#ifdef SUPPORT_DOUBLE
case OP_WITHDOUBLE:
#endif
#ifdef SUPPORT_STRING
case OP_WITHSTRING:
#endif
_regs.withCode = _regs.opCode;
break;
case OP_LOCAL:
{
//Serial.print("local: local frame (");
//Serial.print(_regs.localFrame);
//Serial.print(") - ");
STACKPTR varOffset;
_regs.pc = _storage.read<uint16_t>(_regs.pc, (uint16_t&)varOffset);
pushStackPtr(_stack, (STACKPTR)_regs.localFrame.top + varOffset);
//Serial.print(varOffset);
//Serial.print(" = global ");
//Serial.println(_regs.localFrame + varOffset);
}
break;
case OP_PARAM:
{
//Serial.print("param: ");
STACKPTR varOffset;
_regs.pc = _storage.read<uint16_t>(_regs.pc, (uint16_t&)varOffset);
// We have an index into the parameters, which were put on
// the stack in reverse order before the function call.
// Calculate the absolute stack offset using the local
// stack frame offset.
// Also, the total size of the arguments was pushed last,
// so we need to step past that too.
// TODO: Check against the total argument size.
pushStackPtr(_stack,
getArgsLocation()
- sizeof(uint8_t) // Number of args
- varOffset // Offset to the required param
);
}
break;
case OP_BLOCK:
{
//Serial.print("block ");
descendBlock(); // Shift the flag to the next block bit
//char psz[10];
//utoa(_regs.blockDepthMask, psz, 2);
//Serial.println(psz);
uint16_t blockLength;
// Push address of next instruction (following the block
// length data)
pushProgPtr(_stack, (PROGPTR)_regs.pc + sizeof(uint16_t));
_storage.read<uint16_t>(_regs.pc, blockLength);
// Step past the block (tests there will bring execution back)
_regs.pc.increment(blockLength);
}
break;
case OP_EOB:
//Serial.println("--eob--");
setBlockExecuted(); // Set the bit to indicate that this block has executed
break;
case OP_DO:
{
//Serial.println("---do---");
// OP_DO is like OP_BLOCK except that execution falls through to
// the top of the block of code unconditionally rather than jump
// to the end where some condition is tested.
// Need to:
// - Push the address that the following OP_BLOCK would push
// - Step past the:
// - The OP_BLOCK code (uint8_t)
// - The block size (uint16_t)
// After going through the code block it should jump back to
// the beginning of the OP_BLOCK and loop as usual.
descendBlock(); // Shift the flag to the next block bit (normally done by OP_BLOCK)
PROGPTR startOfBlock = (PROGPTR)_regs.pc + sizeof(uint8_t) +sizeof(uint16_t);
pushProgPtr(_stack, startOfBlock);
_regs.pc.set(startOfBlock);
}
break;
case OP_WHILE:
{
//Serial.print("while ");
bool condition;
PROGPTR blockAddr;
popUint8(_stack, (uint8_t*)&condition);
//Serial.println(condition ? "true" : "false");
//_stack.pop(blockAddr);
if (condition) // if true then go to the block start address
{
topProgPtr(_stack, &blockAddr);
_regs.pc = blockAddr;
}
else
{
popProgPtr(_stack, &blockAddr); // Throw it away
ascendBlock(); // Finished with this block now
}
}
break;
case OP_REPEAT:
{
//Serial.print("repeat ");
PROGPTR blockAddr;
uint16_t max;
popProgPtr(_stack, &blockAddr);
popUint16(_stack, &max);
uint16_t repcount;
if (!hasBlockExecuted()) // First time around?
{
repcount = 1;
pushUint16(_stack, repcount);
// point to the counter we just pushed
STACKPTR slot = getTop(_stack);
// Save outer loop's repcount pointer
pushStackPtr(_stack, _regs.repcountLocation);
// Set it to ours
_regs.repcountLocation = slot;
}
else
{
getUint16(_stack, _regs.repcountLocation, &repcount); // Get counter value
repcount++;
}
//Serial.println(repcount);
if (repcount <= max)
{
setUint16(_stack, _regs.repcountLocation, repcount);
pushUint16(_stack, max);
pushProgPtr(_stack, blockAddr);
_regs.pc = blockAddr;
}
else
{
// Restore the outer loop's repcount pointer
popStackPtr(_stack, &_regs.repcountLocation);
popUint16(_stack, &repcount); // Dispose of counter
ascendBlock(); // Finished with this block now
}
}
break;
case OP_REPCOUNT:
{
uint16_t repcount;
getUint16(_stack, _regs.repcountLocation, &repcount);
pushUint16(_stack, repcount);
}
break;
case OP_FOR:
{
//Serial.println("for ");
// The counter variable has already been set to the from
// value, so the from value isn't on the stack.
PROGPTR blockAddr;
int16_t step, to, from;
STACKPTR counterOff;
int16_t counterVal;
popProgPtr(_stack, &blockAddr);
popUint16(_stack, (uint16_t*)&step);
popUint16(_stack, (uint16_t*)&to);
popUint16(_stack, (uint16_t*)&from);
popStackPtr(_stack, &counterOff);
getUint16(_stack, counterOff, (uint16_t*)&counterVal);
//Serial.print(counterVal);
//Serial.print(" ");
//Serial.print(to);
//Serial.print(" ");
//Serial.println(step);
bool keepGoing;
// See if this is the first time around
if (!hasBlockExecuted())
{
counterVal = from;
keepGoing = true;
}
else
{
// If step > 0 then assume from < to otherwise assume from > to
keepGoing = (step > 0) ? (counterVal < to) : (counterVal > to);
counterVal += step;
}
if (keepGoing)
{
setUint16(_stack, counterOff, (uint16_t)counterVal);
_regs.pc = blockAddr; // reiterate
pushStackPtr(_stack, counterOff); // Var offset
pushUint16(_stack, (uint16_t)from); // to
pushUint16(_stack, (uint16_t)to); // to
pushUint16(_stack, (uint16_t)step); // step
pushProgPtr(_stack, blockAddr);
}
else
{
ascendBlock();
}
}
break;
case OP_IF:
{
//Serial.print("if ");
// If it's the first time through then check the
// condition
if (!hasBlockExecuted())
{
PROGPTR blockAddr;
bool condition;
popProgPtr(_stack, &blockAddr); // Block initial address
popUint8(_stack, (uint8_t*)&condition); // argument to test
//Serial.println(condition ? "true" : "false");
if (condition)
{
_regs.pc = blockAddr;
}
else
{
ascendBlock();
}
}
else
{
ascendBlock();
}
}
break;
// IFELSE starts with address of THEN and ELSE lists (and
// CONDITIONAL) on the stack. CONDITIONAL is tested and
// appropriate list is run.
case OP_IFELSE:
{
//Serial.print("ifelse ");
PROGPTR elseBlock;
popProgPtr(_stack, &elseBlock); // ELSE block start
// Note that descendBlock() will have been called twice
// the first time around (once for each block).
ascendBlock(); // Remove the else block...
// ...and use the then block flag for both purposes
if (!hasBlockExecuted())
{
PROGPTR thenBlock;
bool condition;
popProgPtr(_stack, &thenBlock); // THEN block start
popUint8(_stack, (uint8_t*)&condition); // argument to test
if (condition)
{
//Serial.println("(then)");
_regs.pc = thenBlock;
// The ELSE address will get pushed again when
// execution falls into the ELSE block after
// exiting the THEN block.
// Another else block will be descended into
// as well, and ascended away again above.
// The eob code will be encountered at the end
// of the then block, and that will set the
// executed flag.
}
else
{
//Serial.println("(else)");
_regs.pc = elseBlock; // the "ELSE" list
pushProgPtr(_stack, (PROGPTR)0); // Push fake ELSE to balance
// Borrow the then block flag and set it now as
// executed since it won't actually be set
// otherwise.
setBlockExecuted();
// Descend the else block now, as
// this also won't be done in the block's code.
descendBlock();
}
}
else
{
//popProgPtr(_stack, &elseBlock); // dispose of unrequired address
ascendBlock(); // ie. the then block
}
}
break;
case OP_PUSH:
{
//Serial.print("push ");
uint8_t amount;
popUint8(_stack, &amount);
pushn(_stack, (STACKPTR)amount);
}
break;
case OP_POP:
{
//Serial.print("pop ");
uint8_t amount;
popUint8(_stack, &amount);
popn(_stack, (STACKPTR)amount);
}
break;
case OP_CHKPOINT:
getStackState(_stack, &_regs.checkPoint);
break;
case OP_ROLLBACK:
setStackState(_stack, &_regs.checkPoint);
break;
case OP_CALL:
{
//Serial.println("call");
PROGPTR procAddr;
popProgPtr(_stack, &procAddr); // Get the function location
// Save the args location used by the calling function, and
// set it to what was the stack top before it was pushed.
/*
_regs.argsLoc = _stack.push(_regs.argsLoc);
//Serial.print("args location: ");
//Serial.println(_regs.argsLoc);
*/
pushProgPtr(_stack, (PROGPTR)_regs.pc); // Save the current code location for the return
_regs.pc.set(procAddr); // Now jump to the function
}
break;
case OP_BEGIN:
//Serial.println("begin");
pushRegisters(); // Save state of the caller
_regs.blockDepthMask = 0;
_regs.blocksExecuted = 0;
getStackState(_stack, &_regs.localFrame); // = getTop(_stack);
//Serial.println(_regs.localFrame);
break;
case OP_RETURN:
{
//Serial.print("return ");
//Unwind the stack to the beginning of the local frame
setStackState(_stack, &_regs.localFrame);
popRegisters();
PROGPTR returnAddr;
popProgPtr(_stack, &returnAddr); // Get the return address
//_stack.pop(_regs.argsLoc); // Restore the param location for the calling function
_regs.pc.set(returnAddr);
}
break;
case OP_EXIT:
reset();
//Serial.println("---exit---");
break;
case OP_LOOP:
{
//Serial.println("---loop---");
PROGPTR blockAddr;
topProgPtr(_stack, &blockAddr);
_regs.pc.set(blockAddr);
}
break;
case OP_WAIT:
{
//Serial.println("---wait---");
uint16_t tenths;
popUint16(_stack, &tenths);
delay(100 * tenths);
}
break;
case OP_TIMER:
//Serial.print("timer ");
pushUint16(_stack, _timerCount); // TODO: implement timer!!
break;
case OP_RESETT:
//Serial.println("---resett---");
_timerCount = 0;
break;
case OP_RANDOM:
//Serial.print("random ");
pushUint16(_stack, (uint16_t)random(0, 32767));
break;
case OP_RANDOMXY:
{
//Serial.print("randomxy ");
int16_t x;
int16_t y;
popUint16(_stack, (uint16_t*)&y);
popUint16(_stack, (uint16_t*)&x);
pushUint16(_stack, (uint16_t)random(x, y));
}
break;
case OP_MOTORS:
{
//Serial.print("motors ");
uint8_t selected;
popUint8(_stack, &selected);
_selectedMotors = (Motors::Selected)selected;
}
break;
case OP_THISWAY:
//Serial.print("---thisway---");
_motors.setDirection(_selectedMotors, MotorBase::THIS_WAY);
break;
case OP_THATWAY:
//Serial.print("---thatway---");
_motors.setDirection(_selectedMotors, MotorBase::THAT_WAY);
break;
case OP_RD:
//Serial.print("---rd---");
_motors.reverseDirection(_selectedMotors);
break;
case OP_SETPOWER:
{
//Serial.print("setpower ");
uint8_t power;
popUint8(_stack, &power);
if (power > 7)
power = 7;
_motors.setPower(_selectedMotors, power);
}
break;
case OP_BRAKE:
//Serial.println("---brake---");
_motors.setBrake(_selectedMotors, MotorBase::BRAKE_ON);
break;
case OP_ON:
//Serial.println("---on---");
_motors.on(_selectedMotors);
break;
case OP_ONFOR:
{
//Serial.print("onfor");
uint16_t tenths;
popUint16(_stack, &tenths);
_motors.on(_selectedMotors);
delay(100 * tenths);
_motors.off(_selectedMotors);
}
break;
case OP_OFF:
//Serial.println("---off---");
_motors.off(_selectedMotors);
break;
case OP_SENSOR1:
case OP_SENSOR2:
case OP_SENSOR3:
case OP_SENSOR4:
case OP_SENSOR5:
case OP_SENSOR6:
case OP_SENSOR7:
case OP_SENSOR8:
//Serial.print("sensor ");
//Serial.print(_regs.opCode - OP_SENSOR1);
//Serial.print(" ");
pushUint16(_stack, (uint16_t)readAnalog(_regs.opCode - OP_SENSOR1));
break;
case OP_AIN:
{
//Serial.print("ain ");
uint8_t input;
popUint8(_stack, &input);
pushUint16(_stack, (uint16_t)readAnalog(input));
}
break;
case OP_AOUT:
{
//Serial.print("aout ");
uint8_t output;
uint8_t value;
popUint8(_stack, &output);
popUint8(_stack, &value);
writeAnalog(output, value);
}
break;
case OP_SWITCH1:
case OP_SWITCH2:
case OP_SWITCH3:
case OP_SWITCH4:
case OP_SWITCH5:
case OP_SWITCH6:
case OP_SWITCH7:
case OP_SWITCH8:
{
//Serial.print("switch ");
//Serial.print(_regs.opCode - OP_SWITCH1);
//Serial.print(" ");
int16_t val = readAnalog(_regs.opCode - OP_SWITCH1);
if (val < 0)
pushUint8(_stack, (uint8_t)false);
else
pushUint8(_stack, (uint8_t)!!(val >> 7));
}
break;
case OP_NOT:
break;
case OP_AND:
break;
case OP_OR:
break;
case OP_XOR:
break;
case OP_DIN:
{
//Serial.print("din ");
uint8_t input;
popUint8(_stack, &input);
pushUint8(_stack, (uint8_t)readDigital(input));
}
break;
case OP_DOUT:
{
//Serial.print("dout ");
uint8_t output;
bool value;
popUint8(_stack, &output);
popUint8(_stack, (uint8_t*)&value);
writeDigital(output, value);
}
break;
case OP_BTOS:
{
int8_t value;
popUint8(_stack, (uint8_t*)&value);
pushUint16(_stack,(uint16_t)value);
}
break;
case OP_UBTOS:
{
uint8_t value;
popUint8(_stack, &value);
pushUint16(_stack,(uint16_t)value);
}
break;
case OP_STOB: // and OP_USTOB
{
//Serial.print("stob: ");
uint16_t value;
popUint16(_stack, (uint16_t*)&value);
//Serial.println(value);
pushUint8(_stack, (uint8_t)value);
}
break;
#ifdef SUPPORT_32BIT
case OP_BTOI:
{
int8_t value;
popUint8(_stack, (uint8_t*)&value);
pushUint32(_stack,(uint32_t)value);
}
break;
case OP_UBTOI:
{
uint8_t value;
popUint8(_stack, &value);
pushUint32(_stack,(uint32_t)value);
}
break;
case OP_STOI:
{
int16_t value;
popUint16(_stack, (uint16_t*)&value);
pushUint32(_stack,(uint32_t)value);
}
break;
case OP_USTOI:
{
uint16_t value;
popUint16(_stack, &value);
pushUint32(_stack,(uint32_t)value);
}
break;
case OP_ITOB: // and OP_UITOB
{
int32_t value;
popUint32(_stack, (uint32_t*)&value);
pushUint8(_stack, (uint8_t)value);
}
break;
case OP_ITOS: //and OP_UITOS
{
int32_t value;
popUint32(_stack, (uint32_t*)&value);
pushUint16(_stack, (uint16_t)value);
}
break;
#endif
#ifdef SUPPORT_FLOAT
case OP_BTOF:
{
int8_t value;
popUint8(_stack, (uint8_t*)&value);
pushFloat(_stack, (float)value);
}
break;
case OP_UBTOF:
{
uint8_t value;
popUint8(_stack, &value);
pushFloat(_stack, (float)value);
}
break;
case OP_STOF:
{
int16_t value;
popUint16(_stack, (uint16_t*)&value);
pushFloat(_stack, (float)value);
}
break;
case OP_USTOF:
{
uint16_t value;
popUint16(_stack, &value);
pushFloat(_stack, (float)value);
}
break;
case OP_FTOB:
{
float value;
popFloat(_stack, &value);
pushUint8(_stack, (uint8_t)value);
}
break;
case OP_FTOS:
{
float value;
popFloat(_stack, &value);
pushUint16(_stack, (uint16_t)value);
}
break;
#endif
#ifdef SUPPORT_DOUBLE
case OP_BTOD:
{
int8_t value;
popUint8(_stack, (uint8_t*)&value);
pushDouble(_stack, (double)value);
}
break;
case OP_UBTOD:
{
uint8_t value;
popUint8(_stack, &value);
pushDouble(_stack, (double)value);
}
break;
case OP_STOD:
{
int16_t value;
popUint16(_stack, (uint16_t*)&value);
pushDouble(_stack, (double)value);
}
break;
case OP_USTOD:
{
uint16_t value;
popUint8(_stack, (uint8_t*)&value);
pushDouble(_stack, (double)value);
}
break;
case OP_DTOB:
{
double value;
popDouble(_stack, &value);
pushUint8(_stack, (uint8_t)value);
}
break;
case OP_DTOS:
{
double value;
popDouble(_stack, &value);
pushUint16(_stack, (uint16_t)value);
}
break;
#endif
#if defined(SUPPORT_DOUBLE) && defined(SUPPORT_32BIT)
case OP_ITOD:
{
int32_t value;
popUint32(_stack, (uint32_t*)&value);
pushDouble(_stack, (double)value);
}
break;
case OP_UITOD:
{
uint32_t value;
popUint32(_stack, &value);
pushDouble(_stack, (double)value);
}
break;
case OP_DTOI:
{
double value;
popDouble(_stack, &value);
pushUint32(_stack, (uint32_t)value);
}
break;
#endif
#if defined(SUPPORT_DOUBLE) && defined(SUPPORT_FLOAT)
case OP_FTOD:
{
float value;
popFloat(_stack, &value);
pushDouble(_stack, (double)value);
}
break;
case OP_DTOF:
{
double value;
popDouble(_stack, &value);
pushFloat(_stack, (float)value);
}
break;
#endif
#if defined(SUPPORT_FLOAT) && defined(SUPPORT_32BIT)
case OP_ITOF:
{
int32_t value;
popUint32(_stack, (uint32_t*)&value);
pushFloat(_stack, (float)value);
}
break;
case OP_UITOF:
{
uint32_t value;
popUint32(_stack, &value);
pushFloat(_stack, (float)value);
}
break;
case OP_FTOI:
{
float value;
popFloat(_stack, &value);
pushUint32(_stack, (uint32_t)value);
}
break;
#endif
case OP_I2CSTART:
i2cStart();
break;
case OP_I2CSTOP:
i2cStop();
break;
case OP_I2CTXRX:
{
uint16_t i2cAddr;
uint16_t txBuffOffset;
uint8_t txBuffLen;
uint16_t rxBuffOffset;
uint8_t rxBuffLen;
uint16_t timeout;
popUint16(_stack, &i2cAddr);
popUint16(_stack, &txBuffOffset);
popUint8(_stack, &txBuffLen);
popUint16(_stack, &rxBuffOffset);
popUint8(_stack, &rxBuffLen);
popUint16(_stack, &timeout);
i2cTxRx(i2cAddr,
(uint8_t*)getStackAddress(_stack, txBuffOffset),
txBuffLen,
(uint8_t*)getStackAddress(_stack, rxBuffOffset),
rxBuffLen,
timeout);
}
break;
case OP_I2CRX:
{
uint16_t addr;
uint16_t rxBuffOffset;
uint8_t rxBuffLen;
uint16_t timeout;
popUint16(_stack, &addr);
popUint16(_stack, &rxBuffOffset);
popUint8(_stack, &rxBuffLen);
popUint16(_stack, &timeout);
i2cRx(addr, (uint8_t*)getStackAddress(_stack, rxBuffOffset), rxBuffLen, timeout);
}
break;
#ifdef SUPPORT_32BIT
case OP_I2CERR:
{
//Serial.println("---i2cerr---");
uint32_t errors = i2cErrors();
pushUint32(_stack, errors);
}
break;
#endif
case OP_WAITUNTIL:
case OP_RECORD:
case OP_RECALL:
case OP_RESETDP:
case OP_SETDP:
case OP_ERASE:
case OP_SETSVH:
case OP_SVR:
case OP_SVL:
// TODO!!!
break;
default:
// All of the type specific codes are dealt with here
if (!withCurrentType())
{
//beep(); // Just an indication for now.
Serial.print("unrecognised opcode: ");
Serial.println(_regs.opCode);
}
break;
}
}
}
/******************************************************************************
*
* Description:
* Read a specified number of bytes from the I2C network.
*
* Note: After this function is run, you may need a bus free time before a
* new data transfer can be initiated.
*
* Params:
* [in] addr - address
* [in] pBuf - receive buffer
* [in] len - number of bytes to receive
*
* Returns:
* I2C_CODE_OK or I2C status code
*
*****************************************************************************/
tS8 i2cRead(tU8 addr, tU8* pBuf, tU16 len) {
tS8 retCode = 0;
tU8 status = 0;
tU8 i = 0;
/* Generate Start condition */
retCode = i2cStart();
/* Transmit address */
if (retCode == I2C_CODE_OK) {
/* write SLA+R */
retCode = i2cPutChar(addr);
while (retCode == I2C_CODE_BUSY) {
retCode = i2cPutChar(addr);
}
}
if (retCode == I2C_CODE_OK) {
/* wait until address transmitted and receive data */
for (i = 1; i <= len; i++) {
/* wait until data transmitted */
while (1) {
/* get new status */
status = i2cCheckStatus();
/*
* SLA+R transmitted, ACK received or
* SLA+R transmitted, ACK not received
* data byte received in master mode, ACK transmitted
*/
if ((status == 0x40) || (status == 0x48) || (status == 0x50)) {
/* data received */
if (i == len) {
/* Set generate NACK */
retCode = i2cGetChar(I2C_MODE_ACK1, pBuf);
} else {
retCode = i2cGetChar(I2C_MODE_ACK0, pBuf);
}
/* Read data */
retCode = i2cGetChar(I2C_MODE_READ, pBuf);
while (retCode == I2C_CODE_EMPTY) {
retCode = i2cGetChar(I2C_MODE_READ, pBuf);
}
pBuf++;
break;
}
/* no relevant status information */
else if (status != 0xf8) {
/* error */
i = len;
retCode = I2C_CODE_ERROR;
break;
}
}
}
}
/*--- Generate Stop condition ---*/
i2cStop();
return retCode;
}
/******************************************************************************
*
* Description:
* Sends data on the I2C network
*
* Note: After this function is run, you may need a bus free time before a
* new data transfer can be initiated.
*
* Params:
* [in] addr - address
* [in] pData - data to transmit
* [in] len - number of bytes to transmit
*
* Returns:
* I2C_CODE_OK - successful
* I2C_CODE_ERROR - an error occured
*
*****************************************************************************/
tS8 i2cWrite(tU8 addr, tU8* pData, tU16 len) {
tS8 retCode = 0;
tU8 status = 0;
tU8 i = 0;
/* generate Start condition */
retCode = i2cStart();
/* Transmit address */
if (retCode == I2C_CODE_OK) {
/* write SLA+W */
retCode = i2cPutChar(addr);
while (retCode == I2C_CODE_BUSY) {
retCode = i2cPutChar(addr);
}
}
if (retCode == I2C_CODE_OK) {
/* wait until address transmitted and transmit data */
for (i = 0; i < len; i++) {
/* wait until data transmitted */
while (1) {
/* get new status */
status = i2cCheckStatus();
/*
* SLA+W transmitted, ACK received or
* data byte transmitted, ACK received
*/
if ((status == 0x18) || (status == 0x28)) {
/* Data transmitted and ACK received */
/* write data */
retCode = i2cPutChar(*pData);
while (retCode == I2C_CODE_BUSY) {
retCode = i2cPutChar(*pData);
}
pData++;
break;
}
/* no relevant status information */
else if (status != 0xf8) {
/* error */
i = len;
retCode = I2C_CODE_ERROR;
break;
}
}
}
}
/* wait until data transmitted */
while (1) {
/* get new status */
status = i2cCheckStatus();
/*
* SLA+W transmitted, ACK received or
* data byte transmitted, ACK received
*/
if ((status == 0x18) || (status == 0x28)) {
/* data transmitted and ACK received */
break;
}
/* no relevant status information */
else if (status != 0xf8) {
/* error */
i = len;
retCode = I2C_CODE_ERROR;
break;
}
}
/* generate Stop condition */
i2cStop();
return retCode;
}
static int reset_init_mpu(void) {
msg_t res = MSG_OK;
palSetPadMode(SCL_GPIO, SCL_PAD,
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1);
for(int i = 0;i < 16;i++) {
palClearPad(SCL_GPIO, SCL_PAD);
delay_short();
palSetPad(SCL_GPIO, SCL_PAD);
delay_short();
}
palSetPadMode(SCL_GPIO, SCL_PAD,
PAL_MODE_ALTERNATE(GPIO_AF_I2C1) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1);
I2C_DEV.state = I2C_STOP;
i2cStart(&I2C_DEV, &i2cfg);
chThdSleepMicroseconds(1000);
// Set clock source to gyro x
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_PWR_MGMT_1;
tx_buf[1] = 0x01;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
// Try the other address
if (res != MSG_OK) {
if (mpu_addr == MPU_ADDR1) {
mpu_addr = MPU_ADDR2;
} else {
mpu_addr = MPU_ADDR1;
}
// Set clock source to gyro x
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_PWR_MGMT_1;
tx_buf[1] = 0x01;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
}
// Set accelerometer full-scale range to +/- 16g
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_ACCEL_CONFIG;
tx_buf[1] = MPU9150_ACCEL_FS_16 << MPU9150_ACONFIG_AFS_SEL_BIT;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
// Set gyroscope full-scale range to +/- 2000 deg/s
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_GYRO_CONFIG;
tx_buf[1] = MPU9150_GYRO_FS_2000 << MPU9150_GCONFIG_FS_SEL_BIT;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
// Set low pass filter to 256Hz (1ms delay)
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_CONFIG;
tx_buf[1] = MPU9150_DLPF_BW_256;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
#if USE_MAGNETOMETER
// Set the i2c bypass enable pin to true to access the magnetometer
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_INT_PIN_CFG;
tx_buf[1] = 0x02;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
#endif
is_mpu9250 = read_single_reg(MPU9150_WHO_AM_I) == 0x71;
return 1;
}
