/* Checks the ID of the Magno to ensure we're actually talking to the Magno and not some other component. */
static bool_t hmc58831_ID_check(void) {
uint8_t buf;
uint8_t id_reg;
bool_t success = TRUE;
/* Forcefully try to read each ID register since it seems like they don't exist. */
id_reg = HMC5883L_RA_ID_A;
if(i2cMasterTransmitTimeout(&I2CD2, HMC5883L_I2C_ADDR, &id_reg, 1, &buf, 1, 1000) == RDY_OK) {
success &= (buf == 0x48);
} else {
return FALSE;
}
id_reg = HMC5883L_RA_ID_B;
if(i2cMasterTransmitTimeout(&I2CD2, HMC5883L_I2C_ADDR, &id_reg, 1, &buf, 1, 1000) == RDY_OK) {
success &= (buf == 0x34);
} else {
return FALSE;
}
id_reg = HMC5883L_RA_ID_C;
if(i2cMasterTransmitTimeout(&I2CD2, HMC5883L_I2C_ADDR, &id_reg, 1, &buf, 1, 1000) == RDY_OK) {
success &= (buf == 0x33);
} else {
return FALSE;
}
return success;
}
static int get_raw_mag(int16_t* mag) {
msg_t res = MSG_OK;
tx_buf[0] = MPU9150_HXL;
i2cAcquireBus(&I2C_DEV);
res = i2cMasterTransmitTimeout(&I2C_DEV, 0x0C, tx_buf, 1, rx_buf, 6, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
for (int i = 0; i < 3; i++) {
mag[i] = ((int16_t) ((uint16_t) rx_buf[2 * i + 1] << 8) + rx_buf[2 * i]);
}
// Start the measurement for the next iteration
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_CNTL;
tx_buf[1] = 0x01;
res = i2cMasterTransmitTimeout(&I2C_DEV, 0x0C, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
return 1;
}
static msg_t Thread2(void *arg) {
(void)arg;
uint8_t data[2], count = 0;
msg_t status = RDY_OK;
chRegSetThreadName("counter");
// First set the direction registers
data[0] = 0x00; // direction reg
data[1] = 0x00; // all outputs
i2cAcquireBus(&I2C_DRIVER);
status = i2cMasterTransmitTimeout(&I2C_DRIVER, I2C_CNT_ADDR, data, 2, NULL, 0, 1000);
i2cReleaseBus(&I2C_DRIVER);
data[0] = 0x14; // output reg
while (TRUE) {
data[1] = count;
i2cAcquireBus(&I2C_DRIVER);
status = i2cMasterTransmitTimeout(&I2C_DRIVER, I2C_CNT_ADDR, data, 2, NULL, 0, 1000);
i2cReleaseBus(&I2C_DRIVER);
chThdSleepMilliseconds(100);
count++;
}
// keep compiler happy
return status;
}
static THD_FUNCTION(PollEepromThread, arg) {
unsigned i;
uint8_t tx_data[5];
uint8_t rx_data[4];
msg_t status;
(void)arg;
chRegSetThreadName("PollEeprom");
/* set initial data to write */
tx_data[0] = EEPROM_START_ADDR;
tx_data[1] = 0xA0;
tx_data[2] = 0xA1;
tx_data[3] = 0xA2;
tx_data[4] = 0xA3;
while (true) {
/* write out initial data */
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, I2C_ADDR, tx_data, sizeof(tx_data), NULL, 0, TIME_INFINITE);
i2cReleaseBus(&I2CD1);
osalDbgCheck(MSG_OK == status);
/* read back inital data */
osalThreadSleepMilliseconds(2);
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, I2C_ADDR, tx_data, 1, rx_data, sizeof(rx_data), TIME_INFINITE);
i2cReleaseBus(&I2CD1);
osalDbgCheck(MSG_OK == status);
/* invert the data */
for (i = 1; i < sizeof(tx_data); i++)
tx_data[i] ^= 0xff;
/* write out inverted data */
osalThreadSleepMilliseconds(2);
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, I2C_ADDR, tx_data, sizeof(tx_data), NULL, 0, TIME_INFINITE);
i2cReleaseBus(&I2CD1);
osalDbgCheck(MSG_OK == status);
/* read back inverted data */
osalThreadSleepMilliseconds(2);
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, I2C_ADDR, tx_data, 1, rx_data, sizeof(rx_data), TIME_INFINITE);
i2cReleaseBus(&I2CD1);
osalDbgCheck(MSG_OK == status);
osalThreadSleepMilliseconds(TIME_INFINITE);
}
}
/*
* Application entry point.
*/
int main(void) {
uint8_t tx[8], rx[8];
/*
* 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();
palSetPadMode(IOPORT2, 0, PAL_MODE_ALTERNATIVE_2);
palSetPadMode(IOPORT2, 1, PAL_MODE_ALTERNATIVE_2);
i2cStart(&I2CD1, &i2ccfg);
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
while (1) {
tx[0] = 0x10;
tx[1] = 0x02;
i2cMasterTransmitTimeout(&I2CD1, 0x21, tx, 2, rx, 6, TIME_INFINITE);
i2cOk = (rx[0] == 0x10) ? true : false;
chThdSleepMilliseconds(2000);
}
}
static void MPU6050WriteRegister(uint8_t reg, uint8_t val)
{
uint8_t cmd[2] = {reg, val};
// i2cAcquireBus(&I2CD2);
(void)i2cMasterTransmitTimeout(&I2CD2, MPU6050_I2C_ADDR, cmd, 2, NULL, 0, TIME_INFINITE);
// i2cReleaseBus(&I2CD2);
}
static int get_raw_accel_gyro(int16_t* accel_gyro) {
msg_t res = MSG_OK;
tx_buf[0] = MPU9150_ACCEL_XOUT_H;
i2cAcquireBus(&I2C_DEV);
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 1, rx_buf, 14, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
// Acceleration
for (int i = 0;i < 3; i++) {
accel_gyro[i] = ((int16_t) ((uint16_t) rx_buf[2 * i] << 8)
+ rx_buf[2 * i + 1]);
}
// Angular rate
for (int i = 4;i < 7; i++) {
accel_gyro[i - 1] = ((int16_t) ((uint16_t) rx_buf[2 * i] << 8)
+ rx_buf[2 * i + 1]);
}
return 1;
}
int SMBusGet(I2CDriver * driver, uint8_t addr, uint8_t command, uint16_t* data){
uint8_t tx[1] = {command};
uint8_t rx[2];
i2cflags_t errors;
i2cAcquireBus(driver);
msg_t status = i2cMasterTransmitTimeout(driver, addr, tx, sizeof(tx), rx, sizeof(rx), I2C_TIMEOUT);
switch(status){
case RDY_OK:
i2cReleaseBus(driver);
break;
case RDY_RESET:
errors = i2cGetErrors(driver);
i2cReleaseBus(driver);
return errors;
case RDY_TIMEOUT:
/* On a timeout ChibiOS will set the bus into I2C_LOCKED, which will
* cause an assert to be hit if a bus transfer is tried again.
* I2C_LOCKED can only be cleared by i2cStart(), but i2cStart will hit
* an assert if i2cStop is not issued before restarting.
* This seems like a really terrible way to handle timeous*/
i2cStop(driver);
i2cStart(driver, driver->config);
i2cReleaseBus(driver);
return RDY_TIMEOUT;
default:
i2cReleaseBus(driver);
}
*data = DATA_FROM_BYTES(rx[0], rx[1]);
return RDY_OK;
}
/**
* @brief Loads all user defined settings from external EEPROM chip.
* @return 1 - if write operation is successful;
* 0 - if write operation fails.
*/
uint8_t eepromLoadSettings(void) {
msg_t status = RDY_OK;
uint8_t addr = EEPROM_START_ADDR;
i2cAcquireBus(&I2CD2);
status = i2cMasterTransmitTimeout(&I2CD2, EEPROM_24C02_ADDR, &addr, 1,
(uint8_t *)&eepromData, sizeof(eepromData), MS2ST(EEPROM_READ_TIMEOUT_MS));
i2cReleaseBus(&I2CD2);
if (status != RDY_OK) {
g_i2cErrorInfo.last_i2c_error = i2cGetErrors(&I2CD2);
if (g_i2cErrorInfo.last_i2c_error) {
debugLog("E:eeprom-load");
g_i2cErrorInfo.i2c_error_counter++;
}
return 0;
}
if (eepromData.crc32 != crcCRC32((uint32_t *)&eepromData, sizeof(eepromData) / sizeof(uint32_t) - 1)) {
/* Fill with default settings. */
return eepromSaveSettings();
} else {
pidSettingsUpdate(eepromData.pidSettings);
pwmOutputSettingsUpdate(eepromData.pwmOutput);
mixedInputSettingsUpdate(eepromData.mixedInput);
inputModeSettingsUpdate(eepromData.modeSettings);
sensorSettingsUpdate(eepromData.sensorSettings);
accelBiasUpdate(&g_IMU1, eepromData.accelBias);
gyroBiasUpdate(&g_IMU1, eepromData.gyroBias);
}
return 1;
}
/**
* Init function.
*/
int lsm303_mag_init(I2CDriver *i2cp) {
uint8_t rx_data[LSM303_RX_DEPTH];
uint8_t tx_data[LSM303_TX_DEPTH];
msg_t status = RDY_OK;
systime_t timeout = MS2ST(4);
/* configure accelerometer */
tx_data[0] = LSM303_CRA_REG_M; /* register address */
tx_data[1] = 0x1C;
tx_data[2] = 0x20;
tx_data[3] = 0x00;
/* sending */
i2cAcquireBus(i2cp);
status = i2cMasterTransmitTimeout(i2cp, LSM303_ADDR_M, tx_data, 4, rx_data, 0,
timeout);
i2cReleaseBus(i2cp);
if (status != RDY_OK) {
errors = i2cGetErrors(i2cp);
while (1);
}
return 0;
}
/*
* Application entry point.
*/
int main(void) {
uint8_t tx[1], rx[1];
/*
* 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();
palSetPad(IOPORT3, 3);
palSetPad(IOPORT4, 4);
palSetPad(IOPORT1, 2);
i2cStart(&I2CD1, NULL);
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
while (1) {
tx[0] = WHO_AM_I;
i2cMasterTransmitTimeout(&I2CD1, MMA8451_ADDR, tx, 1, rx, 1, TIME_INFINITE);
i2cOk = (rx[0] == 0x1A) ? true : false;
chThdSleepMilliseconds(2000);
}
}
/**
* @brief EEPROM write routine.
* @details Function writes data to EEPROM.
* @pre Data must be fit to single EEPROM page.
*
* @param[in] eepcfg pointer to configuration structure of eeprom file
* @param[in] offset addres of 1-st byte to be write
* @param[in] data pointer to buffer with data to be written
* @param[in] len number of bytes to be written
*/
static msg_t eeprom_write(const I2CEepromFileConfig *eepcfg, uint32_t offset,
const uint8_t *data, size_t len) {
msg_t status = RDY_RESET;
systime_t tmo = calc_timeout(eepcfg->i2cp, (len + 2), 0);
chDbgCheck(((len <= eepcfg->size) && ((offset + len) <= eepcfg->size)),
"out of device bounds");
chDbgCheck((((offset + eepcfg->barrier_low) / eepcfg->pagesize) ==
(((offset + eepcfg->barrier_low) + len - 1) / eepcfg->pagesize)),
"data can not be fitted in single page");
/* write address bytes */
eeprom_split_addr(eepcfg->write_buf, (offset + eepcfg->barrier_low));
/* write data bytes */
memcpy(&(eepcfg->write_buf[2]), data, len);
#if I2C_USE_MUTUAL_EXCLUSION
i2cAcquireBus(eepcfg->i2cp);
#endif
status = i2cMasterTransmitTimeout(eepcfg->i2cp, eepcfg->addr,
eepcfg->write_buf, (len + 2), NULL, 0, tmo);
#if I2C_USE_MUTUAL_EXCLUSION
i2cReleaseBus(eepcfg->i2cp);
#endif
/* wait until EEPROM process data */
chThdSleep(eepcfg->write_time);
return status;
}
/**
* @brief Writes a value into a register using I2C.
* @pre The I2C interface must be initialized and the driver started.
*
* @param[in] i2cp pointer to the I2C interface
* @param[in] sad slave address without R bit
* @param[in] txbuf buffer containing sub-address value in first position
* and values to write
* @param[in] n size of txbuf less one (not considering the first
* element)
* @return the operation status.
* @notapi
* @return the operation status.
*/
msg_t lps25hI2CWriteRegister(I2CDriver *i2cp, lps25h_sad_t sad, uint8_t* txbuf,
size_t n) {
if (n > 1)
(*txbuf) |= LPS25H_SUB_MS;
return i2cMasterTransmitTimeout(i2cp, sad, txbuf, n + 1, NULL, 0,
TIME_INFINITE);
}
/**
* Init function.
*/
int lsm303_acc_init(I2CDriver *i2cp) {
uint8_t rx_data[LSM303_RX_DEPTH];
uint8_t tx_data[LSM303_TX_DEPTH];
msg_t status = RDY_OK;
systime_t timeout = MS2ST(4);
/* configure accelerometer */
tx_data[0] = LSM303_CTRL_REG1_A | LSM303_AUTO_INCREMENT; /* register address */
tx_data[1] = 0x67; /* 200 Hz */
tx_data[2] = 0x80;
tx_data[3] = 0x10;
tx_data[4] = 0x08;
tx_data[5] = 0x08;
tx_data[6] = 0x00;
/* sending */
i2cAcquireBus(i2cp);
status = i2cMasterTransmitTimeout(i2cp, LSM303_ADDR_A, tx_data, 7, rx_data, 0,
timeout);
i2cReleaseBus(i2cp);
if (status != RDY_OK) {
errors = i2cGetErrors(i2cp);
while (1);
}
return 0;
}
/*
* Application entry point.
*/
int main(void) {
uint8_t tx[1], rx[1];
/*
* 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();
/*
* Turn off the RGB LED.
*/
palSetPad(GPIO_LED_RED, PIN_LED_RED); /* red */
palSetPad(GPIO_LED_GREEN, PIN_LED_GREEN); /* green */
palSetPad(GPIO_LED_BLUE, PIN_LED_BLUE); /* blue */
i2cStart(&I2CD1, &i2ccfg);
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
while (1) {
tx[0] = FXOS8700CQ_WHOAMI;
i2cMasterTransmitTimeout(&I2CD1, FXOS8700CQ_ADDR, tx, 1, rx, 1, TIME_INFINITE);
i2cOk = (rx[0] == FXOS8700CQ_WHOAMI_VAL) ? true : false;
chThdSleepMilliseconds(2000);
}
}
void request_color_data(void){
msg_t status = RDY_OK;
systime_t tmo = MS2ST(4);
color_tx_data[0] = COLOR_Y_STATUS_REG; /* register address */
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, vm6101_addr, color_tx_data, 1, color_rx_data, 20, tmo);
i2cReleaseBus(&I2CD1);
if (status != RDY_OK){
errors = i2cGetErrors(&I2CD1);
}
/* get the vals */
y_cnt_val = (color_rx_data[Y_CNT3]<<24) |
(color_rx_data[Y_CNT2]<<16) |
(color_rx_data[Y_CNT1]<<8) |
(color_rx_data[Y_CNT0]<<0) ;
r_cnt_val = (color_rx_data[R_CNT3]<<24) |
(color_rx_data[R_CNT2]<<16) |
(color_rx_data[R_CNT1]<<8) |
(color_rx_data[R_CNT0]<<0) ;
g_cnt_val = (color_rx_data[G_CNT3]<<24) |
(color_rx_data[G_CNT2]<<16) |
(color_rx_data[G_CNT1]<<8) |
(color_rx_data[G_CNT0]<<0) ;
b_cnt_val = (color_rx_data[B_CNT3]<<24) |
(color_rx_data[B_CNT2]<<16) |
(color_rx_data[B_CNT1]<<8) |
(color_rx_data[B_CNT0]<<0) ;
}
static msg_t temp_thread(void *arg) {
(void)arg;
chRegSetThreadName("I2C Temp samp");
uint8_t rxbuf[10];
uint8_t txbuf[10];
msg_t status = RDY_OK;
systime_t tmo = MS2ST(5);
i2caddr_t temp_addr = 0x48;
hw_start_i2c();
chThdSleepMilliseconds(10);
for(;;) {
if (i2c_running) {
txbuf[0] = 0x00;
i2cAcquireBus(&HW_I2C_DEV);
status = i2cMasterTransmitTimeout(&HW_I2C_DEV, temp_addr, txbuf, 1, rxbuf, 2, tmo);
i2cReleaseBus(&HW_I2C_DEV);
if (status == RDY_OK){
int16_t tempi = rxbuf[0] << 8 | rxbuf[1];
float temp = (float)tempi;
temp /= 128;
temp_now = temp;
} else {
// Try to restore the i2c bus
i2cAcquireBus(&HW_I2C_DEV);
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
for(int i = 0;i < 16;i++) {
palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
chThdSleep(1);
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
chThdSleep(1);
}
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
i2cStop(&HW_I2C_DEV);
i2cStart(&HW_I2C_DEV, &i2cfg);
i2cReleaseBus(&HW_I2C_DEV);
}
}
chThdSleepMilliseconds(100);
}
return 0;
}
/* Transmit data to sensor (used in init function only) */
static bool_t hmc5883l_transmit(uint8_t address, uint8_t data) {
uint8_t buffer[2];
buffer[0] = address;
buffer[1] = data;
/* Transmit message */
return (i2cMasterTransmitTimeout(&I2CD2, HMC5883L_I2C_ADDR, buffer, 2, NULL, 0, 1000) == RDY_OK);
}
static int ReadGyroData(void)
{
uint8_t reg = MPU6050_GYRO_XOUT_H;
// i2cAcquireBus(&I2CD2);
(void)i2cMasterTransmitTimeout(&I2CD2, MPU6050_I2C_ADDR, ®, 1, GyroReadDataBuffer, GYRO_DATA_SIZE, TIME_INFINITE);
// i2cReleaseBus(&I2CD2);
return 0;
}
static uint8_t MPU6050ReadRegister(uint8_t reg)
{
uint8_t data;
// i2cAcquireBus(&I2CD2);
(void)i2cMasterTransmitTimeout(&I2CD2, MPU6050_I2C_ADDR, ®, 1, &data, 1, TIME_INFINITE);
// i2cReleaseBus(&I2CD2);
return data;
}
bool io(const std::array<uint8_t, TxSize>& tx, std::array<uint8_t, RxSize>& rx)
{
const unsigned Address = 0x1E;
i2cAcquireBus(&I2CD);
const msg_t status = i2cMasterTransmitTimeout(&I2CD, Address, tx.data(), TxSize, rx.data(), RxSize, MS2ST(5));
i2cReleaseBus(&I2CD);
return status == RDY_OK;
}
/**
* @brief Reads registers value using I2C.
* @pre The I2C interface must be initialized and the driver started.
*
* @param[in] i2cp pointer to the I2C interface
* @param[in] sad slave address without R bit
* @param[in] reg first sub-register address
* @param[out] rxbuf pointer to an output buffer
* @param[in] n number of consecutive register to read
* @return the operation status.
* @notapi
*/
msg_t lps25hI2CReadRegister(I2CDriver *i2cp, lps25h_sad_t sad, uint8_t reg,
uint8_t* rxbuf, size_t n) {
uint8_t txbuf = reg;
if(n > 1)
txbuf |= LPS25H_SUB_MS;
return i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, n,
TIME_INFINITE);
}
static inline msg_t ms5611_reg_writec(uint8_t val)
{
msg_t ret;
i2cAcquireBus(&I2CD1);
ret = i2cMasterTransmitTimeout(&I2CD1, MS5611_I2C_ADDR, &val, sizeof(val), NULL, 0, I2C_TIMEOUT);
i2cReleaseBus(&I2CD1);
return ret;
}
static inline msg_t hmc5883_reg_readm(uint8_t reg, uint8_t *rdbuf, size_t rdsize)
{
msg_t ret;
i2cAcquireBus(&I2CD1);
ret = i2cMasterTransmitTimeout(&I2CD1, HMC5883_I2C_ADDR, ®, sizeof(reg), rdbuf, rdsize, I2C_TIMEOUT);
i2cReleaseBus(&I2CD1);
return ret;
}
static inline msg_t mpu6050_reg_readm(uint8_t reg, uint8_t *rdbuf, size_t rdsize)
{
msg_t ret;
i2cAcquireBus(&I2CD1);
ret = i2cMasterTransmitTimeout(&I2CD1, dev.addr, ®, sizeof(reg), rdbuf, rdsize, I2C_TIMEOUT);
i2cReleaseBus(&I2CD1);
return ret;
}
void lsm303_acc_update(I2CDriver *i2cp) {
uint8_t rx_data[LSM303_RX_DEPTH];
uint8_t tx_data[LSM303_TX_DEPTH];
msg_t status = RDY_OK;
systime_t tmo = MS2ST(4);
acc_data.t = chTimeNow();
tx_data[0] = LSM303_OUT_X_L_A | LSM303_AUTO_INCREMENT; /* register address */
i2cAcquireBus(i2cp);
status = i2cMasterTransmitTimeout(i2cp, LSM303_ADDR_A, tx_data, 1, rx_data, 6,
tmo);
i2cReleaseBus(i2cp);
if (status != RDY_OK) {
errors = i2cGetErrors(i2cp);
palClearPad(LED_GPIO, LED1);
palClearPad(LED_GPIO, LED2);
palClearPad(LED_GPIO, LED3);
palClearPad(LED_GPIO, LED4);
while (1);
}
acc_data.x = *((int16_t*)&(rx_data[0])) >> 4;
acc_data.y = *((int16_t*)&(rx_data[2])) >> 4;
acc_data.z = *((int16_t*)&(rx_data[4])) >> 4;
tx_data[0] = LSM303_STATUS_REG_A; /* register address */
i2cAcquireBus(i2cp);
status = i2cMasterTransmitTimeout(i2cp, LSM303_ADDR_A, tx_data, 1, rx_data, 2,
tmo);
i2cReleaseBus(i2cp);
if (status != RDY_OK) {
errors = i2cGetErrors(i2cp);
palClearPad(LED_GPIO, LED1);
palClearPad(LED_GPIO, LED2);
palClearPad(LED_GPIO, LED3);
palClearPad(LED_GPIO, LED4);
while (1);
}
status_a = rx_data[0];
}
static inline msg_t hmc5883_reg_writeb(uint8_t reg, uint8_t val)
{
uint8_t regbuf[] = { reg, val };
msg_t ret;
i2cAcquireBus(&I2CD1);
ret = i2cMasterTransmitTimeout(&I2CD1, HMC5883_I2C_ADDR, regbuf, sizeof(regbuf), NULL, 0, I2C_TIMEOUT);
i2cReleaseBus(&I2CD1);
return ret;
}
static inline msg_t mpu6050_reg_writeb(uint8_t reg, uint8_t val)
{
uint8_t regbuf[] = { reg, val };
msg_t ret;
i2cAcquireBus(&I2CD1);
ret = i2cMasterTransmitTimeout(&I2CD1, dev.addr, regbuf, sizeof(regbuf), NULL, 0, I2C_TIMEOUT);
i2cReleaseBus(&I2CD1);
return ret;
}
static msg_t mpl3115A2_write_register(I2CDriver* i2cptr, MPL3115A2_regaddr ra, mpl3115a2_i2c_data d) {
msg_t status = RDY_OK;
mpl3115a2_driver.txbuf[0] = ra;
mpl3115a2_driver.txbuf[1] = d;
i2cAcquireBus(i2cptr);
status = i2cMasterTransmitTimeout(i2cptr, mpl3115a2_i2c_slave_addr, mpl3115a2_driver.txbuf, 2, mpl3115a2_driver.rxbuf, 0, mpl3115a2_i2c_timeout);
i2cReleaseBus(i2cptr);
return status;
}
msg_t I2C::transmit(i2caddr_t addr, const uint8_t* txbuf, size_t txbytes,
uint8_t* rxbuf, size_t rxbytes, systime_t timeout){
msg_t msg;
lock();
msg = i2cMasterTransmitTimeout(&driver, addr, txbuf, txbytes, rxbuf,
rxbytes, timeout);
if(msg == RDY_TIMEOUT){
i2cStop(&driver);
i2cStart(&driver, &config);
}
unlock();
return msg;
}
