byte MPU9250_DMP::begin(bool fusion, int rate)
{
i2c_port_t i2c_master_port = I2C_NUM_0;
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = (gpio_num_t)21;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_io_num = (gpio_num_t)22;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = 100000;
if (i2c_param_config(i2c_master_port, &conf) != ESP_OK || i2c_driver_install(i2c_master_port, conf.mode, 0, 0, 0) != ESP_OK)
return 0;
struct int_param_s int_param;
inv_error_t result = mpu_init(&int_param);
if (result)
return 0;
mpu_set_bypass(1); // Place all slaves (including compass) on primary bus
setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS | INV_XYZ_COMPASS);
_gSense = getGyroSens();
_aSense = getAccelSens();
_features = DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_CAL_GYRO;
if (fusion) _features |= DMP_FEATURE_6X_LP_QUAT;
// Set DMP FIFO rate to 10 Hz
if (dmpBegin(_features, rate) == INV_ERROR)
return 0;
return 1;
}
static int i2c_init()
{
int res;
res = i2c_param_config(I2C_NUM_0, &es_i2c_cfg);
res |= i2c_driver_install(I2C_NUM_0, es_i2c_cfg.mode, 0, 0, 0);
ES_ASSERT(res, "i2c_init error", -1);
return res;
}
void initI2C() {
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = SDA_PIN;
conf.scl_io_num = SCL_PIN;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = 100000;
ESP_ERROR_CHECK(i2c_param_config(I2C_NUM_0, &conf));
ESP_ERROR_CHECK(i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0));
}
// void i2c_setup(cell sda, cell scl)
cell i2c_open(uint8_t sda, uint8_t scl)
{
int i2c_master_port = 1;
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = sda;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_io_num = scl;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = 100000;
i2c_param_config(i2c_master_port, &conf);
return i2c_driver_install(i2c_master_port, conf.mode,
0, 0, 0 // No Rx buf, No Tx buf, no intr flags
);
}
/*
* HAL callback function as prescribed by the U8G2 library. This callback is invoked
* to handle I2C communications.
*/
uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr) {
ESP_LOGD(TAG, "i2c_cb: Received a msg: %d, arg_int: %d, arg_ptr: %p", msg, arg_int, arg_ptr);
switch(msg) {
case U8X8_MSG_BYTE_SET_DC: {
if (u8g2_esp32_hal.dc != U8G2_ESP32_HAL_UNDEFINED) {
gpio_set_level(u8g2_esp32_hal.dc, arg_int);
}
break;
}
case U8X8_MSG_BYTE_INIT: {
if (u8g2_esp32_hal.sda == U8G2_ESP32_HAL_UNDEFINED ||
u8g2_esp32_hal.scl == U8G2_ESP32_HAL_UNDEFINED) {
break;
}
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
ESP_LOGI(TAG, "sda_io_num %d", u8g2_esp32_hal.sda);
conf.sda_io_num = u8g2_esp32_hal.sda;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
ESP_LOGI(TAG, "scl_io_num %d", u8g2_esp32_hal.scl);
conf.scl_io_num = u8g2_esp32_hal.scl;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
ESP_LOGI(TAG, "clk_speed %d", I2C_MASTER_FREQ_HZ);
conf.master.clk_speed = I2C_MASTER_FREQ_HZ;
ESP_LOGI(TAG, "i2c_param_config %d", conf.mode);
ESP_ERROR_CHECK(i2c_param_config(I2C_MASTER_NUM, &conf));
ESP_LOGI(TAG, "i2c_driver_install %d", I2C_MASTER_NUM);
ESP_ERROR_CHECK(i2c_driver_install(I2C_MASTER_NUM, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0));
break;
}
case U8X8_MSG_BYTE_SEND: {
uint8_t* data_ptr = (uint8_t*)arg_ptr;
ESP_LOG_BUFFER_HEXDUMP(TAG, data_ptr, arg_int, ESP_LOG_VERBOSE);
while( arg_int > 0 ) {
ESP_ERROR_CHECK(i2c_master_write_byte(handle_i2c, *data_ptr, ACK_CHECK_EN));
data_ptr++;
arg_int--;
}
break;
}
case U8X8_MSG_BYTE_START_TRANSFER: {
uint8_t i2c_address = u8x8_GetI2CAddress(u8x8);
handle_i2c = i2c_cmd_link_create();
ESP_LOGD(TAG, "Start I2C transfer to %02X.", i2c_address>>1);
ESP_ERROR_CHECK(i2c_master_start(handle_i2c));
ESP_ERROR_CHECK(i2c_master_write_byte(handle_i2c, i2c_address | I2C_MASTER_WRITE, ACK_CHECK_EN));
break;
}
case U8X8_MSG_BYTE_END_TRANSFER: {
ESP_LOGD(TAG, "End I2C transfer.");
ESP_ERROR_CHECK(i2c_master_stop(handle_i2c));
ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_MASTER_NUM, handle_i2c, I2C_TIMEOUT_MS / portTICK_RATE_MS));
i2c_cmd_link_delete(handle_i2c);
break;
}
}
return 0;
} // u8g2_esp32_i2c_byte_cb
void task_hmc5883l(void *ignore) {
ESP_LOGD(tag, ">> hmc5883l");
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = PIN_SDA;
conf.scl_io_num = PIN_CLK;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = 100000;
ESP_ERROR_CHECK(i2c_param_config(I2C_NUM_0, &conf));
ESP_ERROR_CHECK(i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0));
uint8_t data[6];
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (I2C_ADDRESS << 1) | I2C_MASTER_WRITE, 1);
i2c_master_write_byte(cmd, 0x02, 1); // 0x02 = "Mode register"
i2c_master_write_byte(cmd, 0x00, 1); // 0x00 = "Continuous-Measurement Mode"
i2c_master_stop(cmd);
i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000/portTICK_PERIOD_MS);
i2c_cmd_link_delete(cmd);
//Set value in "Configuration Register B"
cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (I2C_ADDRESS << 1) | I2C_MASTER_WRITE, 1);
i2c_master_write_byte(cmd, 0x01, 1); // 0x01 = "Configuration Register B"
i2c_master_write_byte(cmd, 0x20, 1); // 0x20 = default Gain setting
i2c_master_stop(cmd);
i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000/portTICK_PERIOD_MS);
i2c_cmd_link_delete(cmd);
//Set active register to "Identification Register A"
cmd = i2c_cmd_link_create();
ESP_ERROR_CHECK(i2c_master_start(cmd));
ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (I2C_ADDRESS << 1) | I2C_MASTER_WRITE, 1));
ESP_ERROR_CHECK(i2c_master_write_byte(cmd, 10, 1)); //10 = 0x0A = "Identification Register A"
ESP_ERROR_CHECK(i2c_master_stop(cmd));
ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM_0, cmd, 100/portTICK_PERIOD_MS));
i2c_cmd_link_delete(cmd);
//Get data from Identification Register A, B and C
cmd = i2c_cmd_link_create();
ESP_ERROR_CHECK(i2c_master_start(cmd));
ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (I2C_ADDRESS << 1) | I2C_MASTER_READ, 1));
i2c_master_read_byte(cmd, data, 0);
i2c_master_read_byte(cmd, data+1, 0);
i2c_master_read_byte(cmd, data+2, 1);
ESP_ERROR_CHECK(i2c_master_stop(cmd));
ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM_0, cmd, 100/portTICK_PERIOD_MS));
i2c_cmd_link_delete(cmd);
while(1) {
//Set active registert to "Data Output X MSB Register"
cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (I2C_ADDRESS << 1) | I2C_MASTER_WRITE, 1);
i2c_master_write_byte(cmd, 0x03, 1); //0x03 = "Data Output X MSB Register"
i2c_master_stop(cmd);
i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000/portTICK_PERIOD_MS);
i2c_cmd_link_delete(cmd);
//Read values for X, Y and Z
cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (I2C_ADDRESS << 1) | I2C_MASTER_READ, 1);
i2c_master_read_byte(cmd, data, 0); //"Data Output X MSB Register"
i2c_master_read_byte(cmd, data+1, 0); //"Data Output X LSB Register"
i2c_master_read_byte(cmd, data+2, 0); //"Data Output Z MSB Register"
i2c_master_read_byte(cmd, data+3, 0); //"Data Output Z LSB Register"
i2c_master_read_byte(cmd, data+4, 0); //"Data Output Y MSB Register"
i2c_master_read_byte(cmd, data+5, 1); //"Data Output Y LSB Register "
i2c_master_stop(cmd);
i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000/portTICK_PERIOD_MS);
i2c_cmd_link_delete(cmd);
short x = data[0] << 8 | data[1];
short z = data[2] << 8 | data[3];
short y = data[4] << 8 | data[5];
int angle = atan2((double)y,(double)x) * (180 / 3.14159265) + 180; // angle in degrees
ESP_LOGD(tag, "angle: %d, x: %d, y: %d, z: %d", angle, x, y, z);
vTaskDelay(1000/portTICK_PERIOD_MS);
}
vTaskDelete(NULL);
} // task_hmc5883l
