串口ok

2024-09-26 22:32:20 +08:00
commit 097089ef4e
323 changed files with 135661 additions and 0 deletions
@@ -0,0 +1,471 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+/*
+  driver for all supported Invensensev2 IMUs
+  ICM-20608 and ICM-20602
+ */
+
+#include "InertialSensor_Invensensev2.h"
+
+#define GRAVITY_MSS     9.80665f
+
+#define debug(fmt, args ...)  do {printf("INV2: " fmt "\n", ## args); } while(0)
+
+/*
+ *  DS-000189-ICM-20948-v1.3.pdf, page 11, section 3.1 lists LSB sensitivity of
+ *  gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
+ */
+static const float GYRO_SCALE = (0.0174532f / 16.4f);
+
+/*
+  EXT_SYNC allows for frame synchronisation with an external device
+  such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit
+ */
+#ifndef INVENSENSE_EXT_SYNC_ENABLE
+#define INVENSENSE_EXT_SYNC_ENABLE 0
+#endif
+
+#include "InertialSensor_Invensensev2_registers.h"
+
+#define INV2_SAMPLE_SIZE 14
+#define INV2_FIFO_BUFFER_LEN 16
+
+#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
+#define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])
+
+// acclerometers on Invensense sensors will return values up to
+// 24G, but they are not guaranteed to be remotely linear past
+// 16G
+static const uint16_t multiplier_accel = INT16_MAX/(26*GRAVITY_MSS);
+
+static bool _select_bank(InertialSensor_Invensensev2_t *inven, uint8_t bank)
+{
+    if (inven->_current_bank != bank) {
+        if (!SPI_DEV_write_register(inven->_dev, INV2REG_BANK_SEL, bank << 4)) {
+            return false;
+        }
+        inven->_current_bank = bank;
+    }
+    return true;
+}
+
+static bool _block_read(InertialSensor_Invensensev2_t *inven, uint16_t reg, uint8_t *buf, uint32_t size)
+{
+    bool rslt = false;
+	SPI_DEV_select(inven->_dev);
+    if (_select_bank(inven, GET_BANK(reg)))
+    {
+       rslt = SPI_DEV_read_registers(inven->_dev, GET_REG(reg), buf, size);
+    }
+	SPI_DEV_unselect(inven->_dev);
+	return rslt;
+}
+
+static uint8_t _register_read(InertialSensor_Invensensev2_t *inven, uint16_t reg)
+{
+    uint8_t val = 0;
+	SPI_DEV_select(inven->_dev);
+    if (_select_bank(inven, GET_BANK(reg)))
+    {
+       SPI_DEV_read_registers(inven->_dev, GET_REG(reg), &val, 1);
+    }
+	SPI_DEV_unselect(inven->_dev);
+    return val;
+}
+
+static void _register_write(InertialSensor_Invensensev2_t *inven, uint16_t reg, uint8_t val)
+{
+	SPI_DEV_select(inven->_dev);
+    if (_select_bank(inven, GET_BANK(reg)))
+    {
+       SPI_DEV_write_register(inven->_dev, GET_REG(reg), val);
+    }
+	SPI_DEV_unselect(inven->_dev);
+}
+
+/*
+  fetch temperature in order to detect FIFO sync errors
+*/
+static bool _check_raw_temp(InertialSensor_Invensensev2_t *inven, int16_t t2)
+{
+    if (abs(t2 - inven->_raw_temp) < 400) {
+        // cached copy OK
+        return true;
+    }
+    uint8_t trx[2];
+    if (_block_read(inven, INV2REG_TEMP_OUT_H, trx, 2)) {
+        inven->_raw_temp = int16_val(trx, 0);
+    }
+    return (abs(t2 - inven->_raw_temp) < 400);
+}
+
+
+static void _fifo_reset(InertialSensor_Invensensev2_t *inven)
+{
+    uint8_t user_ctrl = inven->_last_stat_user_ctrl;
+    user_ctrl &= ~(BIT_USER_CTRL_FIFO_EN);
+
+    _register_write(inven, INV2REG_FIFO_EN_2, 0);
+    _register_write(inven, INV2REG_USER_CTRL, user_ctrl);
+    _register_write(inven, INV2REG_FIFO_RST, 0x0F);
+    _register_write(inven, INV2REG_FIFO_RST, 0x00);
+    _register_write(inven, INV2REG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
+    _register_write(inven, INV2REG_FIFO_EN_2, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
+                    BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN);
+    osDelay(1);
+    inven->_last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
+}
+
+
+/*
+ * Return true if the Invensense has new data available for reading.
+ *
+ * We use the data ready pin if it is available.  Otherwise, read the
+ * status register.
+ */
+bool _data_ready(InertialSensor_Invensensev2_t *inven)
+{
+    if (inven->_drdy) {
+        return (HAL_GPIO_ReadPin(inven->_drdy, inven->_drdy_Pin) == GPIO_PIN_SET);
+    }
+    uint8_t status = _register_read(inven, INV2REG_INT_STATUS_1);
+    return status != 0;
+}
+
+/*
+ * Timer process to poll for new data from the Invensense. Called from bus thread with semaphore held
+ */
+void InertialSensor_Invensensev2_update(InertialSensor_Invensensev2_t *inven)
+{
+	if (inven->success)
+	{
+		if (SPI_DEV_begin(inven->_dev, 0)) {
+			_read_fifo(inven);
+			SPI_DEV_end(inven->_dev);
+		}
+	}
+}
+
+bool _accumulate(InertialSensor_Invensensev2_t *inven, uint8_t *samples, uint8_t n_samples)
+{
+    for (uint8_t i = 0; i < n_samples; i++) {
+        const uint8_t *data = samples + INV2_SAMPLE_SIZE * i;
+        float accel[3], gyro[3];
+        
+        accel[0] = int16_val(data, 1);
+        accel[1] = int16_val(data, 0);
+        accel[2] = -int16_val(data, 2);
+
+        accel[0] *= inven->_accel_scale;
+        accel[1] *= inven->_accel_scale;
+        accel[2] *= inven->_accel_scale;
+
+        int16_t t2 = int16_val(data, 6);
+        if (!_check_raw_temp(inven, t2)) {
+            debug("temp reset IMU %d %d", inven->_raw_temp, t2);
+            _fifo_reset(inven);
+            return false;
+        }
+        float temp = t2 * inven->temp_sensitivity + inven->temp_zero;
+        
+        gyro[0] = int16_val(data, 4);
+        gyro[1] = int16_val(data, 3);
+        gyro[2] = -int16_val(data, 5);
+
+        gyro[0] *= GYRO_SCALE;
+        gyro[1] *= GYRO_SCALE;
+        gyro[2] *= GYRO_SCALE;
+    }
+    return true;
+}
+
+void _read_fifo(InertialSensor_Invensensev2_t *inven)
+{
+    uint8_t n_samples;
+    uint16_t bytes_read;
+    uint8_t *rx = inven->_fifo_buffer;
+    bool need_reset = false;
+
+    if (!_block_read(inven, INV2REG_FIFO_COUNTH, rx, 2)) {
+        return;
+    }
+
+    bytes_read = uint16_val(rx, 0);
+    n_samples = bytes_read / INV2_SAMPLE_SIZE;
+
+    if (n_samples == 0) {
+        /* Not enough data in FIFO */
+        return;
+    }
+
+    /*
+      testing has shown that if we have more than 32 samples in the
+      FIFO then some of those samples will be corrupt. It always is
+      the ones at the end of the FIFO, so clear those with a reset
+      once we've read the first 24. Reading 24 gives us the normal
+      number of samples for fast sampling at 400Hz
+
+      On I2C with the much lower clock rates we need a lower threshold
+      or we may never catch up
+     */
+        if (n_samples > 32) {
+            need_reset = true;
+            n_samples = 24;
+        }
+
+    while (n_samples > 0) {
+        uint8_t n = MIN(n_samples, INV2_FIFO_BUFFER_LEN);
+        if (!_block_read(inven, INV2REG_FIFO_R_W, rx, n * INV2_SAMPLE_SIZE)) {
+            return;
+        }
+
+        if (!_accumulate(inven, rx, n)) {
+                break;
+        }
+        n_samples -= n;
+    }
+
+    if (need_reset) {
+        //debug("fifo reset n_samples %u", bytes_read/INV2_SAMPLE_SIZE);
+        _fifo_reset(inven);
+    }
+}
+
+/*
+  set the DLPF filter frequency and Gyro Accel Scaling. Assumes caller has taken semaphore
+ */
+void _set_filter_and_scaling(InertialSensor_Invensensev2_t *inven)
+{
+    uint8_t gyro_config = (inven->_inv2_type == Invensensev2_ICM20649)?BITS_GYRO_FS_2000DPS_20649 : BITS_GYRO_FS_2000DPS;
+    uint8_t accel_config = (inven->_inv2_type == Invensensev2_ICM20649)?BITS_ACCEL_FS_30G_20649:BITS_ACCEL_FS_16G;
+
+    // assume 1.125kHz sampling to start
+    inven->_gyro_fifo_downsample_rate = inven->_accel_fifo_downsample_rate = 1;
+    inven->_gyro_backend_rate_hz = inven->_accel_backend_rate_hz =  1125;
+    
+    bool _fast_sampling = true;
+        if (_fast_sampling) {
+            // constrain the gyro rate to be at least the loop rate
+            uint8_t loop_limit = 1;
+            if (get_loop_rate_hz() > 1125) {
+                loop_limit = 2;
+            }
+            if (get_loop_rate_hz() > 2250) {
+                loop_limit = 4;
+            }
+            // constrain the gyro rate to be a 2^N multiple
+            uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8);
+
+            // calculate rate we will be giving gyro samples to the backend
+            //_gyro_fifo_downsample_rate = 8 / fast_sampling_rate;
+            //_gyro_backend_rate_hz *= fast_sampling_rate;
+
+            // calculate rate we will be giving accel samples to the backend
+            //_accel_fifo_downsample_rate = MAX(4 / fast_sampling_rate, 1);
+            //_accel_backend_rate_hz *= MIN(fast_sampling_rate, 4);
+
+            // for logging purposes set the oversamping rate
+            //_set_accel_oversampling(_accel_instance, _accel_fifo_downsample_rate);
+            //_set_gyro_oversampling(_gyro_instance, _gyro_fifo_downsample_rate);
+
+            //_set_accel_sensor_rate_sampling_enabled(_accel_instance, true);
+            //_set_gyro_sensor_rate_sampling_enabled(_gyro_instance, true);
+
+            /* set divider for internal sample rate to 0x1F when fast
+             sampling enabled. This reduces the impact of the slave
+             sensor on the sample rate.
+             */
+            _register_write(inven, INV2REG_I2C_SLV4_CTRL, 0x1F);
+        }
+    
+    if (_fast_sampling) {
+        // this gives us 9kHz sampling on gyros
+        gyro_config |= BIT_GYRO_NODLPF_9KHZ;
+        accel_config |= BIT_ACCEL_NODLPF_4_5KHZ;
+    } else {
+        // limit to 1.125kHz if not on SPI
+        gyro_config |= BIT_GYRO_DLPF_ENABLE | (GYRO_DLPF_CFG_188HZ << GYRO_DLPF_CFG_SHIFT);
+        accel_config |= BIT_ACCEL_DLPF_ENABLE | (ACCEL_DLPF_CFG_265HZ << ACCEL_DLPF_CFG_SHIFT);
+    }
+    _register_write(inven, INV2REG_GYRO_CONFIG_1, gyro_config);
+    _register_write(inven, INV2REG_ACCEL_CONFIG, accel_config);
+    _register_write(inven, INV2REG_FIFO_MODE, 0xF);
+}
+
+/*
+  check whoami for sensor type
+ */
+bool _check_whoami(InertialSensor_Invensensev2_t *inven)
+{
+    uint8_t whoami = _register_read(inven, INV2REG_WHO_AM_I);
+    switch (whoami) {
+    case INV2_WHOAMI_ICM20648:
+        inven->_inv2_type = Invensensev2_ICM20648;
+        return true;
+    case INV2_WHOAMI_ICM20948:
+        inven->_inv2_type = Invensensev2_ICM20948;
+        return true;
+    case INV2_WHOAMI_ICM20649:
+        inven->_inv2_type = Invensensev2_ICM20649;
+        return true;
+    }
+    // not a value WHOAMI result
+    return false;
+}
+
+bool _hardware_init(InertialSensor_Invensensev2_t *inven)
+{
+    if (!SPI_DEV_begin(inven->_dev, 3000)) {
+        return false;
+    }
+
+    inven->_current_bank = 0;
+
+    if (!_check_whoami(inven)) {
+        goto fail;
+    }
+
+    // Chip reset
+    uint8_t tries;
+    for (tries = 0; tries < 5; tries++) {
+        inven->_last_stat_user_ctrl = _register_read(inven, INV2REG_USER_CTRL);
+
+        /* First disable the master I2C to avoid hanging the slaves on the
+         * aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
+         * is used */
+        if (inven->_last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
+            inven->_last_stat_user_ctrl &= ~BIT_USER_CTRL_I2C_MST_EN;
+            _register_write(inven, INV2REG_USER_CTRL, inven->_last_stat_user_ctrl);
+            osDelay(10);
+        }
+
+        /* reset device */
+        _register_write(inven, INV2REG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
+        osDelay(100);
+
+        /* bus-dependent initialization */
+            /* Disable I2C bus if SPI selected (Recommended in Datasheet to be
+             * done just after the device is reset) */
+            inven->_last_stat_user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
+            _register_write(inven, INV2REG_USER_CTRL, inven->_last_stat_user_ctrl);
+
+        // Wake up device and select Auto clock. Note that the
+        // Invensense starts up in sleep mode, and it can take some time
+        // for it to come out of sleep
+        _register_write(inven, INV2REG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_AUTO);
+        osDelay(5);
+
+        // check it has woken up
+        if (_register_read(inven, INV2REG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_AUTO) {
+            break;
+        }
+
+        osDelay(10);
+        if (_data_ready(inven)) {
+            break;
+        }
+    }
+
+    if (tries == 5) {
+        debug("Failed to boot Invensense 5 times\n");
+        SPI_DEV_end(inven->_dev);
+        return false;
+    }
+
+    if (inven->_inv2_type == Invensensev2_ICM20649) {
+        inven->_clip_limit = 29.5f * GRAVITY_MSS;
+    }
+fail:
+    SPI_DEV_end(inven->_dev);
+    
+    return true;
+}
+
+bool InertialSensor_Invensensev2_init(InertialSensor_Invensensev2_t *inven, const char *name, SPI_DEV_t* dev, GPIO_TypeDef* drdy, uint16_t drdy_Pin)
+{
+    inven->name = name;
+    inven->_dev = dev;
+
+    inven->temp_sensitivity = 1.0/340; // degC/LSB
+    inven->temp_zero = 36.53; // degC
+
+    inven->_drdy = drdy;
+    inven->_drdy_Pin = drdy_Pin;
+
+    bool success = _hardware_init(inven);
+
+    return success;
+}
+
+void InertialSensor_Invensensev2_start(InertialSensor_Invensensev2_t *inven)
+{
+    if (!SPI_DEV_begin(inven->_dev, 1000)) {
+        return;
+    }
+
+    // only used for wake-up in accelerometer only low power mode
+    _register_write(inven, INV2REG_PWR_MGMT_2, 0x00);
+    osDelay(1);
+
+    // always use FIFO
+    _fifo_reset(inven);
+
+    // grab the used instances
+    switch (inven->_inv2_type) {
+    case Invensensev2_ICM20648:
+        // using 16g full range, 2048 LSB/g
+        inven->_accel_scale = (GRAVITY_MSS / 2048);
+        break;
+    case Invensensev2_ICM20649:
+        // 20649 is setup for 30g full scale, 1024 LSB/g
+        inven->_accel_scale = (GRAVITY_MSS / 1024);
+        break;
+    case Invensensev2_ICM20948:
+    default:
+        // using 16g full range, 2048 LSB/g
+        inven->_accel_scale = (GRAVITY_MSS / 2048);
+        break;
+    }
+
+    // setup on-sensor filtering and scaling
+    _set_filter_and_scaling(inven);
+#if INVENSENSE_EXT_SYNC_ENABLE
+    _register_write(inven, INV2REG_FSYNC_CONFIG, FSYNC_CONFIG_EXT_SYNC_AZ, true);
+#endif
+    // update backend sample rate
+    _set_accel_raw_sample_rate(inven, inven->_accel_backend_rate_hz);
+    _set_gyro_raw_sample_rate(inven,  inven->_gyro_backend_rate_hz);
+
+    // set sample rate to 1.125KHz
+    _register_write(inven, INV2REG_GYRO_SMPLRT_DIV, 0);
+    osDelay(1);
+    
+    // configure interrupt to fire when new data arrives
+    _register_write(inven, INV2REG_INT_ENABLE_1, 0x01);
+    osDelay(1);
+
+    SPI_DEV_end(inven->_dev);
+
+    // setup scale factors for fifo data after downsampling
+    inven->_fifo_accel_scale = inven->_accel_scale / inven->_accel_fifo_downsample_rate;
+    inven->_fifo_gyro_scale = GYRO_SCALE / inven->_gyro_fifo_downsample_rate;
+    
+    // allocate fifo buffer
+    inven->_fifo_buffer = (uint8_t *)pvPortMalloc(INV2_FIFO_BUFFER_LEN * INV2_SAMPLE_SIZE);
+    if (!inven->_fifo_buffer) {
+        debug("Invensense: Unable to allocate FIFO buffer");
+    }
+}
+