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 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 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;
}
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);
}
}
/**
* @brief Deactivates the LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
*
* @api
*/
void lsm6ds0Stop(LSM6DS0Driver *devp) {
uint8_t cr[2];
osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
"lsm6ds0Stop(), invalid state");
if (devp->state == LSM6DS0_READY) {
if((devp)->config->acccfg != NULL) {
cr[0] = LSM6DS0_AD_CTRL_REG6_XL;
/* Control register 6 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 */
}
if((devp)->config->gyrocfg != NULL) {
cr[0] = LSM6DS0_AD_CTRL_REG9;
/* Control register 9 configuration block.*/
{
cr[1] = LSM6DS0_CTRL_REG9_SLEEP_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, 1);
i2cStop((devp)->config->i2cp);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
}
}
devp->state = LSM6DS0_STOP;
}
/**
* @brief Deactivates the LPS25H Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LPS25HDriver object
*
* @api
*/
void lps25hStop(LPS25HDriver *devp) {
uint8_t cr[2];
osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state == LPS25H_STOP) || (devp->state == LPS25H_READY),
"lps25hStop(), invalid state");
if (devp->state == LPS25H_READY) {
#if (LPS25H_USE_I2C)
#if LPS25H_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
i2cStart((devp)->config->i2cp,
(devp)->config->i2ccfg);
#endif /* LPS25H_SHARED_I2C */
cr[0] = LPS25H_AD_CTRL_REG1;
cr[1] = 0;
lps25hI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
i2cStop((devp)->config->i2cp);
#if LPS25H_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LPS25H_SHARED_I2C */
#endif /* LPS25H_USE_I2C */
}
devp->state = LPS25H_STOP;
}
static msg_t read_raw(void *ip, int32_t* axis) {
uint8_t buff[3];
msg_t msg = MSG_OK;
*axis = 0;
osalDbgCheck((ip != NULL) && (axis != NULL));
osalDbgAssert((((LPS25HDriver *)ip)->state == LPS25H_READY),
"read_raw(), invalid state");
#if LPS25H_USE_I2C
osalDbgAssert((((LPS25HDriver *)ip)->config->i2cp->state == I2C_READY),
"read_raw(), channel not ready");
#if LPS25H_SHARED_I2C
i2cAcquireBus(((LPS25HDriver *)ip)->config->i2cp);
i2cStart(((LPS25HDriver *)ip)->config->i2cp,
((LPS25HDriver *)ip)->config->i2ccfg);
#endif /* LPS25H_SHARED_I2C */
msg = lps25hI2CReadRegister(((LPS25HDriver *)ip)->config->i2cp,
((LPS25HDriver *)ip)->config->slaveaddress,
LPS25H_AD_PRESS_OUT_XL, buff, 3);
#if LPS25H_SHARED_I2C
i2cReleaseBus(((LPS25HDriver *)ip)->config->i2cp);
#endif /* LPS25H_SHARED_I2C */
#endif /* LPS25H_USE_I2C */
if(msg == MSG_OK) {
*axis = buff[0] + (buff[1] << 8) + (buff[2] << 16);
}
return msg;
}
/**
* @brief Deactivates the LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
*
* @api
*/
void lsm6ds0Stop(LSM6DS0Driver *devp) {
uint8_t cr[2];
osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
"lsm6ds0Stop(), invalid state");
if (devp->state == LSM6DS0_READY) {
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus(devp->config->i2cp);
i2cStart(devp->config->i2cp, devp->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
/* Disabling accelerometer.*/
cr[0] = LSM6DS0_AD_CTRL_REG6_XL;
cr[1] = 0;
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
/* Disabling gyroscope.*/
cr[0] = LSM6DS0_AD_CTRL_REG9;
cr[1] = LSM6DS0_CTRL_REG9_SLEEP_G;
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
i2cStop(devp->config->i2cp);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
}
devp->state = LSM6DS0_STOP;
}
static uint32_t getTemperatureStr(char * temperatureStr)
{
float temperature = 0;
float pressure = 0;
spiAcquireBus(&CC3000_SPI_DRIVER);
i2cAcquireBus(&I2C_DRIVER);
i2cStart(&I2C_DRIVER, &i2cConfig);
if (RDY_OK != mplOneShotReadBarometer(&I2C_DRIVER,
MPL3115A2_DEFAULT_ADDR,
&pressure,
&temperature))
{
return 1;
}
i2cStop(&I2C_DRIVER);
i2cReleaseBus(&I2C_DRIVER);
spiReleaseBus(&CC3000_SPI_DRIVER);
temperature += CELSIUS_TO_KELVIN;
snprintf(temperatureStr, TEMPERATURE_STRING_SIZE, "%.2f K", temperature);
temperatureStr[TEMPERATURE_STRING_SIZE-1] = 0;
return 0;
}
static uint32_t getPressureStr(char * pressureStr)
{
float pressure;
float temperature;
spiAcquireBus(&CC3000_SPI_DRIVER);
i2cAcquireBus(&I2C_DRIVER);
i2cStart(&I2C_DRIVER, &i2cConfig);
if (RDY_OK != mplOneShotReadBarometer(&I2C_DRIVER,
MPL3115A2_DEFAULT_ADDR,
&pressure,
&temperature))
{
return 1;
}
i2cStop(&I2C_DRIVER);
i2cReleaseBus(&I2C_DRIVER);
spiReleaseBus(&CC3000_SPI_DRIVER);
pressure /= 1000; /* kPa */
snprintf(pressureStr, PRESSURE_STRING_SIZE, "%.2f kPa", pressure);
pressureStr[PRESSURE_STRING_SIZE-1] = 0;
return 0;
}
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) ;
}
/**
* @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;
}
uint8_t I2C::read_byte_test(i2caddr_t addr, uint8_t regaddr){
msg_t msg;
uint8_t ret;
msg = transmit(addr, ®addr, 1, &ret, 1, MS2ST(4));
if(msg == RDY_TIMEOUT){
ret = 0;
i2cAcquireBus(&driver);
driver.i2c->CR1 |= I2C_CR1_START;
chThdSleep(MS2ST(1));
if((driver.i2c->SR2 & I2C_SR2_MSL) == 0){
// I2C is messed up
scl.set_mode(gpio_pin_t::MODE_OUTPUT);
for(uint32_t i = 0; i < 10; i++){
scl.clear();
chThdSleep(MS2ST(1));
scl.set();
chThdSleep(MS2ST(1));
}
scl.set_mode(gpio_pin_t::MODE_ALT);
}
while(driver.i2c->SR1 & I2C_SR1_TXE);
i2cReleaseBus(&driver);
msg = transmit(addr, ®addr, 1, &ret, 1, MS2ST(4));
if(msg == RDY_TIMEOUT){
ret = 0;
}
}
return ret;
}
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;
}
/**
* 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;
}
/**
* 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;
}
void i2cScanner(I2CDriver *FindI2C, const char *driverName) {
uint8_t x = 0, txbuf[2], rxbuf[2] ;
msg_t messages = 0 ;
for(x = 0 ; x < 128 ; x++){
txbuf[0] = 0x00 ;
txbuf[1] = 0x00 ;
i2cAcquireBus(FindI2C) ;
messages = i2cMasterTransmit(FindI2C, x, txbuf, 2, rxbuf, 0) ;
i2cReleaseBus(FindI2C) ;
if(messages == 0) {
if(x == 0x28)
chprintf((BaseSequentialStream *)&OUTPUT, "Differential Pressure Sensor\tDetected on %s at Address 0x28\r\n", driverName) ;
if(x == 0x1E)
chprintf((BaseSequentialStream *)&OUTPUT, "3-axis Magnetometer\t\tDetected on %s at Address 0x1E\r\n", driverName) ;
if(x == 0x68)
chprintf((BaseSequentialStream *)&OUTPUT, "3-axis Accel, 3-axis Gyro\tDetected on %s at Address 0x68\r\n", driverName) ;
if(x == 0x69)
chprintf((BaseSequentialStream *)&OUTPUT, "3-axis Accel, 3-axis Gyro\tDetected on %s at Address 0x69\r\n", driverName) ;
if(x == 0x77)
chprintf((BaseSequentialStream *)&OUTPUT, "Static Pressure Sensor (Baro)\tDetected on %s at Address 0x77\r\n", driverName) ;
}
chThdSleepMilliseconds(1) ;
}
chThdSleepMilliseconds(100) ;
}
/**
* @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;
}
//-----------------------------------------------------------------------------
static void lcd_HD44780_write(uint8_t data) {
if (engineConfiguration->displayMode == DM_HD44780) {
palWritePad(HD44780_PORT_DB7, HD44780_PIN_DB7, data & 0x80 ? 1 : 0);
palWritePad(HD44780_PORT_DB6, HD44780_PIN_DB6, data & 0x40 ? 1 : 0);
palWritePad(HD44780_PORT_DB5, HD44780_PIN_DB5, data & 0x20 ? 1 : 0);
palWritePad(HD44780_PORT_DB4, HD44780_PIN_DB4, data & 0x10 ? 1 : 0);
palSetPad(HD44780_PORT_E, HD44780_PIN_E); // En high
lcdSleep(10); // enable pulse must be >450ns
palClearPad(HD44780_PORT_E, HD44780_PIN_E); // En low
lcdSleep(40); // commands need > 37us to settle
} else {
// LCD D4_pin -> P4
// LCD D5_pin -> P5
// LCD D6_pin -> P6
// LCD D7_pin -> P7
// LCD Pin RS -> P0
// LCD Pin RW -> P1
// LCD Pin E -> P2
i2cAcquireBus(&I2CD1);
txbuf[0] = 4;
i2cMasterTransmit(&I2CD1, LCD_PORT_EXP_ADDR, txbuf, 1, NULL, 0);
lcdSleep(10); // enable pulse must be >450ns
txbuf[0] = 0;
i2cMasterTransmit(&I2CD1, LCD_PORT_EXP_ADDR, txbuf, 1, NULL, 0);
i2cReleaseBus(&I2CD1);
}
}
int hmc5843Read (int16_t *xouth, int16_t *youth, int16_t *zouth,
int16_t *xoutl, int16_t *youtl, int16_t *zoutl)
{
int s = 0;
i2cAcquireBus(d.i2cp);
/* Read X/Y/Z high/low bits and status from 8-bit registers */
d.rx[0] = d.rx[1] = d.rx[2] = d.rx[3] = d.rx[4] = d.rx[5] = d.rx[6] = 0;
i2cMasterReceive(d.i2cp, &d.cfg, d.addr, d.rx, 2);
#if 1
d.tx[0] = HMC5843_MODE;
d.tx[1] = HMC5843_MODE_SINGLE;
i2cMasterTransmit(d.i2cp, &dummycfg, d.addr, d.tx, 2, NULL, 0);
#endif
i2cReleaseBus(d.i2cp);
chThdSleepMilliseconds(60);
*xouth = d.rx[0];
*xoutl = d.rx[1];
*youth = d.rx[2];
*youtl = d.rx[3];
*zouth = d.rx[4];
*zoutl = d.rx[5];
return s * 5;
}
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 Deactivates the LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
*
* @api
*/
void lsm6ds0Stop(LSM6DS0Driver *devp) {
osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
"lsm6ds0Stop(), invalid state");
#if (LSM6DS0_USE_I2C)
if (devp->state == LSM6DS0_STOP) {
#if LSM6DS0_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
i2cStart((devp)->config->i2cp,
(devp)->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
if((devp)->config->acccfg != NULL) {
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG6_XL,
LSM6DS0_ACC_ODR_PD);
}
if((devp)->config->gyrocfg != NULL) {
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG9,
LSM6DS0_GYRO_SLP_ENABLED);
}
i2cStop((devp)->config->i2cp);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
}
#endif /* LSM6DS0_USE_I2C */
devp->state = LSM6DS0_STOP;
}
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;
}
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 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;
}
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_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;
}
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;
}
