|
11 | 11 | #include "py/obj.h" |
12 | 12 | #include "py/runtime.h" |
13 | 13 |
|
| 14 | +#if PYBRICKS_HUB_CPLUSHUB |
| 15 | + |
| 16 | +#include <lsm6ds3tr_c_reg.h> |
| 17 | +#include <stm32l4xx_hal.h> |
| 18 | +#include <stm32l4xx_ll_i2c.h> |
| 19 | + |
| 20 | +typedef struct { |
| 21 | + mp_obj_base_t base; |
| 22 | + stmdev_ctx_t ctx; |
| 23 | +} mod_experimental_IMU_obj_t; |
| 24 | + |
| 25 | +STATIC I2C_HandleTypeDef hi2c; |
| 26 | + |
| 27 | +void mod_experimental_IMU_handle_i2c_er_irq() { |
| 28 | + HAL_I2C_ER_IRQHandler(&hi2c); |
| 29 | +} |
| 30 | + |
| 31 | +void mod_experimental_IMU_handle_i2c_ev_irq() { |
| 32 | + HAL_I2C_EV_IRQHandler(&hi2c); |
| 33 | +} |
| 34 | + |
| 35 | +STATIC int32_t mod_experimental_IMU_write_reg(void *handle, uint8_t reg, uint8_t *data, uint16_t len) { |
| 36 | + HAL_StatusTypeDef ret; |
| 37 | + |
| 38 | + ret = HAL_I2C_Mem_Write(&hi2c, LSM6DS3TR_C_I2C_ADD_L, reg, I2C_MEMADD_SIZE_8BIT, data, len, 500); |
| 39 | + |
| 40 | + return ret; |
| 41 | +} |
| 42 | + |
| 43 | +STATIC int32_t mod_experimental_IMU_read_reg(void *handle, uint8_t reg, uint8_t *data, uint16_t len) { |
| 44 | + HAL_StatusTypeDef ret; |
| 45 | + |
| 46 | + ret = HAL_I2C_Mem_Read(&hi2c, LSM6DS3TR_C_I2C_ADD_L, reg, I2C_MEMADD_SIZE_8BIT, data, len, 500); |
| 47 | + |
| 48 | + return ret; |
| 49 | +} |
| 50 | + |
| 51 | +STATIC mp_obj_t mod_experimental_IMU_make_new(const mp_obj_type_t *otype, size_t n_args, size_t n_kw, const mp_obj_t *args) { |
| 52 | + mod_experimental_IMU_obj_t *self = m_new_obj(mod_experimental_IMU_obj_t); |
| 53 | + |
| 54 | + self->base.type = (mp_obj_type_t *)otype; |
| 55 | + self->ctx.write_reg = mod_experimental_IMU_write_reg; |
| 56 | + self->ctx.read_reg = mod_experimental_IMU_read_reg; |
| 57 | + |
| 58 | + if (hi2c.Instance == NULL) { |
| 59 | + hi2c.Instance = I2C1; |
| 60 | + // Clock is 5MHz, so these timing come out to 1 usec. When combined with |
| 61 | + // internal delays, this is slightly slower than 400kHz |
| 62 | + hi2c.Init.Timing = __LL_I2C_CONVERT_TIMINGS(0, 0, 0, 4, 4); |
| 63 | + hi2c.Init.OwnAddress1 = 0; |
| 64 | + hi2c.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; |
| 65 | + hi2c.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; |
| 66 | + hi2c.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; |
| 67 | + hi2c.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; |
| 68 | + HAL_StatusTypeDef ret = HAL_I2C_Init(&hi2c); |
| 69 | + if (ret != HAL_OK) { |
| 70 | + hi2c.Instance = NULL; |
| 71 | + mp_raise_msg(&mp_type_RuntimeError, "failed to init I2C"); |
| 72 | + } |
| 73 | + } |
| 74 | + |
| 75 | + HAL_StatusTypeDef ret = HAL_I2C_IsDeviceReady(&hi2c, LSM6DS3TR_C_I2C_ADD_L, 10, 200); |
| 76 | + if (ret != HAL_OK) { |
| 77 | + mp_raise_msg(&mp_type_RuntimeError, "device is not ready"); |
| 78 | + } |
| 79 | + |
| 80 | + uint8_t id; |
| 81 | + if (lsm6ds3tr_c_device_id_get(&self->ctx, &id) != 0) { |
| 82 | + mp_raise_msg(&mp_type_RuntimeError, "failed to get device id"); |
| 83 | + } |
| 84 | + |
| 85 | + if (id != LSM6DS3TR_C_ID) { |
| 86 | + mp_raise_msg(&mp_type_RuntimeError, "incorrect device id"); |
| 87 | + } |
| 88 | + |
| 89 | + // Init based on data polling example |
| 90 | + |
| 91 | + /* |
| 92 | + * Restore default configuration |
| 93 | + */ |
| 94 | + uint8_t rst; |
| 95 | + lsm6ds3tr_c_reset_set(&self->ctx, PROPERTY_ENABLE); |
| 96 | + do { |
| 97 | + lsm6ds3tr_c_reset_get(&self->ctx, &rst); |
| 98 | + } while (rst); |
| 99 | + /* |
| 100 | + * Enable Block Data Update |
| 101 | + */ |
| 102 | + lsm6ds3tr_c_block_data_update_set(&self->ctx, PROPERTY_ENABLE); |
| 103 | + /* |
| 104 | + * Set Output Data Rate |
| 105 | + */ |
| 106 | + lsm6ds3tr_c_xl_data_rate_set(&self->ctx, LSM6DS3TR_C_XL_ODR_12Hz5); |
| 107 | + lsm6ds3tr_c_gy_data_rate_set(&self->ctx, LSM6DS3TR_C_GY_ODR_12Hz5); |
| 108 | + /* |
| 109 | + * Set full scale |
| 110 | + */ |
| 111 | + lsm6ds3tr_c_xl_full_scale_set(&self->ctx, LSM6DS3TR_C_2g); |
| 112 | + lsm6ds3tr_c_gy_full_scale_set(&self->ctx, LSM6DS3TR_C_2000dps); |
| 113 | + |
| 114 | + /* |
| 115 | + * Configure filtering chain(No aux interface) |
| 116 | + */ |
| 117 | + /* Accelerometer - analog filter */ |
| 118 | + lsm6ds3tr_c_xl_filter_analog_set(&self->ctx, LSM6DS3TR_C_XL_ANA_BW_400Hz); |
| 119 | + |
| 120 | + /* Accelerometer - LPF1 path ( LPF2 not used )*/ |
| 121 | + // lsm6ds3tr_c_xl_lp1_bandwidth_set(&self->ctx, LSM6DS3TR_C_XL_LP1_ODR_DIV_4); |
| 122 | + |
| 123 | + /* Accelerometer - LPF1 + LPF2 path */ |
| 124 | + lsm6ds3tr_c_xl_lp2_bandwidth_set(&self->ctx, LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100); |
| 125 | + |
| 126 | + /* Accelerometer - High Pass / Slope path */ |
| 127 | + // lsm6ds3tr_c_xl_reference_mode_set(&self->ctx, PROPERTY_DISABLE); |
| 128 | + // lsm6ds3tr_c_xl_hp_bandwidth_set(&self->ctx, LSM6DS3TR_C_XL_HP_ODR_DIV_100); |
| 129 | + |
| 130 | + /* Gyroscope - filtering chain */ |
| 131 | + lsm6ds3tr_c_gy_band_pass_set(&self->ctx, LSM6DS3TR_C_HP_260mHz_LP1_STRONG); |
| 132 | + |
| 133 | + return MP_OBJ_FROM_PTR(self); |
| 134 | +} |
| 135 | + |
| 136 | +STATIC mp_obj_t mod_experimental_IMU_accel(mp_obj_t self_in) { |
| 137 | + mod_experimental_IMU_obj_t *self = MP_OBJ_TO_PTR(self_in); |
| 138 | + int16_t data[3]; |
| 139 | + |
| 140 | + if (lsm6ds3tr_c_acceleration_raw_get(&self->ctx, (uint8_t *)data) != 0) { |
| 141 | + mp_raise_msg(&mp_type_RuntimeError, "failed to read data"); |
| 142 | + } |
| 143 | + |
| 144 | + mp_obj_t values[3]; |
| 145 | + values[0] = mp_obj_new_float_from_f(lsm6ds3tr_c_from_fs2g_to_mg(data[0]) / 1000.0f); |
| 146 | + values[1] = mp_obj_new_float_from_f(lsm6ds3tr_c_from_fs2g_to_mg(data[1]) / 1000.0f); |
| 147 | + values[2] = mp_obj_new_float_from_f(lsm6ds3tr_c_from_fs2g_to_mg(data[2]) / 1000.0f); |
| 148 | + |
| 149 | + return mp_obj_new_tuple(3, values); |
| 150 | +} |
| 151 | +STATIC MP_DEFINE_CONST_FUN_OBJ_1(mod_experimental_IMU_accel_obj, mod_experimental_IMU_accel); |
| 152 | + |
| 153 | +STATIC mp_obj_t mod_experimental_IMU_gyro(mp_obj_t self_in) { |
| 154 | + mod_experimental_IMU_obj_t *self = MP_OBJ_TO_PTR(self_in); |
| 155 | + int16_t data[3]; |
| 156 | + |
| 157 | + if (lsm6ds3tr_c_angular_rate_raw_get(&self->ctx, (uint8_t *)data) != 0) { |
| 158 | + mp_raise_msg(&mp_type_RuntimeError, "failed to read data"); |
| 159 | + } |
| 160 | + |
| 161 | + mp_obj_t values[3]; |
| 162 | + values[0] = mp_obj_new_float_from_f(lsm6ds3tr_c_from_fs2000dps_to_mdps(data[0]) / 1000.0f); |
| 163 | + values[1] = mp_obj_new_float_from_f(lsm6ds3tr_c_from_fs2000dps_to_mdps(data[1]) / 1000.0f); |
| 164 | + values[2] = mp_obj_new_float_from_f(lsm6ds3tr_c_from_fs2000dps_to_mdps(data[2]) / 1000.0f); |
| 165 | + |
| 166 | + return mp_obj_new_tuple(3, values); |
| 167 | +} |
| 168 | +STATIC MP_DEFINE_CONST_FUN_OBJ_1(mod_experimental_IMU_gyro_obj, mod_experimental_IMU_gyro); |
| 169 | + |
| 170 | +STATIC const mp_rom_map_elem_t mod_experimental_IMU_locals_dict_table[] = { |
| 171 | + { MP_ROM_QSTR(MP_QSTR_accel), MP_ROM_PTR(&mod_experimental_IMU_accel_obj) }, |
| 172 | + { MP_ROM_QSTR(MP_QSTR_gyro), MP_ROM_PTR(&mod_experimental_IMU_gyro_obj) }, |
| 173 | +}; |
| 174 | +STATIC MP_DEFINE_CONST_DICT(mod_experimental_IMU_locals_dict, mod_experimental_IMU_locals_dict_table); |
| 175 | + |
| 176 | +STATIC const mp_obj_type_t mod_experimental_IMU_type = { |
| 177 | + { &mp_type_type }, |
| 178 | + .name = MP_QSTR_IMU, |
| 179 | + .make_new = mod_experimental_IMU_make_new, |
| 180 | + .locals_dict = (mp_obj_dict_t *)&mod_experimental_IMU_locals_dict, |
| 181 | +}; |
| 182 | + |
| 183 | +#endif // PYBRICKS_HUB_CPLUSHUB |
| 184 | + |
14 | 185 | #if PYBRICKS_HUB_EV3 |
15 | 186 | #if !MICROPY_MODULE_BUILTIN_INIT |
16 | 187 | #error "pybricks.experimental module requires that MICROPY_MODULE_BUILTIN_INIT is enabled" |
@@ -47,6 +218,9 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_2(mod_experimental_pthread_raise_obj, mod_experim |
47 | 218 |
|
48 | 219 | STATIC const mp_rom_map_elem_t mod_experimental_globals_table[] = { |
49 | 220 | { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_experimental_c) }, |
| 221 | + #if PYBRICKS_HUB_CPLUSHUB |
| 222 | + { MP_ROM_QSTR(MP_QSTR_IMU), MP_ROM_PTR(&mod_experimental_IMU_type) }, |
| 223 | + #endif // PYBRICKS_HUB_CPLUSHUB |
50 | 224 | #if PYBRICKS_HUB_EV3 |
51 | 225 | { MP_ROM_QSTR(MP_QSTR___init__), MP_ROM_PTR(&mod_experimental___init___obj) }, |
52 | 226 | { MP_ROM_QSTR(MP_QSTR_pthread_raise), MP_ROM_PTR(&mod_experimental_pthread_raise_obj) }, |
|
0 commit comments