diff --git a/components/flow3r_bsp/flow3r_bsp_imu.c b/components/flow3r_bsp/flow3r_bsp_imu.c index 6ca80ec4..e3946c91 100644 --- a/components/flow3r_bsp/flow3r_bsp_imu.c +++ b/components/flow3r_bsp/flow3r_bsp_imu.c @@ -35,7 +35,7 @@ static tildagon_mux_i2c_obj_t* mux; #define READ ( MP_MACHINE_I2C_FLAG_WRITE1 | MP_MACHINE_I2C_FLAG_READ | MP_MACHINE_I2C_FLAG_STOP ) #define WRITE MP_MACHINE_I2C_FLAG_STOP -static BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) { flow3r_bsp_imu_t *imu = (flow3r_bsp_imu_t *)intf_ptr; @@ -55,7 +55,7 @@ static BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, return BMI2_OK; } -static BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) { flow3r_bsp_imu_t *imu = (flow3r_bsp_imu_t *)intf_ptr; @@ -237,6 +237,20 @@ esp_err_t flow3r_bsp_imu_read_steps(flow3r_bsp_imu_t *imu, uint32_t *steps) { return ESP_ERR_NOT_FOUND; } +/*! + * @brief get the temperature of the device + */ +esp_err_t flow3r_bsp_imu_read_temperature(flow3r_bsp_imu_t *imu, float *temperature) { + uint16_t temp_data; + int8_t rslt = bmi2_get_temperature_data(&temp_data, &(imu->bmi)); + bmi2_error_codes_print_result(rslt); + if (rslt != BMI2_OK) return ESP_FAIL; + const float scaling = 0.001953125F; + const float offset = 23.0F; + *temperature = ((float)temp_data * scaling ) + offset; + return ESP_OK; +} + /*! * @brief Prints the execution status of the APIs. */ @@ -565,10 +579,6 @@ static int8_t set_accel_config(flow3r_bsp_imu_t *imu) { /* Set the accel configurations. */ rslt = bmi2_set_sensor_config(&config, 1, &imu->bmi); bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, &imu->bmi); - bmi2_error_codes_print_result(rslt); } return rslt; @@ -616,10 +626,6 @@ static int8_t set_gyro_config(flow3r_bsp_imu_t *imu) { rslt = bmi2_set_sensor_config(&config, 1, &imu->bmi); bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, &imu->bmi); - bmi2_error_codes_print_result(rslt); } return rslt; diff --git a/components/flow3r_bsp/flow3r_bsp_imu.h b/components/flow3r_bsp/flow3r_bsp_imu.h index 58351ff6..a3cdc1a4 100644 --- a/components/flow3r_bsp/flow3r_bsp_imu.h +++ b/components/flow3r_bsp/flow3r_bsp_imu.h @@ -66,4 +66,21 @@ esp_err_t flow3r_bsp_imu_read_gyro_dps(flow3r_bsp_imu_t *imu, float *x, // Returns ESP_ERR_NOT_FOUND if there is no new reading available. // Returns ESP_FAIL if the sensor could not be read (e.g. I2C unavailable). // Retrurns the number of steps counted. -esp_err_t flow3r_bsp_imu_read_steps(flow3r_bsp_imu_t *imu, uint32_t *steps); \ No newline at end of file +esp_err_t flow3r_bsp_imu_read_steps(flow3r_bsp_imu_t *imu, uint32_t *steps); + +// Get temperature +// +// Returns ESP_FAIL if the sensor could not be read (e.g. I2C unavailable). +// Returns temperature in 'C +esp_err_t flow3r_bsp_imu_read_temperature(flow3r_bsp_imu_t *imu, float *temperature); + +// i2c write +// writes buffer to register address +// Returns bmi fault code +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr,const uint8_t *reg_data, + uint32_t len, void *intf_ptr); +// i2c read +// reads buffer from register address +// Returns bmi fault code +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, + uint32_t len, void *intf_ptr); \ No newline at end of file diff --git a/components/st3m/st3m_imu.c b/components/st3m/st3m_imu.c index 0b433cce..7ae2ad8b 100644 --- a/components/st3m/st3m_imu.c +++ b/components/st3m/st3m_imu.c @@ -10,8 +10,6 @@ static const char *TAG = "st3m-imu"; #include "freertos/semphr.h" #include "freertos/task.h" -static void _task(void *data); - static flow3r_bsp_imu_t _imu; static SemaphoreHandle_t _mu; @@ -20,20 +18,20 @@ static SemaphoreHandle_t _mu; static float _acc_x, _acc_y, _acc_z; static float _gyro_x, _gyro_y, _gyro_z; +static float _temperature; static uint32_t _steps; -void st3m_imu_init() { +esp_err_t st3m_imu_init() { _mu = xSemaphoreCreateMutex(); assert(_mu != NULL); esp_err_t ret = flow3r_bsp_imu_init(&_imu); if (ret != ESP_OK) { ESP_LOGE(TAG, "IMU init failed: %s", esp_err_to_name(ret)); - return; + return ESP_FAIL; } - xTaskCreate(&_task, "imu", 4096, NULL, configMAX_PRIORITIES - 2, NULL); - ESP_LOGI(TAG, "IMU task started"); + return ESP_OK; } void st3m_imu_read_acc_mps(float *x, float *y, float *z) { @@ -58,10 +56,24 @@ void st3m_imu_read_steps(uint32_t *steps) { UNLOCK; } -static void _task(void *data) { +void st3m_imu_read_temperature(float *temperature) { + LOCK; + *temperature = _temperature; + UNLOCK; +} + +int st3m_imu_read(uint8_t reg_addr, uint8_t *reg_data, uint8_t len) { + return bmi2_i2c_read(reg_addr, reg_data, len, &_imu ); +} + +int st3m_imu_write(uint8_t reg_addr, uint8_t *reg_data, uint8_t len) { + return bmi2_i2c_write(reg_addr, reg_data, len, &_imu ); +} + +void st3m_imu_task(void *data) { TickType_t last_wake = xTaskGetTickCount(); esp_err_t ret; - float a, b, c; + float a, b, c, temperature; uint32_t steps; while (1) { vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(10)); // 100 Hz @@ -91,6 +103,11 @@ static void _task(void *data) { _steps = steps; } + ret = flow3r_bsp_imu_read_temperature(&_imu, &temperature); + if (ret == ESP_OK) { + _temperature = temperature; + } + UNLOCK; } } diff --git a/components/st3m/st3m_imu.h b/components/st3m/st3m_imu.h index f289c90b..e0dd69d7 100644 --- a/components/st3m/st3m_imu.h +++ b/components/st3m/st3m_imu.h @@ -5,9 +5,13 @@ #include #include -void st3m_imu_init(void); +esp_err_t st3m_imu_init(void); -void st3m_imu_read_acc_mps(float *x, float *y, float *z); -void st3m_imu_read_gyro_dps(float *x, float *y, float *z); -void st3m_imu_read_pressure(float *pressure, float *temperature); -void st3m_imu_read_steps(uint32_t *steps); +extern void st3m_imu_task(void *data); +extern void st3m_imu_read_acc_mps(float *x, float *y, float *z); +extern void st3m_imu_read_gyro_dps(float *x, float *y, float *z); +extern void st3m_imu_read_pressure(float *pressure, float *temperature); +extern void st3m_imu_read_steps(uint32_t *steps); +extern void st3m_imu_read_temperature(float *temperature); +extern int st3m_imu_read(uint8_t reg_addr, uint8_t *reg_data, uint8_t len); +extern int st3m_imu_write(uint8_t reg_addr, uint8_t *reg_data, uint8_t len); \ No newline at end of file diff --git a/drivers/micropython.cmake b/drivers/micropython.cmake index da9e56e0..d8ddc7fa 100644 --- a/drivers/micropython.cmake +++ b/drivers/micropython.cmake @@ -16,6 +16,9 @@ include(${CMAKE_CURRENT_LIST_DIR}/tildagon_i2c/tildagon_i2c.cmake) # Add PMIC and usb PD and micropython power bindings include(${CMAKE_CURRENT_LIST_DIR}/tildagon_power/tildagon_power.cmake) +# Add imu bindings +include(${CMAKE_CURRENT_LIST_DIR}/tildagon_imu/tildagon_imu.cmake) + # Add OTA helpers include(${CMAKE_CURRENT_LIST_DIR}/ota/micropython.cmake) diff --git a/drivers/tildagon_helpers/micropython.cmake b/drivers/tildagon_helpers/micropython.cmake index db7281d3..1ec8085e 100644 --- a/drivers/tildagon_helpers/micropython.cmake +++ b/drivers/tildagon_helpers/micropython.cmake @@ -11,19 +11,3 @@ target_sources(usermod_tildagon_helpers INTERFACE target_link_libraries(usermod INTERFACE usermod_tildagon_helpers) - -# Create an INTERFACE library for our C module. -add_library(usermod_imu INTERFACE) -# Add our source files to the lib -target_sources(usermod_imu INTERFACE - ${CMAKE_CURRENT_LIST_DIR}/mp_imu.c -) - -# Add the current directory as an include directory. -target_include_directories(usermod_imu INTERFACE - ${CMAKE_CURRENT_LIST_DIR} - ${CMAKE_CURRENT_LIST_DIR}/../../components/st3m -) - -# Link our INTERFACE library to the usermod target. -target_link_libraries(usermod INTERFACE usermod_imu) \ No newline at end of file diff --git a/drivers/tildagon_helpers/mp_imu.c b/drivers/tildagon_helpers/mp_imu.c deleted file mode 100644 index 20cdc551..00000000 --- a/drivers/tildagon_helpers/mp_imu.c +++ /dev/null @@ -1,55 +0,0 @@ -#include "st3m_imu.h" - -#include "py/builtin.h" -#include "py/runtime.h" - -static mp_obj_t mp_imu_acc_read(void) { - static float x, y, z; - - // Will not overwrite old data if there is an error - st3m_imu_read_acc_mps(&x, &y, &z); - - mp_obj_t items[3] = { mp_obj_new_float(x), mp_obj_new_float(y), - mp_obj_new_float(z) }; - return mp_obj_new_tuple(3, items); -} - -static MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_acc_read_obj, mp_imu_acc_read); - -static mp_obj_t mp_imu_gyro_read(void) { - static float x, y, z; - - // Will not overwrite old data if there is an error - st3m_imu_read_gyro_dps(&x, &y, &z); - - mp_obj_t items[3] = { mp_obj_new_float(x), mp_obj_new_float(y), - mp_obj_new_float(z) }; - return mp_obj_new_tuple(3, items); -} - -static MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_gyro_read_obj, mp_imu_gyro_read); - -static mp_obj_t mp_imu_step_counter_read(void) { - static uint32_t steps; - - st3m_imu_read_steps(&steps); - - return mp_obj_new_int_from_uint(steps); -} - -static MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_step_counter_read_obj, mp_imu_step_counter_read); - -static const mp_rom_map_elem_t globals_table[] = { - { MP_ROM_QSTR(MP_QSTR_acc_read), MP_ROM_PTR(&mp_imu_acc_read_obj) }, - { MP_ROM_QSTR(MP_QSTR_gyro_read), MP_ROM_PTR(&mp_imu_gyro_read_obj) }, - { MP_ROM_QSTR(MP_QSTR_step_counter_read), MP_ROM_PTR(&mp_imu_step_counter_read_obj) }, -}; - -static MP_DEFINE_CONST_DICT(globals, globals_table); - -const mp_obj_module_t mp_module_imu_user_cmodule = { - .base = { &mp_type_module }, - .globals = (mp_obj_dict_t *)&globals, -}; - -MP_REGISTER_MODULE(MP_QSTR_imu, mp_module_imu_user_cmodule); diff --git a/drivers/tildagon_imu/lsm6ds3/lsm6ds3.c b/drivers/tildagon_imu/lsm6ds3/lsm6ds3.c new file mode 100644 index 00000000..14911085 --- /dev/null +++ b/drivers/tildagon_imu/lsm6ds3/lsm6ds3.c @@ -0,0 +1,204 @@ +#include "lsm6ds3.h" + +#include "tildagon_i2c.h" +#include "esp_err.h" +#include "freertos/semphr.h" + +/* Accelerometer and gyroscope control registers */ +#define CTRL1_XL 0x10 +#define CTRL3_C 0x12 +#define CTRL10_C 0x19 +/* Temperature output data registers */ +#define OUT_TEMP_L 0x20 +/* Step counter output registers */ +#define STEP_COUNTER_L 0x4B + +static esp_err_t reset( void ); + +#define ADDRESS 0x6B +#define READ ( MP_MACHINE_I2C_FLAG_WRITE1 | MP_MACHINE_I2C_FLAG_READ | MP_MACHINE_I2C_FLAG_STOP ) +#define WRITE MP_MACHINE_I2C_FLAG_STOP + +static float acc_x = 0.0F; +static float acc_y = 0.0F; +static float acc_z = 0.0F; +static float gyro_x = 0.0F; +static float gyro_y = 0.0F; +static float gyro_z = 0.0F; +static float _temperature = 0.0F; +static uint32_t _steps = 0U; +tildagon_mux_i2c_obj_t* mux_port; + +static SemaphoreHandle_t _mu; +#define LOCK xSemaphoreTake(_mu, portMAX_DELAY) +#define UNLOCK xSemaphoreGive(_mu) + +/** + * @brief initialise lsm6ds3 + * @details setup the lsm6ds3 2g, 2000dps, 104Hz + * @return esp_err_t expect ESP_OK or ESP_FAIL + */ +esp_err_t lsm6ds3_init( void ) +{ + _mu = xSemaphoreCreateMutex(); + assert(_mu != NULL); + + esp_err_t err = ESP_FAIL; + mux_port = tildagon_get_mux_obj( 7 ); + if (reset() >= 0) + { + /* 2 g accel range, 104Hz, 2000 dps gyro range */ + uint8_t write_buffer[3] = { CTRL1_XL, 0x41, 0x4C }; + mp_machine_i2c_buf_t buffer = { .len = 3, .buf = write_buffer }; + tildagon_mux_i2c_transaction( mux_port, ADDRESS, 1, &buffer, WRITE ); + /* enable step count */ + write_buffer[0] = CTRL10_C; + /* PEDO_EN and FUNC_EN */ + write_buffer[1] = 0x14; + tildagon_mux_i2c_transaction( mux_port, ADDRESS, 1, &buffer, WRITE ); + err = ESP_OK; + } + return err; +} + +/** + * @brief get accelerometer data + * @param x pointer for x axis data + * @param y pointer for y axis data + * @param z pointer for z axis data + */ +void lsm6ds3_read_acc_mps(float *x, float *y, float *z) +{ + LOCK; + *x = acc_x; + *y = acc_y; + *z = acc_z; + UNLOCK; +} + +/** + * @brief get Gyroscope data + * @param x pointer for x axis data + * @param y pointer for y axis data + * @param z pointer for z axis data + */ +void lsm6ds3_read_gyro_dps(float *x, float *y, float *z) +{ + LOCK; + *x = gyro_x; + *y = gyro_y; + *z = gyro_z; + UNLOCK; +} + +/** + * @brief get step count + * @param steps pointer for data + */ +void lsm6ds3_read_steps(uint32_t *steps) +{ + LOCK; + *steps = _steps; + _steps = 0; + UNLOCK; +} + +/** + * @brief get temperature + * @param temperature pointer for data + */ +void lsm6ds3_read_temperature(float *temperature) +{ + LOCK; + *temperature = _temperature; + UNLOCK; +} + +/** + * @brief raw i2c write access + * @param reg_addr address to write + * @param reg_data data + * @param len length of data + */ +int lsm6ds3_write(uint8_t reg_addr, uint8_t *reg_data, uint8_t len ) +{ + uint8_t data[len+1]; + data[0] = reg_addr; + for (uint8_t i = 0; i= 0) + { + LOCK; + _temperature = (((float)((int16_t)(read_buffer[0] + ( (uint16_t)read_buffer[1] << 8 )))) * 0.001953125F) + 23.0F; + const float gyroscaling = (2000.0F / 32768.0F); + gyro_x = ((float)((int16_t)( read_buffer[2] + ( (uint16_t)read_buffer[3] << 8 ) ))) * gyroscaling; + gyro_y = ((float)((int16_t)( read_buffer[4] + ( (uint16_t)read_buffer[5] << 8 ) ))) * gyroscaling; + gyro_z = ((float)((int16_t)( read_buffer[6] + ( (uint16_t)read_buffer[7] << 8 ) ))) * gyroscaling; + /* 2g fsd, 1g = 9.80665m/s */ + const float accelscaling = (2.0F * 9.80665F) / 32768.0F; + acc_x = ((float)((int16_t)( read_buffer[8] + ( (uint16_t)read_buffer[9] << 8 ) ))) * accelscaling; + acc_y = ((float)((int16_t)( read_buffer[10] + ( (uint16_t)read_buffer[11] << 8 ) ))) * accelscaling; + acc_z = ((float)((int16_t)( read_buffer[12] + ( (uint16_t)read_buffer[13] << 8 ) ))) * accelscaling; + UNLOCK; + } + write_buffer[0] = STEP_COUNTER_L; + buffer[1].len = 2; + ret = tildagon_mux_i2c_transaction( mux_port, ADDRESS, 2, buffer, READ ); + if (ret >= 0) + { + LOCK; + _steps += read_buffer[0] + ( (uint16_t)read_buffer[1] << 8 ); + /* reset step count */ + buffer[0].len = 2; + write_buffer[0] = CTRL10_C; + tildagon_mux_i2c_transaction( mux_port, ADDRESS, 1, buffer, WRITE ); + UNLOCK; + } + } +} + +/** + * @brief reset + * @details perform a software reset + * @return esp_err_t + */ +esp_err_t reset() +{ + uint8_t write_buffer[2] = { CTRL3_C, 0x01 }; + mp_machine_i2c_buf_t buffer = { .len = 2, .buf = write_buffer }; + return tildagon_mux_i2c_transaction( mux_port, ADDRESS, 1, &buffer, WRITE ); +} diff --git a/drivers/tildagon_imu/lsm6ds3/lsm6ds3.h b/drivers/tildagon_imu/lsm6ds3/lsm6ds3.h new file mode 100644 index 00000000..1ef898bb --- /dev/null +++ b/drivers/tildagon_imu/lsm6ds3/lsm6ds3.h @@ -0,0 +1,14 @@ +#ifndef LSM6DS3_H +#define LSM6DS3_H + +#include + +extern int lsm6ds3_init( void ); +extern void lsm6ds3_task(void *data); +extern void lsm6ds3_read_acc_mps(float *x, float *y, float *z); +extern void lsm6ds3_read_gyro_dps(float *x, float *y, float *z); +extern void lsm6ds3_read_steps(uint32_t *steps); +extern void lsm6ds3_read_temperature(float *temperature); +extern int lsm6ds3_write(uint8_t reg_addr, uint8_t *reg_data, uint8_t len ); +extern int lsm6ds3_read(uint8_t reg_addr, uint8_t *reg_data, uint8_t len ); +#endif /* LSM6DS3_H */ \ No newline at end of file diff --git a/drivers/tildagon_imu/mp_imu.c b/drivers/tildagon_imu/mp_imu.c new file mode 100644 index 00000000..596a3d7c --- /dev/null +++ b/drivers/tildagon_imu/mp_imu.c @@ -0,0 +1,115 @@ + +#include "tildagon_imu.h" + +#include "py/builtin.h" +#include "py/runtime.h" +#include "esp_err.h" +#include + +static mp_obj_t mp_imu_acc_read(void) { + static float x, y, z; + + // Will not overwrite old data if there is an error + tildagon_imu_acc_read(&x, &y, &z); + + mp_obj_t items[3] = { mp_obj_new_float(x), mp_obj_new_float(y), + mp_obj_new_float(z) }; + return mp_obj_new_tuple(3, items); +} + +static MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_acc_read_obj, mp_imu_acc_read); + +static mp_obj_t mp_imu_gyro_read(void) { + static float x, y, z; + + // Will not overwrite old data if there is an error + tildagon_imu_gyro_read(&x, &y, &z); + + mp_obj_t items[3] = { mp_obj_new_float(x), mp_obj_new_float(y), + mp_obj_new_float(z) }; + return mp_obj_new_tuple(3, items); +} + +static MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_gyro_read_obj, mp_imu_gyro_read); + +static mp_obj_t mp_imu_step_counter_read(void) { + static uint32_t steps; + + tildagon_imu_step_counter_read(&steps); + + return mp_obj_new_int_from_uint(steps); +} + +static MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_step_counter_read_obj, mp_imu_step_counter_read); + +static mp_obj_t mp_imu_temperature_read(void) { + static float temperature; + + tildagon_imu_temperature_read(&temperature); + + return mp_obj_new_float_from_f(temperature); +} + +static MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_temperature_read_obj, mp_imu_temperature_read); + +static mp_obj_t mp_imu_id(void) { + static char* id; + id = tildagon_imu_get_id(); + return mp_obj_new_str(id, strlen(id)); +} + +static MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_id_obj, mp_imu_id); + +static mp_obj_t mp_imu_read_from(mp_obj_t reg_address, mp_obj_t length ) { + uint8_t address = mp_obj_get_int(reg_address); + uint8_t len = mp_obj_get_int(length); + uint8_t buffer[len]; + int err = tildagon_imu_read(address, len, buffer); + if ( err < 0 ) + { + return mp_obj_new_int(err); + } + else + { + mp_obj_t buf = mp_obj_new_bytes(buffer, len); + return buf; + } +} + +static MP_DEFINE_CONST_FUN_OBJ_2(mp_imu_read_from_obj, mp_imu_read_from); + +static mp_obj_t mp_imu_write_to(mp_obj_t reg_address, mp_obj_t buffer ) { + uint8_t address = mp_obj_get_int(reg_address); + mp_buffer_info_t bufinfo; + mp_get_buffer_raise(buffer, &bufinfo, MP_BUFFER_READ); + int err = tildagon_imu_write(address, (uint8_t)bufinfo.len, (uint8_t *)bufinfo.buf); + if ( err < 0 ) + { + return mp_obj_new_int(err); + } + else + { + return mp_const_none; + } +} + +static MP_DEFINE_CONST_FUN_OBJ_2(mp_imu_write_to_obj, mp_imu_write_to); + +static const mp_rom_map_elem_t globals_table[] = { + { MP_ROM_QSTR(MP_QSTR_acc_read), MP_ROM_PTR(&mp_imu_acc_read_obj) }, + { MP_ROM_QSTR(MP_QSTR_gyro_read), MP_ROM_PTR(&mp_imu_gyro_read_obj) }, + { MP_ROM_QSTR(MP_QSTR_step_counter_read), MP_ROM_PTR(&mp_imu_step_counter_read_obj) }, + { MP_ROM_QSTR(MP_QSTR_temperature_read), MP_ROM_PTR(&mp_imu_temperature_read_obj) }, + { MP_ROM_QSTR(MP_QSTR_id), MP_ROM_PTR(&mp_imu_id_obj) }, + { MP_ROM_QSTR(MP_QSTR_readfrom), MP_ROM_PTR(&mp_imu_read_from_obj) }, + { MP_ROM_QSTR(MP_QSTR_writeto), MP_ROM_PTR(&mp_imu_write_to_obj) }, +}; + +static MP_DEFINE_CONST_DICT(globals, globals_table); + +const mp_obj_module_t mp_module_imu_user_cmodule = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t *)&globals, +}; + +MP_REGISTER_MODULE(MP_QSTR_imu, mp_module_imu_user_cmodule); diff --git a/drivers/tildagon_imu/tildagon_imu.c b/drivers/tildagon_imu/tildagon_imu.c new file mode 100644 index 00000000..1e61d742 --- /dev/null +++ b/drivers/tildagon_imu/tildagon_imu.c @@ -0,0 +1,183 @@ + +#include "esp_err.h" +#include "esp_log.h" +#include "py/mperrno.h" +#include "freertos/FreeRTOS.h" +#include "freertos/semphr.h" +#include "freertos/task.h" +#include "st3m_imu.h" +#include "lsm6ds3.h" + +typedef enum +{ + ST3M, + LSM6DS3, + /* add new imu type here */ + MAX_DEVICES, +} which_imu_t; + +typedef void (*updatefuncptr_t) ( void* data ); +typedef void (*accfuncptr_t) ( float* x, float*y, float*z ); +typedef void (*gyrfuncptr_t) ( float* x, float*y, float*z ); +typedef void (*stepfuncptr_t) ( uint32_t* steps ); +typedef void (*tempfuncptr_t) ( float* temperature ); +typedef int (*i2cfuncptr_t) ( uint8_t reg_addr, uint8_t *reg_data, uint8_t len ); + +i2cfuncptr_t i2c_write[MAX_DEVICES] = +{ + /* ST3M */ st3m_imu_write, + /* LSM6DS3 */ lsm6ds3_write, +}; + +i2cfuncptr_t i2c_read[MAX_DEVICES] = +{ + /* ST3M */ st3m_imu_read, + /* LSM6DS3 */ lsm6ds3_read, +}; + +updatefuncptr_t update[MAX_DEVICES] = +{ + /* ST3M */ st3m_imu_task, + /* LSM6DS3 */ lsm6ds3_task, +}; + +accfuncptr_t accel_read[MAX_DEVICES] = +{ + /* ST3M */ st3m_imu_read_acc_mps, + /* LSM6DS3 */ lsm6ds3_read_acc_mps, +}; + +gyrfuncptr_t gyro_read[MAX_DEVICES] = +{ + /* ST3M */ st3m_imu_read_gyro_dps, + /* LSM6DS3 */ lsm6ds3_read_gyro_dps, +}; + +stepfuncptr_t step_read[MAX_DEVICES] = +{ + /* ST3M */ st3m_imu_read_steps, + /* LSM6DS3 */ lsm6ds3_read_steps, +}; + +tempfuncptr_t temp_read[MAX_DEVICES] = +{ + /* ST3M */ st3m_imu_read_temperature, + /* LSM6DS3 */ lsm6ds3_read_temperature, +}; + +static char st3m_id[] = "bmi270"; +static char lsm6ds3_id[] = "lsm6ds3"; +static char* id_list[MAX_DEVICES] = +{ + /* ST3M */ st3m_id, + /* LSM6DS3 */ lsm6ds3_id, +}; + +which_imu_t imu = MAX_DEVICES; + +void tildagon_imu_init( void ) +{ + if ( st3m_imu_init() == ESP_OK ) + { + imu = ST3M; + } + else if ( lsm6ds3_init() == ESP_OK ) + { + imu = LSM6DS3; + } + + if ( imu < MAX_DEVICES ) + { + /* create task */ + xTaskCreate( update[imu], "imu", 4096, NULL, configMAX_PRIORITIES - 2, NULL); + } +} + +void tildagon_imu_acc_read( float* x, float*y, float*z ) +{ + if ( imu < MAX_DEVICES) + { + ( *accel_read[imu] )( x, y, z ); + } + else + { + *x = 1.0F; + *y = 1.0F; + *z = 1.0F; + } +} + +void tildagon_imu_gyro_read( float* x, float*y, float*z ) +{ + if ( imu < MAX_DEVICES) + { + ( *gyro_read[imu] )( x, y, z ); + } + else + { + *x = 1.0F; + *y = 1.0F; + *z = 1.0F; + } +} + +void tildagon_imu_step_counter_read( uint32_t* steps ) +{ + if ( imu < MAX_DEVICES) + { + ( *step_read[imu] )( steps ); + } + else + { + *steps = 0xFFFFFFFF; + } +} + +void tildagon_imu_temperature_read( float* temperature ) +{ + if ( imu < MAX_DEVICES) + { + ( *temp_read[imu] )( temperature ); + } + else + { + *temperature = -274.0F; + } +} + +char* tildagon_imu_get_id( void ) +{ + if ( imu < MAX_DEVICES ) + { + return id_list[imu]; + } + else + { + static char no_device[] = "no device present"; + return no_device; + } +} + +int tildagon_imu_write( uint8_t address, uint8_t length, uint8_t* buffer ) +{ + if ( imu < MAX_DEVICES ) + { + return ( *i2c_write[imu] )(address, buffer, length ); + } + else + { + return -MP_ENODEV; + } +} + +int tildagon_imu_read( uint8_t address, uint8_t length, uint8_t* buffer ) +{ + if ( imu < MAX_DEVICES ) + { + return ( *i2c_read[imu] )(address, buffer, length ); + } + else + { + return -MP_ENODEV; + } +} diff --git a/drivers/tildagon_imu/tildagon_imu.cmake b/drivers/tildagon_imu/tildagon_imu.cmake new file mode 100644 index 00000000..9aa3ae0c --- /dev/null +++ b/drivers/tildagon_imu/tildagon_imu.cmake @@ -0,0 +1,21 @@ +# Create an INTERFACE library for our C module. +add_library(usermod_tildagon_imu INTERFACE) + + +# Add our source files to the lib +target_sources(usermod_tildagon_imu INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/tildagon_imu.c + ${CMAKE_CURRENT_LIST_DIR}/lsm6ds3/lsm6ds3.c + ${CMAKE_CURRENT_LIST_DIR}/mp_imu.c +) + +# Add the current directory as an include directory. +target_include_directories(usermod_tildagon_imu INTERFACE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/lsm6ds3 + ${CMAKE_CURRENT_LIST_DIR}/../../components/st3m +) + +# Link our INTERFACE library to the usermod target. +target_link_libraries(usermod INTERFACE usermod_tildagon_imu) + diff --git a/drivers/tildagon_imu/tildagon_imu.h b/drivers/tildagon_imu/tildagon_imu.h new file mode 100644 index 00000000..0e95ddeb --- /dev/null +++ b/drivers/tildagon_imu/tildagon_imu.h @@ -0,0 +1,14 @@ +#ifndef TILDAGON_IMU_H +#define TILDAGON_IMU_H +#include "stdint.h" + +extern void tildagon_imu_init( void ); +extern void tildagon_imu_acc_read( float* x, float*y, float*z ); +extern void tildagon_imu_gyro_read( float* x, float*y, float*z ); +extern void tildagon_imu_step_counter_read( uint32_t* steps ); +extern void tildagon_imu_temperature_read( float* temperature ); +extern char* tildagon_imu_get_id( void ); +extern int tildagon_imu_write( uint8_t address, uint8_t length, uint8_t* buffer ); +extern int tildagon_imu_read( uint8_t address, uint8_t length, uint8_t* buffer ); + +#endif diff --git a/tildagon/board_init.c b/tildagon/board_init.c index c8223b87..2904e7c0 100644 --- a/tildagon/board_init.c +++ b/tildagon/board_init.c @@ -1,6 +1,7 @@ #include "st3m_imu.h" #include "tildagon_power.h" +#include "tildagon_imu.h" // This is the default startup handler for ESP32, does VFS and stuff void boardctrl_startup(void); @@ -15,7 +16,8 @@ void tildagon_i2c_init(void); void tildagon_pins_init(void); -void tildagon_startup(void) { +void tildagon_startup(void) +{ // call the micropy default startup - does VFS init on ESP32 boardctrl_startup(); @@ -27,7 +29,6 @@ void tildagon_startup(void) { tildagon_usb_init(); - - st3m_imu_init(); + tildagon_imu_init(); }