ESP32-ULP-I2C-MPU6050

Post Reply
aniketshantanubenke
Posts: 1
Joined: Thu Apr 13, 2023 9:32 am

ESP32-ULP-I2C-MPU6050

Postby aniketshantanubenke » Fri Apr 21, 2023 10:28 am

I am trying to read mpu6050 through i2c using ulp core. I used https://github.com/tomtor/ulp-i2c as a reference to get started. I've modified this for my use case. I am fairly new to ulp programming. The execution of ulp gets stuck on readmpu6050. Attaching below relevant code. Thank you for the help.

Code: Select all

[Codebox=c file=main.c]
#include <stdio.h>
#include "driver/gpio.h"
#include "driver/i2c.h"
#include "driver/rtc_io.h"
#include "freertos/FreeRTOS.h"
#include "soc/rtc_cntl_reg.h"
#include "soc/rtc_io_reg.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "ulp_main.h"
#include "esp_log.h"
#include "soc/sens_reg.h"
#include "soc/soc.h"
#include "esp32/ulp.h"
#include "esp_sleep.h"

#define PIN_SDA 32
#define PIN_CLK 33
#define I2C_ADDRESS 0x68 // I2C address of MPU6050

#define MPU6050_PWR_MGMT_1   0x6B

#define ac1 ((short)ulp_ax)
#define ac2 ((short)ulp_ay)
#define ac3 ((short)ulp_az)

extern const uint8_t ulp_main_bin_start[] asm("_binary_ulp_main_bin_start");
extern const uint8_t ulp_main_bin_end[]   asm("_binary_ulp_main_bin_end");


const gpio_num_t gpio_sda = GPIO_NUM_32;
const gpio_num_t gpio_scl = GPIO_NUM_33;


static void init_ulp_program()
{
    rtc_gpio_init(GPIO_NUM_2);
    rtc_gpio_set_direction(GPIO_NUM_2, RTC_GPIO_MODE_OUTPUT_ONLY);

	rtc_gpio_init(GPIO_NUM_12);
    rtc_gpio_set_direction(GPIO_NUM_2, RTC_GPIO_MODE_OUTPUT_ONLY);

    rtc_gpio_init(gpio_sda);
    rtc_gpio_set_direction(gpio_sda, RTC_GPIO_MODE_INPUT_ONLY);
    rtc_gpio_init(gpio_scl);
    rtc_gpio_set_direction(gpio_scl, RTC_GPIO_MODE_INPUT_ONLY);

    esp_err_t err = ulp_load_binary(0, ulp_main_bin_start,
            (ulp_main_bin_end - ulp_main_bin_start) / sizeof(uint32_t));
    ESP_ERROR_CHECK(err);

    /* Set ULP wake up period to T = 1000ms
     * Minimum pulse width has to be T * (ulp_debounce_counter + 1) = 80ms.
     */

}

void i2c_task(void *pvParameter) {
	gpio_set_direction(GPIO_NUM_22, GPIO_MODE_OUTPUT);
	gpio_set_level(GPIO_NUM_22, 0);

	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));

	i2c_cmd_handle_t cmd;
	vTaskDelay(200/portTICK_PERIOD_MS);

	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));
	i2c_master_write_byte(cmd, MPU6050_PWR_MGMT_1, 1);
	i2c_master_write_byte(cmd, 0, 1);
	ESP_ERROR_CHECK(i2c_master_stop(cmd));
	i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000/portTICK_PERIOD_MS);
	i2c_cmd_link_delete(cmd);

	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));
	i2c_master_write_byte(cmd, 0x1B, 1);
	i2c_master_write_byte(cmd, 0, 1);
	ESP_ERROR_CHECK(i2c_master_stop(cmd));
	i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000/portTICK_PERIOD_MS);
	i2c_cmd_link_delete(cmd);

	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));
	i2c_master_write_byte(cmd, 0x1C, 1);
	i2c_master_write_byte(cmd, 0, 1);
	ESP_ERROR_CHECK(i2c_master_stop(cmd));
	i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000/portTICK_PERIOD_MS);
	i2c_cmd_link_delete(cmd);


	uint8_t data[14];

	short accel_x;
	short accel_y;
	short accel_z;

	while(1) {
		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, 0x3B, 1));
		ESP_ERROR_CHECK(i2c_master_stop(cmd));
		ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000/portTICK_PERIOD_MS));
		i2c_cmd_link_delete(cmd);

		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));

		ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data,   0));
		ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data+1, 0));
		ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data+2, 0));
		ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data+3, 0));
		ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data+4, 0));
		ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data+5, 1));

		//i2c_master_read(cmd, data, sizeof(data), 1);
		ESP_ERROR_CHECK(i2c_master_stop(cmd));
		ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000/portTICK_PERIOD_MS));
		i2c_cmd_link_delete(cmd);

		accel_x = (data[0] << 8) | data[1];
		accel_y = (data[2] << 8) | data[3];
		accel_z = (data[4] << 8) | data[5];

		accel_x = ((float)accel_x/16384)*100;
		accel_y = ((float)accel_y/16384)*100;
		accel_z = ((float)accel_z/16384)*100;
		ESP_LOGE("MPU", "accel_x: %d, accel_y: %d, accel_z: %d", accel_x, accel_y, accel_z);

		vTaskDelay(500/portTICK_PERIOD_MS);

		esp_sleep_wakeup_cause_t cause = esp_sleep_get_wakeup_cause();
		if (cause != ESP_SLEEP_WAKEUP_ULP) {
			printf("Not ULP wakeup, initializing ULP\n");
			init_ulp_program();
		} else {

			printf("ULP wakeup, printing status\n");
		}

		ESP_ERROR_CHECK( esp_sleep_enable_ulp_wakeup() );
		ulp_set_wakeup_period(0, 1000*1000);
		esp_err_t err = ulp_run(&ulp_entry - RTC_SLOW_MEM);
		ESP_ERROR_CHECK(err);

		ESP_LOGE("ESP-SLEEP","ESP IN DEEP SLEEP");
		vTaskDelay(100 / portTICK_PERIOD_MS);
		esp_deep_sleep_start();
	}
}

void app_main(void){

    xTaskCreate(&i2c_task, "i2c_task", 16384, NULL, 1, NULL);
}
[/Codebox]

Code: Select all

[Codebox=text file=i2c.S]
/*
 * Demo of I2C ULP routines
 */

#include "soc/rtc_cntl_reg.h"
#include "soc/rtc_io_reg.h"
#include "soc/soc_ulp.h"

#include "stack.S"

.global entry

	/* Code goes into .text section */
	.text
entry:
	WRITE_RTC_REG(RTC_GPIO_OUT_REG, RTC_GPIO_OUT_DATA_S + 12, 1, 1)
	WRITE_RTC_REG(RTC_GPIO_ENABLE_W1TS_REG, RTC_GPIO_ENABLE_W1TS_S + 12, 1, 1)

	move r2,1000
	psr
	jump waitMs

	psr
	jump readmpu6050

	WRITE_RTC_REG(RTC_GPIO_OUT_REG, RTC_GPIO_OUT_DATA_S + 12, 1, 0)
	WRITE_RTC_REG(RTC_GPIO_ENABLE_W1TC_REG, RTC_GPIO_ENABLE_W1TC_S + 12, 1, 1)

	halt

[/Codebox]

Code: Select all

[Codebox=text file=i2c-utils.S]
/*
 * I2C ULP utility routines
 */

#include "soc/rtc_cntl_reg.h"
#include "soc/rtc_io_reg.h"
#include "soc/soc_ulp.h"

#include "stack.S"

.text

write_intro:
	psr
	jump i2c_start_cond

	ld r2,r3,20 // Address
	lsh r2,r2,1
	psr
	jump i2c_write_byte
	jumpr popfail,1,ge

	ld r2,r3,16 // Register
	psr
	jump i2c_write_byte
	jumpr popfail,1,ge
	ret


.global write8
write8:
	psr
	jump write_intro

.global write_b
write_b:
	ld r2,r3,8 // data byte
	psr
	jump i2c_write_byte
	jumpr fail,1,ge

	psr
	jump i2c_stop_cond

	move r2,0 // Ok
	ret


.global write16
write16:
	psr
	jump write_intro

	ld r2,r3,8 // data byte 1
	rsh r2,r2,8
	psr
	jump i2c_write_byte
	jumpr fail,1,ge

	jump write_b


read_intro:
	psr
	jump i2c_start_cond

	ld r2,r3,16 // Address
	lsh r2,r2,1
	psr
	jump i2c_write_byte
	jumpr popfail,1,ge

	ld r2,r3,12 // Register
	psr
	jump i2c_write_byte
	jumpr popfail,1,ge

	psr
	jump i2c_start_cond

	ld r2,r3,16
	lsh r2,r2,1
	or r2,r2,1 // Address Read
	psr
	jump i2c_write_byte
	jumpr popfail,1,ge

	ret
popfail:
	pop r1 // pop caller return address
	move r2,1
	ret

.global read8
read8:
	psr
	jump read_intro

	move r2,1 // last byte
	psr
	jump i2c_read_byte
	push r0

	psr
	jump i2c_stop_cond

	pop r0

	move r2,0 // OK
	ret
fail:
	move r2,1
	ret

.global read16
read16:
	psr
	jump read_intro

	move r2,0
	psr
	jump i2c_read_byte
	push r0

	move r2,1 // last byte
	psr
	jump i2c_read_byte
	push r0

	psr
	jump i2c_stop_cond

	pop r0
	pop r2 // first byte
	lsh r2,r2,8
	or r2,r2,r0
	move r0,r2

	move r2,0 // OK
	ret
[/Codebox]

Code: Select all

[Codebox=text file=stack.S]
/*
 * ULP stack and subroutine macros
 */

.macro push rx
	st \rx,r3,0
	sub r3,r3,1
.endm

.macro pop rx
	add r3,r3,1
	ld \rx,r3,0
.endm

// Prepare subroutine jump, uses scratch register sr
.macro psr sr=r1 pos=.
	.set _next2,(\pos+16)
	move \sr,_next2
	push \sr
.endm

// Return from subroutine
.macro ret sr=r1
	pop \sr
	jump \sr
.endm
[/Codebox]

Code: Select all

[Codebox=text file=i2cmpu-6050.S]
/*
 * Demo of I2C ULP routines
 */

#include "soc/rtc_cntl_reg.h"
#include "soc/rtc_io_reg.h"
#include "soc/soc_ulp.h"

#include "stack.S"

.set MPU_ADDR,0x68 // 7-bit address
.set MPU_ACC_REG_ADDR,0x3B

.bss

.global threshold,ax,ay,az,temp
threshold: .long 0
ax: .long 0	
ay: .long 0
az: .long 0
temp: .long 0

.global readmpu6050
.text

readmpu6050:
	move r1,MPU_ADDR
	push r1
	move r1,MPU_ACC_REG_ADDR
	push r1
	psr
	jump write8
	add r3,r3,3 // remove 3 arguments from stack
	move r0,r2 // test for error in r2
	jumpr fail,1,ge

	// Wait 1ms for sensor computation
	move r2,1
	psr
	jump waitMs

	move r1,MPU_ADDR
	push r1
	psr
	jump i2c_start_cond
	jump write_b
	jump i2c_read_byte
	add r3,r3,1 // remove call parameters from stack
	
	lsh r1,r0,8
	move r2,ax
	st r1,r2,0
	jump i2c_read_byte
	move r2,ax
	or r1,r2,r0
	move r2,ax
	st r1,r2,0

	jump i2c_read_byte
	lsh r1,r0,8
	move r2,ay
	st r1,r2,0
	jump i2c_read_byte
	move r2,ay
	or r1,r2,r0
	move r2,ay
	st r1,r2,0

	jump i2c_read_byte
	lsh r1,r0,8
	move r2,az
	st r1,r2,0
	jump i2c_read_byte
	move r2,az
	or r1,r2,r0
	move r2,az
	st r1,r2,0

fail:
	move r1,temp
	move r0,0 // 0 signals error
	st r0,r1,0
	ret
.global waitMs
waitMs:
	wait 8000
	sub r2,r2,1
	jump doneWaitMs,eq
	jump waitMs
doneWaitMs:
	ret
[/Codebox]
Top
Post Reply

Return to “ESP-IDF”

Who is online

Users browsing this forum: No registered users and 79 guests