| /* |
| * Licensed to the Apache Software Foundation (ASF) under one |
| * or more contributor license agreements. See the NOTICE file |
| * distributed with this work for additional information |
| * resarding copyright ownership. The ASF licenses this file |
| * to you under the Apache License, Version 2.0 (the |
| * "License"); you may not use this file except in compliance |
| * with the License. You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, |
| * software distributed under the License is distributed on an |
| * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY |
| * KIND, either express or implied. See the License for the |
| * specific language governing permissions and limitations |
| * under the License. |
| */ |
| |
| #include <string.h> |
| #include <errno.h> |
| #include <assert.h> |
| |
| #include "os/mynewt.h" |
| #include "hal/hal_i2c.h" |
| #include "hal/hal_gpio.h" |
| #include "i2cn/i2cn.h" |
| #include "drv2605/drv2605.h" |
| #include "drv2605_priv.h" |
| #include <syscfg/syscfg.h> |
| |
| #if MYNEWT_VAL(DRV2605_LOG) |
| #include "modlog/modlog.h" |
| #endif |
| |
| #if MYNEWT_VAL(DRV2605_STATS) |
| #include "stats/stats.h" |
| #endif |
| |
| #if MYNEWT_VAL(DRV2605_STATS) |
| /* Define the stats section and records */ |
| STATS_SECT_START(drv2605_stat_section) |
| STATS_SECT_ENTRY(errors) |
| STATS_SECT_END |
| |
| /* Define stat names for querying */ |
| STATS_NAME_START(drv2605_stat_section) |
| STATS_NAME(drv2605_stat_section, errors) |
| STATS_NAME_END(drv2605_stat_section) |
| |
| /* Global variable used to hold stats data */ |
| STATS_SECT_DECL(drv2605_stat_section) g_drv2605stats; |
| #endif |
| |
| #if MYNEWT_VAL(DRV2605_LOG) |
| #define DRV2605_LOG(lvl_, ...) \ |
| MODLOG_ ## lvl_(MYNEWT_VAL(DRV2605_LOG_MODULE), __VA_ARGS__) |
| #else |
| #define DRV2605_LOG(lvl_, ...) |
| #endif |
| |
| /** |
| * Writes a single byte to the specified register |
| * |
| * @param The Sensor interface |
| * @param The register address to write to |
| * @param The value to write |
| * |
| * @return 0 on success, non-zero error on failure. |
| */ |
| int |
| drv2605_write8(struct sensor_itf *itf, uint8_t reg, uint8_t value) |
| { |
| int rc; |
| uint8_t payload[2] = { reg, value}; |
| |
| struct hal_i2c_master_data data_struct = { |
| .address = itf->si_addr, |
| .len = 2, |
| .buffer = payload |
| }; |
| |
| rc = sensor_itf_lock(itf, MYNEWT_VAL(DRV2605_ITF_LOCK_TMO)); |
| if (rc) { |
| return rc; |
| } |
| |
| rc = i2cn_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC, 1, |
| MYNEWT_VAL(DRV2605_I2C_RETRIES)); |
| if (rc) { |
| DRV2605_LOG(ERROR, |
| "Failed to write to 0x%02X:0x%02X with value 0x%02X\n", |
| data_struct.address, reg, value); |
| #if MYNEWT_VAL(DRV2605_STATS) |
| STATS_INC(g_drv2605stats, errors); |
| #endif |
| goto err; |
| } |
| |
| err: |
| sensor_itf_unlock(itf); |
| return rc; |
| } |
| |
| /** |
| * Writes multiple bytes starting at the specified register (MAX: 8 bytes) |
| * |
| * @param The Sensor interface |
| * @param The register address to write to |
| * @param The data buffer to write from |
| * |
| * @return 0 on success, non-zero error on failure. |
| */ |
| int |
| drv2605_writelen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer, |
| uint8_t len) |
| { |
| int rc; |
| uint8_t payload[9] = { reg, 0, 0, 0, 0, 0, 0, 0, 0}; |
| |
| struct hal_i2c_master_data data_struct = { |
| .address = itf->si_addr, |
| .len = len + 1, |
| .buffer = payload |
| }; |
| |
| if (len > (sizeof(payload) - 1)) { |
| rc = OS_EINVAL; |
| goto err; |
| } |
| |
| memcpy(&payload[1], buffer, len); |
| |
| rc = sensor_itf_lock(itf, MYNEWT_VAL(DRV2605_ITF_LOCK_TMO)); |
| if (rc) { |
| return rc; |
| } |
| |
| /* Register write */ |
| rc = i2cn_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1, |
| MYNEWT_VAL(DRV2605_I2C_RETRIES)); |
| if (rc) { |
| DRV2605_LOG(ERROR, "I2C access failed at address 0x%02X\n", |
| data_struct.address); |
| #if MYNEWT_VAL(DRV2605_STATS) |
| STATS_INC(g_drv2605stats, errors); |
| #endif |
| goto err; |
| } |
| |
| err: |
| sensor_itf_unlock(itf); |
| return rc; |
| } |
| |
| /** |
| * Reads a single byte from the specified register |
| * |
| * @param The Sensor interface |
| * @param The register address to read from |
| * @param Pointer to where the register value should be written |
| * |
| * @return 0 on success, non-zero error on failure. |
| */ |
| int |
| drv2605_read8(struct sensor_itf *itf, uint8_t reg, uint8_t *value) |
| { |
| int rc; |
| uint8_t payload; |
| |
| struct hal_i2c_master_data data_struct = { |
| .address = itf->si_addr, |
| .len = 1, |
| .buffer = &payload |
| }; |
| |
| /* Register write */ |
| payload = reg; |
| |
| rc = sensor_itf_lock(itf, MYNEWT_VAL(DRV2605_ITF_LOCK_TMO)); |
| if (rc) { |
| return rc; |
| } |
| |
| rc = i2cn_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 0, |
| MYNEWT_VAL(DRV2605_I2C_RETRIES)); |
| if (rc) { |
| DRV2605_LOG(ERROR, |
| "I2C register write failed at address 0x%02X:0x%02X\n", |
| data_struct.address, reg); |
| #if MYNEWT_VAL(DRV2605_STATS) |
| STATS_INC(g_drv2605stats, errors); |
| #endif |
| goto err; |
| } |
| |
| /* Read one byte back */ |
| payload = 0; |
| rc = i2cn_master_read(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1, |
| MYNEWT_VAL(DRV2605_I2C_RETRIES)); |
| *value = payload; |
| if (rc) { |
| DRV2605_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n", |
| data_struct.address, reg); |
| #if MYNEWT_VAL(DRV2605_STATS) |
| STATS_INC(g_drv2605stats, errors); |
| #endif |
| goto err; |
| } |
| |
| err: |
| sensor_itf_unlock(itf); |
| return rc; |
| } |
| |
| /** |
| * Read data from the sensor of variable length (MAX: 8 bytes) |
| * |
| * @param The Sensor interface |
| * @param Register to read from |
| * @param Buffer to read into |
| * @param Length of the buffer |
| * |
| * @return 0 on success and non-zero on failure |
| */ |
| int |
| drv2605_readlen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer, |
| uint8_t len) |
| { |
| int rc; |
| uint8_t payload[23] = { reg, 0, 0, 0, 0, 0, 0, 0, |
| 0, 0, 0, 0, 0, 0, 0, 0, |
| 0, 0, 0, 0, 0, 0, 0}; |
| |
| struct hal_i2c_master_data data_struct = { |
| .address = itf->si_addr, |
| .len = 1, |
| .buffer = payload |
| }; |
| |
| /* Clear the supplied buffer */ |
| memset(buffer, 0, len); |
| |
| rc = sensor_itf_lock(itf, MYNEWT_VAL(DRV2605_ITF_LOCK_TMO)); |
| if (rc) { |
| return rc; |
| } |
| |
| /* Register write */ |
| rc = i2cn_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 0, |
| MYNEWT_VAL(DRV2605_I2C_RETRIES)); |
| if (rc) { |
| DRV2605_LOG(ERROR, "I2C access failed at address 0x%02X\n", |
| data_struct.address); |
| #if MYNEWT_VAL(DRV2605_STATS) |
| STATS_INC(g_drv2605stats, errors); |
| #endif |
| goto err; |
| } |
| |
| /* Read len bytes back */ |
| memset(payload, 0, sizeof(payload)); |
| data_struct.len = len; |
| rc = i2cn_master_read(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1, |
| MYNEWT_VAL(DRV2605_I2C_RETRIES)); |
| if (rc) { |
| DRV2605_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n", |
| data_struct.address, reg); |
| #if MYNEWT_VAL(DRV2605_STATS) |
| STATS_INC(g_drv2605stats, errors); |
| #endif |
| goto err; |
| } |
| |
| /* Copy the I2C results into the supplied buffer */ |
| memcpy(buffer, payload, len); |
| |
| err: |
| sensor_itf_unlock(itf); |
| return rc; |
| } |
| |
| // general best fit values from datasheet 8.5.6 |
| int |
| drv2605_default_cal(struct drv2605_cal *cal) |
| { |
| cal->brake_factor = 2; |
| cal->loop_gain = 2; |
| cal->lra_sample_time = 3; |
| cal->lra_blanking_time = 1; |
| cal->lra_idiss_time = 1; |
| cal->auto_cal_time = 3; |
| cal->lra_zc_det_time = 0; |
| |
| return 0; |
| } |
| |
| /** |
| * Expects to be called back through os_dev_create(). |
| * |
| * @param The device object associated with this haptic feedback controller |
| * @param Argument passed to OS device init, unused |
| * |
| * @return 0 on success, non-zero error on failure. |
| */ |
| int |
| drv2605_init(struct os_dev *dev, void *arg) |
| { |
| struct drv2605 *drv2605; |
| struct sensor *sensor; |
| uint8_t id; |
| int rc; |
| |
| if (!arg || !dev) { |
| rc = SYS_ENODEV; |
| goto err; |
| } |
| |
| drv2605 = (struct drv2605 *) dev; |
| |
| sensor = &drv2605->sensor; |
| |
| #if MYNEWT_VAL(DRV2605_STATS) |
| /* Initialise the stats entry */ |
| rc = stats_init( |
| STATS_HDR(g_drv2605stats), |
| STATS_SIZE_INIT_PARMS(g_drv2605stats, STATS_SIZE_32), |
| STATS_NAME_INIT_PARMS(drv2605_stat_section)); |
| SYSINIT_PANIC_ASSERT(rc == 0); |
| /* Register the entry with the stats registry */ |
| rc = stats_register(dev->od_name, STATS_HDR(g_drv2605stats)); |
| SYSINIT_PANIC_ASSERT(rc == 0); |
| #endif |
| |
| rc = sensor_init(sensor, dev); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| /* Set the interface */ |
| rc = sensor_set_interface(sensor, arg); |
| if (rc) { |
| goto err; |
| } |
| |
| /* Check if we can read the chip address */ |
| rc = drv2605_get_chip_id(arg, &id); |
| if (rc) { |
| DRV2605_LOG(ERROR, "unable to get chip id [1]: %d\n", rc); |
| goto err; |
| } |
| |
| if (id != DRV2605_STATUS_DEVICE_ID_2605 && id != DRV2605_STATUS_DEVICE_ID_2605L) { |
| os_time_delay((OS_TICKS_PER_SEC * 100)/1000 + 1); |
| |
| rc = drv2605_get_chip_id(arg, &id); |
| if (rc) { |
| DRV2605_LOG(ERROR, "unable to get chip id [2]: %d\n", rc); |
| goto err; |
| } |
| |
| if (id != DRV2605_STATUS_DEVICE_ID_2605 && id != DRV2605_STATUS_DEVICE_ID_2605L) { |
| rc = SYS_EINVAL; |
| DRV2605_LOG(ERROR, |
| "id not as expected: got: %d, expected %d or %d\n", id, |
| DRV2605_STATUS_DEVICE_ID_2605, |
| DRV2605_STATUS_DEVICE_ID_2605L); |
| goto err; |
| } |
| } |
| |
| return (0); |
| err: |
| DRV2605_LOG(ERROR, "Error initializing DRV2605: %d\n", rc); |
| return (rc); |
| } |
| |
| /** |
| * Get chip ID from the sensor |
| * |
| * @param The sensor interface |
| * @param Pointer to the variable to fill up chip ID in |
| * @return 0 on success, non-zero on failure |
| */ |
| int |
| drv2605_get_chip_id(struct sensor_itf *itf, uint8_t *id) |
| { |
| int rc; |
| uint8_t idtmp; |
| |
| /* Check if we can read the chip address */ |
| rc = drv2605_read8(itf, DRV2605_STATUS_ADDR, &idtmp); |
| if (rc) { |
| goto err; |
| } |
| |
| *id = (idtmp & DRV2605_STATUS_DEVICE_ID_MASK) >> DRV2605_STATUS_DEVICE_ID_POS; |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| |
| // NOTE diagnostics (and frankly all operation) will in all likelihood fail |
| // if your motor is not SECURED to a mass. It can't be floating on your desk |
| // even for prototyping |
| int |
| drv2605_mode_diagnostic(struct sensor_itf *itf) |
| { |
| int rc; |
| uint8_t temp; |
| uint8_t interval = 255; |
| uint8_t last_mode; |
| |
| rc = drv2605_read8(itf, DRV2605_MODE_ADDR, &last_mode); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = drv2605_write8(itf, DRV2605_MODE_ADDR, DRV2605_MODE_DIAGNOSTICS | DRV2605_MODE_ACTIVE); |
| if (rc) { |
| goto err; |
| } |
| |
| // 4. Set the GO bit (write 0x01 to register 0x0C) to start the auto-calibration process |
| rc = drv2605_write8(itf, DRV2605_GO_ADDR, DRV2605_GO_GO); |
| if (rc) { |
| goto err; |
| } |
| |
| // When diagnostic is complete, the GO bit automatically clears. Timeout after 255 x 5ms or 1275ms |
| do{ |
| os_time_delay((OS_TICKS_PER_SEC * 5)/1000 + 1); |
| rc = drv2605_read8(itf, DRV2605_GO_ADDR, &temp); |
| } while (!rc && interval-- && (temp & DRV2605_GO_GO)); |
| |
| // if we timed out |
| if (!interval) { |
| rc = OS_TIMEOUT; |
| goto err; |
| } |
| |
| // 5. Check the status of the DIAG_RESULT bit (in register 0x00) to ensure that the diagnostic routine is complete without faults. |
| rc = drv2605_read8(itf, DRV2605_STATUS_ADDR, &temp); |
| if (rc || (temp & DRV2605_STATUS_DIAG_RESULT_FAIL)) { |
| rc = OS_ERROR; |
| goto err; |
| } |
| |
| // put back into standby like all other successful mode ops |
| rc = drv2605_write8(itf, DRV2605_MODE_ADDR, (last_mode & (~DRV2605_MODE_STANDBY_MASK)) | DRV2605_MODE_STANDBY); |
| if (rc) { |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| |
| int |
| drv2605_send_defaults(struct sensor_itf *itf, struct drv2605_cfg *cfg) |
| { |
| int rc; |
| |
| rc = drv2605_write8(itf, DRV2605_RATED_VOLTAGE_ADDR, MYNEWT_VAL(DRV2605_RATED_VOLTAGE)); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = drv2605_write8(itf, DRV2605_OVERDRIVE_CLAMP_VOLTAGE_ADDR, MYNEWT_VAL(DRV2605_OD_CLAMP)); |
| if (rc) { |
| goto err; |
| } |
| |
| uint8_t motor_mask = 0; |
| if (cfg->motor_type == DRV2605_MOTOR_LRA) { |
| motor_mask = DRV2605_FEEDBACK_CONTROL_N_LRA; |
| } |
| else { |
| motor_mask = DRV2605_FEEDBACK_CONTROL_N_ERM; |
| } |
| rc = drv2605_write8(itf, DRV2605_FEEDBACK_CONTROL_ADDR, ((MYNEWT_VAL(DRV2605_CALIBRATED_BEMF_GAIN) & DRV2605_FEEDBACK_CONTROL_BEMF_GAIN_MAX) << DRV2605_FEEDBACK_CONTROL_BEMF_GAIN_POS) | motor_mask ); |
| if (rc) { |
| goto err; |
| } |
| |
| // They seem to always enable startup boost in the dev kit so throw it in? |
| rc = drv2605_write8(itf, DRV2605_CONTROL1_ADDR, ((MYNEWT_VAL(DRV2605_DRIVE_TIME) & DRV2605_CONTROL1_DRIVE_TIME_MAX) << DRV2605_CONTROL1_DRIVE_TIME_POS) | DRV2605_CONTROL1_STARTUP_BOOST_ENABLE); |
| if (rc) { |
| goto err; |
| } |
| |
| // TODO: the selection of LRA vs ERM could also include open vs. closed loop, allowing the |
| // full matrix of possibilities |
| if (cfg->motor_type == DRV2605_MOTOR_LRA) { |
| rc = drv2605_write8(itf, DRV2605_CONTROL3_ADDR, DRV2605_CONTROL3_LRA_DRIVE_MODE_ONCE | DRV2605_CONTROL3_LRA_OPEN_LOOP_CLOSED); |
| } |
| else { |
| rc = drv2605_write8(itf, DRV2605_CONTROL3_ADDR, DRV2605_CONTROL3_ERM_OPEN_LOOP_ENABLED); |
| } |
| if (rc) { |
| goto err; |
| } |
| |
| rc = drv2605_write8(itf, DRV2605_AUTO_CALIBRATION_COMPENSATION_RESULT_ADDR, MYNEWT_VAL(DRV2605_CALIBRATED_COMP)); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = drv2605_write8(itf, DRV2605_AUTO_CALIBRATION_BACK_EMF_RESULT_ADDR, MYNEWT_VAL(DRV2605_CALIBRATED_BEMF)); |
| if (rc) { |
| goto err; |
| } |
| |
| // Library selection occurs through register 0x03 (see the (Address: 0x03) section). |
| uint8_t library_selection; |
| if (cfg->motor_type == DRV2605_MOTOR_LRA) { |
| // Library 6 is a closed-loop library tuned for LRAs. |
| library_selection = DRV2605_WAVEFORM_CONTROL_LIBRARY_SEL_LRA; |
| } |
| else { |
| // TODO: there could be a setter function for the ERM library choices |
| // Library B is an open-loop ERM set for 3V |
| library_selection = DRV2605_WAVEFORM_CONTROL_LIBRARY_SEL_B; |
| } |
| rc = drv2605_write8(itf, DRV2605_WAVEFORM_CONTROL_ADDR, library_selection); |
| if (rc) { |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| int |
| drv2605_get_power_mode(struct sensor_itf *itf, enum drv2605_power_mode *power_mode) |
| { |
| int rc; |
| uint8_t last_mode; |
| bool standby, en_pin; |
| |
| rc = drv2605_read8(itf, DRV2605_MODE_ADDR, &last_mode); |
| if (rc) { |
| goto err; |
| } |
| |
| standby = last_mode & DRV2605_MODE_STANDBY_MASK; |
| en_pin = hal_gpio_read(itf->si_cs_pin); |
| |
| if(!en_pin){ |
| *power_mode = DRV2605_POWER_OFF; |
| }else{ |
| if(standby){ |
| *power_mode = DRV2605_POWER_STANDBY; |
| }else{ |
| *power_mode = DRV2605_POWER_ACTIVE; |
| } |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| int |
| drv2605_set_standby(struct sensor_itf *itf, bool standby) |
| { |
| int rc; |
| uint8_t last_mode; |
| |
| rc = drv2605_read8(itf, DRV2605_MODE_ADDR, &last_mode); |
| if (rc) { |
| goto err; |
| } |
| |
| uint8_t mode; |
| if(standby){ |
| mode = DRV2605_MODE_STANDBY; |
| }else{ |
| mode = DRV2605_MODE_ACTIVE; |
| } |
| |
| rc = drv2605_write8(itf, DRV2605_MODE_ADDR, (last_mode & (~DRV2605_MODE_STANDBY_MASK)) | mode); |
| if (rc) { |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| int |
| drv2605_set_power_mode(struct sensor_itf *itf, enum drv2605_power_mode power_mode) |
| { |
| // TODO: any hiccup in writing enable if already active? dont like the idea of reading it first though.. |
| switch(power_mode) { |
| case DRV2605_POWER_STANDBY: |
| hal_gpio_write(itf->si_cs_pin, 1); |
| return drv2605_set_standby(itf, 1); |
| case DRV2605_POWER_ACTIVE: |
| hal_gpio_write(itf->si_cs_pin, 1); |
| return drv2605_set_standby(itf, 0); |
| case DRV2605_POWER_OFF: |
| hal_gpio_write(itf->si_cs_pin, 0); |
| return 0; |
| default: |
| return SYS_EINVAL; |
| } |
| } |
| |
| int |
| drv2605_validate_cal(struct drv2605_cal *cal) |
| { |
| int rc; |
| |
| if (cal->brake_factor > DRV2605_FEEDBACK_CONTROL_FB_BRAKE_FACTOR_MAX) { |
| rc = OS_EINVAL; |
| goto err; |
| } |
| |
| if (cal->loop_gain > DRV2605_FEEDBACK_CONTROL_LOOP_GAIN_MAX) { |
| rc = OS_EINVAL; |
| goto err; |
| } |
| |
| if (cal->lra_sample_time > DRV2605_CONTROL2_SAMPLE_TIME_MAX) { |
| rc = OS_EINVAL; |
| goto err; |
| } |
| |
| if (cal->lra_blanking_time > DRV2605_BLANKING_TIME_MAX) { |
| rc = OS_EINVAL; |
| goto err; |
| } |
| |
| if (cal->lra_idiss_time > DRV2605_IDISS_TIME_MAX) { |
| rc = OS_EINVAL; |
| goto err; |
| } |
| |
| if (cal->auto_cal_time > DRV2605_CONTROL4_AUTO_CAL_TIME_MAX) { |
| rc = OS_EINVAL; |
| goto err; |
| } |
| |
| if (cal->lra_zc_det_time > DRV2605_CONTROL4_ZC_DET_TIME_MAX) { |
| rc = OS_EINVAL; |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| // if succesful calibration overrites DRV2605_BEMF_GAIN, DRV2605_CALIBRATED_COMP and DRV2605_CALIBRATED_BEMF |
| int |
| drv2605_mode_calibrate(struct sensor_itf *itf, struct drv2605_cal *cal) |
| { |
| int rc; |
| uint8_t temp; |
| uint8_t interval = 255; |
| uint8_t last_fb, last_mode; |
| |
| rc = drv2605_read8(itf, DRV2605_MODE_ADDR, &last_mode); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = drv2605_validate_cal(cal); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = drv2605_read8(itf, DRV2605_FEEDBACK_CONTROL_ADDR, &last_fb); |
| if (rc) { |
| goto err; |
| } |
| |
| // technically only need to protect the ERM_LRA bit as DRV2605_BEMF_GAIN will be altered anyway, but lets show em our fancy bit math |
| uint8_t mask = (DRV2605_FEEDBACK_CONTROL_FB_BRAKE_FACTOR_MASK | DRV2605_FEEDBACK_CONTROL_LOOP_GAIN_MASK); |
| uint8_t altered = (cal->brake_factor << DRV2605_FEEDBACK_CONTROL_FB_BRAKE_FACTOR_POS) | (cal->loop_gain << DRV2605_FEEDBACK_CONTROL_LOOP_GAIN_POS); |
| uint8_t new = (last_fb & (~mask)) | altered; |
| rc = drv2605_write8(itf, DRV2605_FEEDBACK_CONTROL_ADDR, new); |
| if (rc) { |
| goto err; |
| } |
| |
| int8_t idiss_lsb = cal->lra_idiss_time & 0x03; |
| int8_t blanking_lsb = cal->lra_blanking_time & 0x03; |
| int8_t ctrl2 = (cal->lra_sample_time << DRV2605_CONTROL2_SAMPLE_TIME_POS) | (blanking_lsb << DRV2605_CONTROL2_BLANKING_TIME_LSB_POS) | (idiss_lsb << DRV2605_CONTROL2_IDISS_TIME_LSB_POS); |
| rc = drv2605_write8(itf, DRV2605_CONTROL2_ADDR, ctrl2); |
| if (rc) { |
| goto err; |
| } |
| |
| int8_t blanking_msb = cal->lra_blanking_time & 0x0C; |
| int8_t idiss_msb = cal->lra_idiss_time & 0x0C; |
| rc = drv2605_write8(itf, DRV2605_CONTROL5_ADDR, blanking_msb << DRV2605_CONTROL5_BLANKING_TIME_MSB_POS | idiss_msb << DRV2605_CONTROL5_IDISS_TIME_MSB_POS ); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = drv2605_write8(itf, DRV2605_CONTROL4_ADDR, (cal->lra_zc_det_time << DRV2605_CONTROL4_ZC_DET_TIME_POS) | (cal->auto_cal_time << DRV2605_CONTROL4_AUTO_CAL_TIME_POS)); |
| if (rc) { |
| goto err; |
| } |
| |
| // 2. Write a value of 0x07 to register 0x01. This value moves the DRV2605L device out of STANDBY and places the MODE[2:0] bits in auto-calibration mode. |
| rc = drv2605_write8(itf, DRV2605_MODE_ADDR, DRV2605_MODE_AUTO_CALIBRATION | DRV2605_MODE_ACTIVE); |
| if (rc) { |
| goto err; |
| } |
| |
| // 4. Set the GO bit (write 0x01 to register 0x0C) to start the auto-calibration process. |
| rc = drv2605_write8(itf, DRV2605_GO_ADDR, DRV2605_GO_GO); |
| if (rc) { |
| goto err; |
| } |
| |
| // When auto calibration is complete, the GO bit automatically clears. The auto-calibration results are written in the respective registers as shown in Figure 25. |
| do{ |
| os_time_delay((OS_TICKS_PER_SEC * 5)/1000 + 1); |
| rc = drv2605_read8(itf, DRV2605_GO_ADDR, &temp); |
| } while (!rc && interval-- && (temp & DRV2605_GO_GO)); |
| |
| // if we timed out |
| if (!interval) { |
| rc = OS_TIMEOUT; |
| goto err; |
| } |
| |
| // 5. Check the status of the DIAG_RESULT bit (in register 0x00) to ensure that the auto-calibration routine is complete without faults |
| rc = drv2605_read8(itf, DRV2605_STATUS_ADDR, &temp); |
| if (rc || (temp & DRV2605_STATUS_DIAG_RESULT_FAIL)) { |
| rc = OS_ERROR; |
| goto err; |
| } |
| |
| // put back into standby like all other successful mode ops |
| rc = drv2605_write8(itf, DRV2605_MODE_ADDR, (last_mode & (~DRV2605_MODE_STANDBY_MASK)) | DRV2605_MODE_STANDBY); |
| if (rc) { |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| int |
| drv2605_mode_rom(struct sensor_itf *itf) |
| { |
| int rc; |
| |
| rc = drv2605_write8(itf, DRV2605_MODE_ADDR, DRV2605_MODE_INTERNAL_TRIGGER | DRV2605_MODE_STANDBY); |
| if (rc) { |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| int |
| drv2605_mode_rtp(struct sensor_itf *itf) |
| { |
| int rc; |
| |
| rc = drv2605_write8(itf, DRV2605_MODE_ADDR, DRV2605_MODE_RTP | DRV2605_MODE_STANDBY); |
| if (rc) { |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| int |
| drv2605_mode_pwm(struct sensor_itf *itf) |
| { |
| int rc; |
| |
| rc = drv2605_write8(itf, DRV2605_MODE_ADDR, DRV2605_MODE_PWM_ANALOG_INPUT | DRV2605_MODE_STANDBY); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = drv2605_write8(itf, DRV2605_CONTROL3_ADDR, DRV2605_CONTROL3_N_PWM_ANALOG_MASK); |
| if (rc) { |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| // NOTE reset sets mode back to standby |
| // obviously you must _configure again after a reset |
| int |
| drv2605_mode_reset(struct sensor_itf *itf) |
| { |
| int rc; |
| uint8_t temp; |
| uint8_t interval = 255; |
| |
| rc = drv2605_write8(itf, DRV2605_MODE_ADDR, DRV2605_MODE_RESET); |
| if (rc) { |
| goto err; |
| } |
| |
| // When reset is complete, the reset bit automatically clears. Timeout after 255 x 5ms or 1275ms |
| do{ |
| os_time_delay((OS_TICKS_PER_SEC * 5)/1000 + 1); |
| rc = drv2605_read8(itf, DRV2605_MODE_ADDR, &temp); |
| } while (!rc && interval-- && (temp & DRV2605_MODE_RESET)); |
| |
| // if we timed out |
| if (!interval) { |
| rc = OS_TIMEOUT; |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| // note device MUST be reconfigured for an operational state after a an |
| // error or after succsessful diag and calibration and reset upon success |
| // the device is always left in DRV2605_POWER_STANDBY state no device state |
| // is guaranteed for error returns |
| int |
| drv2605_config(struct drv2605 *drv2605, struct drv2605_cfg *cfg) |
| { |
| int rc; |
| struct sensor_itf *itf; |
| |
| itf = SENSOR_GET_ITF(&(drv2605->sensor)); |
| |
| rc = hal_gpio_init_out(itf->si_cs_pin, 1); |
| if (rc) { |
| return rc; |
| } |
| |
| rc = drv2605_send_defaults(itf, cfg); |
| if (rc) { |
| return rc; |
| } |
| |
| switch(cfg->op_mode) { |
| case DRV2605_OP_ROM: |
| return drv2605_mode_rom(itf); |
| case DRV2605_OP_PWM: |
| return drv2605_mode_pwm(itf); |
| case DRV2605_OP_ANALOG: |
| return drv2605_mode_pwm(itf); |
| case DRV2605_OP_RTP: |
| return drv2605_mode_rtp(itf); |
| case DRV2605_OP_DIAGNOSTIC: |
| return drv2605_mode_diagnostic(itf); |
| case DRV2605_OP_CALIBRATION: |
| return drv2605_mode_calibrate(itf, &cfg->cal); |
| case DRV2605_OP_RESET: |
| return drv2605_mode_reset(itf); |
| default: |
| return SYS_EINVAL; |
| } |
| } |
| |
| int |
| drv2605_load_rom(struct sensor_itf *itf, uint8_t* wav_ids, size_t len) |
| { |
| int rc; |
| |
| if (len > 8) { |
| rc = SYS_EINVAL; |
| goto err; |
| } |
| |
| rc = drv2605_writelen(itf, DRV2605_WAVEFORM_SEQUENCER_ADDR, wav_ids, len); |
| if (rc) { |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| int |
| drv2605_trigger_rom(struct sensor_itf *itf) |
| { |
| return drv2605_write8(itf, DRV2605_GO_ADDR, DRV2605_GO_GO); |
| } |
| |
| // theres sadly no interrupt for knowing when long running roms are finished, youll need to block on this or set a callout |
| // to poll for completion |
| int |
| drv2605_rom_busy(struct sensor_itf *itf, bool *status) |
| { |
| int rc; |
| uint8_t temp; |
| |
| rc = drv2605_read8(itf, DRV2605_GO_ADDR, &temp); |
| if (rc) { |
| goto err; |
| } |
| |
| *status = temp; |
| return 0; |
| err: |
| return rc; |
| } |
| |
| int |
| drv2605_load_rtp(struct sensor_itf *itf, uint8_t value) |
| { |
| int rc; |
| |
| rc = drv2605_write8(itf, DRV2605_REAL_TIME_PLAYBACK_INPUT_ADDR, value); |
| if (rc) { |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |