Staging/saturn

.gitignore

@@ -30,3 +30,4 @@ cconfig.h
 .cortex-debug*
 
 src/lib/dronecan/dsdlc_generated/*
+# target/*/*/board/cubemx/*

.vscode/launch.json

@@ -5,6 +5,20 @@
     "version": "0.2.0",
     "configurations": [
         {
+            "name": "SIEON S1Lite J-Link",
+            "type": "cortex-debug",
+            "request": "launch",
+            "cwd": "${workspaceRoot}",
+            "executable": "target/sieon/s1_lite/build/fmt_sieon-s1_lite.elf",
+            "serverpath": "${env:JLINK_SERVER}",
+            "servertype": "jlink",
+            "device": "STM32H743II",
+            "interface": "swd",
+            "armToolchainPath": "${env:RTT_EXEC_PATH}",
+            "serialNumber": "", //If you have more than one J-Link probe, add the serial number here.
+            "svdFile": "target/sieon/s1_lite/STM32H7x3.svd",
+            "runToMain": true
+        },{
             "name": "FMUv5 Debug J-Link",
             "type": "cortex-debug",
             "request": "launch",

src/driver/imu/bmi088.c

@@ -32,110 +32,112 @@
 #define REG_VAL(_setbits, _clearbits) \
     (reg_val_t) { .setbits = (_setbits), .clearbits = (_clearbits) }
 
+#define RETRY_TIMES                 3
+
 // #define BMI088_ACC_I2C_ADDR1        0x18                 //SDO is low(GND)
 // #define BMI088_ACC_I2C_ADDR2        0x19                 //SDO is high(VCC)
 // #define BMI088_ACC_DEFAULT_ADDRESS  BMI088_ACC_I2C_ADDR2 //in the LPC54102 SPM-S
 
 #define BMI088_ACC_BGW_CHIPID_VALUE 0x1E
 #define BMI088_ACC_BGW_CHIPID       0x00
 
-#define BMI088_ACC_ERR_REG 0x02
-#define BMI088_ACC_STATUS  0x03
+#define BMI088_ACC_ERR_REG          0x02
+#define BMI088_ACC_STATUS           0x03
 
-#define BMI088_ACCD_X_LSB   0x12
-#define BMI088_ACCD_X_MSB   0x13
-#define BMI088_ACCD_Y_LSB   0x14
-#define BMI088_ACCD_Y_MSB   0x15
-#define BMI088_ACCD_Z_LSB   0x16
-#define BMI088_ACCD_Z_MSB   0x17
-#define BMI088_SENSORTIME_0 0x18
-#define BMI088_SENSORTIME_1 0x19
-#define BMI088_SENSORTIME_2 0x1A
+#define BMI088_ACCD_X_LSB           0x12
+#define BMI088_ACCD_X_MSB           0x13
+#define BMI088_ACCD_Y_LSB           0x14
+#define BMI088_ACCD_Y_MSB           0x15
+#define BMI088_ACCD_Z_LSB           0x16
+#define BMI088_ACCD_Z_MSB           0x17
+#define BMI088_SENSORTIME_0         0x18
+#define BMI088_SENSORTIME_1         0x19
+#define BMI088_SENSORTIME_2         0x1A
 
-#define BMI088_INT_STAT_1 0x1D
+#define BMI088_INT_STAT_1           0x1D
 
-#define BMI088_INT_TEMP_MSB 0x22
-#define BMI088_INT_TEMP_LSB 0x23
+#define BMI088_INT_TEMP_MSB         0x22
+#define BMI088_INT_TEMP_LSB         0x23
 
-#define BMI088_ACC_CONF  0x40
-#define BMI088_ACC_RANGE 0x41
+#define BMI088_ACC_CONF             0x40
+#define BMI088_ACC_RANGE            0x41
 
-#define BMI088_ACC_PWR_CONF  0x7C
-#define BMI088_ACC_PWR_CTRL  0x7D
-#define BMI088_ACC_SOFTRESET 0x7E
+#define BMI088_ACC_PWR_CONF         0x7C
+#define BMI088_ACC_PWR_CTRL         0x7D
+#define BMI088_ACC_SOFTRESET        0x7E
 
 // #define BMI088_GYRO_I2C_ADDR1        0x68 //SDO is low(GND)
 // #define BMI088_GYRO_I2C_ADDR2        0x69 //SDO is high(VCC)
 // #define BMI088_GYRO_DEFAULT_ADDRESS  BMI088_GYRO_I2C_ADDR2
 
-#define BMI088_GRRO_CHIP_ID 0x0F
-#define BMI088_CHIP_ID_ADDR 0x00
+#define BMI088_GRRO_CHIP_ID         0x0F
+#define BMI088_CHIP_ID_ADDR         0x00
 
-#define BMI088_RATE_X_LSB_ADDR 0x02
-#define BMI088_RATE_X_MSB_ADDR 0x03
-#define BMI088_RATE_Y_LSB_ADDR 0x04
-#define BMI088_RATE_Y_MSB_ADDR 0x05
-#define BMI088_RATE_Z_LSB_ADDR 0x06
-#define BMI088_RATE_Z_MSB_ADDR 0x07
+#define BMI088_RATE_X_LSB_ADDR      0x02
+#define BMI088_RATE_X_MSB_ADDR      0x03
+#define BMI088_RATE_Y_LSB_ADDR      0x04
+#define BMI088_RATE_Y_MSB_ADDR      0x05
+#define BMI088_RATE_Z_LSB_ADDR      0x06
+#define BMI088_RATE_Z_MSB_ADDR      0x07
 
-#define BMI088_INTR_STAT1_ADDR 0x0A
+#define BMI088_INTR_STAT1_ADDR      0x0A
 
-#define BMI088_RANGE_ADDR 0x0F
+#define BMI088_RANGE_ADDR           0x0F
 
-#define BMI088_BW_ADDR 0x10
+#define BMI088_BW_ADDR              0x10
 
-#define BMI088_MODE_LPM1_ADDR 0x11
+#define BMI088_MODE_LPM1_ADDR       0x11
 
-#define BMI088_BGW_SOFT_RST_ADDR 0x14
+#define BMI088_BGW_SOFT_RST_ADDR    0x14
 
-#define BMI088_INTR_ENABLE0_ADDR 0x15
-#define BMI088_INTR_ENABLE1_ADDR 0x16
+#define BMI088_INTR_ENABLE0_ADDR    0x15
+#define BMI088_INTR_ENABLE1_ADDR    0x16
 
-#define BMI088_SELECTF_TEST_ADDR 0x3C
+#define BMI088_SELECTF_TEST_ADDR    0x3C
 
-#define BMI088_GYRO_RANGE_2000_DPS REG_VAL(0, BIT(2) | BIT(1) | BIT(0))
-#define BMI088_GYRO_RANGE_1000_DPS REG_VAL(BIT(0), BIT(2) | BIT(1))
-#define BMI088_GYRO_RANGE_500_DPS  REG_VAL(BIT(1), BIT(2) | BIT(0))
-#define BMI088_GYRO_RANGE_250_DPS  REG_VAL(BIT(1) | BIT(0), BIT(2))
-#define BMI088_GYRO_RANGE_125_DPS  REG_VAL(BIT(2), BIT(1) | BIT(0))
+#define BMI088_GYRO_RANGE_2000_DPS  REG_VAL(0, BIT(2) | BIT(1) | BIT(0))
+#define BMI088_GYRO_RANGE_1000_DPS  REG_VAL(BIT(0), BIT(2) | BIT(1))
+#define BMI088_GYRO_RANGE_500_DPS   REG_VAL(BIT(1), BIT(2) | BIT(0))
+#define BMI088_GYRO_RANGE_250_DPS   REG_VAL(BIT(1) | BIT(0), BIT(2))
+#define BMI088_GYRO_RANGE_125_DPS   REG_VAL(BIT(2), BIT(1) | BIT(0))
 
-#define BMI088_GYRO_RATE_100  REG_VAL(BIT(2) | BIT(1) | BIT(0), BIT(3))
-#define BMI088_GYRO_RATE_200  REG_VAL(BIT(2) | BIT(1), BIT(3) | BIT(0))
-#define BMI088_GYRO_RATE_400  REG_VAL(BIT(1) | BIT(0), BIT(3) | BIT(2))
-#define BMI088_GYRO_RATE_1000 REG_VAL(BIT(1), BIT(3) | BIT(2) | BIT(0))
-#define BMI088_GYRO_RATE_2000 REG_VAL(BIT(0), BIT(3) | BIT(2) | BIT(1))
+#define BMI088_GYRO_RATE_100        REG_VAL(BIT(2) | BIT(1) | BIT(0), BIT(3))
+#define BMI088_GYRO_RATE_200        REG_VAL(BIT(2) | BIT(1), BIT(3) | BIT(0))
+#define BMI088_GYRO_RATE_400        REG_VAL(BIT(1) | BIT(0), BIT(3) | BIT(2))
+#define BMI088_GYRO_RATE_1000       REG_VAL(BIT(1), BIT(3) | BIT(2) | BIT(0))
+#define BMI088_GYRO_RATE_2000       REG_VAL(BIT(0), BIT(3) | BIT(2) | BIT(1))
 
-#define BMI088_ACCEL_RANGE_3_G  0x00
-#define BMI088_ACCEL_RANGE_6_G  0x01
-#define BMI088_ACCEL_RANGE_12_G 0x02
-#define BMI088_ACCEL_RANGE_24_G 0x03
+#define BMI088_ACCEL_RANGE_3_G      0x00
+#define BMI088_ACCEL_RANGE_6_G      0x01
+#define BMI088_ACCEL_RANGE_12_G     0x02
+#define BMI088_ACCEL_RANGE_24_G     0x03
 
-#define BMI088_ACCEL_BW_12_5 0xA5
-#define BMI088_ACCEL_BW_25   0xA6
-#define BMI088_ACCEL_BW_50   0xA7
-#define BMI088_ACCEL_BW_100  0xA8
-#define BMI088_ACCEL_BW_200  0xA9
-#define BMI088_ACCEL_BW_400  0xAA
-#define BMI088_ACCEL_BW_800  0xAB
-#define BMI088_ACCEL_BW_1600 0xAC
+#define BMI088_ACCEL_BW_12_5        0xA5
+#define BMI088_ACCEL_BW_25          0xA6
+#define BMI088_ACCEL_BW_50          0xA7
+#define BMI088_ACCEL_BW_100         0xA8
+#define BMI088_ACCEL_BW_200         0xA9
+#define BMI088_ACCEL_BW_400         0xAA
+#define BMI088_ACCEL_BW_800         0xAB
+#define BMI088_ACCEL_BW_1600        0xAC
 
-#define BMI088_ACCEL_RATE_12_5 REG_VAL(BIT(0) | BIT(2), BIT(1) | BIT(3))
-#define BMI088_ACCEL_RATE_25   REG_VAL(BIT(1) | BIT(2), BIT(0) | BIT(3))
-#define BMI088_ACCEL_RATE_50   REG_VAL(BIT(0) | BIT(1) | BIT(2), BIT(3))
-#define BMI088_ACCEL_RATE_100  REG_VAL(BIT(3), BIT(0) | BIT(1) | BIT(2))
-#define BMI088_ACCEL_RATE_200  REG_VAL(BIT(0) | BIT(3), BIT(1) | BIT(2))
-#define BMI088_ACCEL_RATE_400  REG_VAL(BIT(1) | BIT(3), BIT(0) | BIT(2))
-#define BMI088_ACCEL_RATE_800  REG_VAL(BIT(0) | BIT(1) | BIT(3), BIT(2))
-#define BMI088_ACCEL_RATE_1600 REG_VAL(BIT(2) | BIT(3), BIT(0) | BIT(1))
+#define BMI088_ACCEL_RATE_12_5      REG_VAL(BIT(0) | BIT(2), BIT(1) | BIT(3))
+#define BMI088_ACCEL_RATE_25        REG_VAL(BIT(1) | BIT(2), BIT(0) | BIT(3))
+#define BMI088_ACCEL_RATE_50        REG_VAL(BIT(0) | BIT(1) | BIT(2), BIT(3))
+#define BMI088_ACCEL_RATE_100       REG_VAL(BIT(3), BIT(0) | BIT(1) | BIT(2))
+#define BMI088_ACCEL_RATE_200       REG_VAL(BIT(0) | BIT(3), BIT(1) | BIT(2))
+#define BMI088_ACCEL_RATE_400       REG_VAL(BIT(1) | BIT(3), BIT(0) | BIT(2))
+#define BMI088_ACCEL_RATE_800       REG_VAL(BIT(0) | BIT(1) | BIT(3), BIT(2))
+#define BMI088_ACCEL_RATE_1600      REG_VAL(BIT(2) | BIT(3), BIT(0) | BIT(1))
 
-#define BMI088_ACCEL_OSR_0 REG_VAL(BIT(5) | BIT(7), BIT(4) | BIT(6))
-#define BMI088_ACCEL_OSR_2 REG_VAL(BIT(4) | BIT(7), BIT(5) | BIT(6))
-#define BMI088_ACCEL_OSR_4 REG_VAL(BIT(7), BIT(4) | BIT(5) | BIT(6))
+#define BMI088_ACCEL_OSR_0          REG_VAL(BIT(5) | BIT(7), BIT(4) | BIT(6))
+#define BMI088_ACCEL_OSR_2          REG_VAL(BIT(4) | BIT(7), BIT(5) | BIT(6))
+#define BMI088_ACCEL_OSR_4          REG_VAL(BIT(7), BIT(4) | BIT(5) | BIT(6))
 
-#define DIR_READ     0x80
-#define DIR_WRITE    0x00
-#define M_PI_F       3.1415926f
-#define BMI088_ONE_G 9.80665f
+#define DIR_READ                    0x80
+#define DIR_WRITE                   0x00
+#define M_PI_F                      3.1415926f
+#define BMI088_ONE_G                9.80665f
 
 typedef struct {
     uint8_t setbits;
@@ -161,8 +163,8 @@ static rt_err_t __write_checked_reg(rt_device_t spi_device, rt_uint8_t reg, rt_u
     rt_uint8_t r_val;
 
     RT_TRY(spi_write_reg8(spi_device, reg, val));
-    /* In case of read operations of the accelerometer part, the requested data is not sent 
-    immediately, but instead first a dummy byte is sent, and after this dummy byte the actual 
+    /* In case of read operations of the accelerometer part, the requested data is not sent
+    immediately, but instead first a dummy byte is sent, and after this dummy byte the actual
     reqested register content is transmitted. */
     RT_TRY(spi_read_reg8(spi_device, reg, &r_val));
     RT_TRY(spi_read_reg8(spi_device, reg, &r_val));
@@ -174,8 +176,8 @@ static rt_err_t __modify_reg(rt_device_t spi_device, rt_uint8_t reg, reg_val_t r
 {
     uint8_t value;
 
-    /* In case of read operations of the accelerometer part, the requested data is not sent 
-    immediately, but instead first a dummy byte is sent, and after this dummy byte the actual 
+    /* In case of read operations of the accelerometer part, the requested data is not sent
+    immediately, but instead first a dummy byte is sent, and after this dummy byte the actual
     reqested register content is transmitted. */
     RT_TRY(spi_read_reg8(spi_device, reg, &value));
     RT_TRY(spi_read_reg8(spi_device, reg, &value));
@@ -281,15 +283,21 @@ static rt_err_t gyroscope_init(void)
     /* init spi bus */
     RT_TRY(rt_device_open(gyro_spi_dev, RT_DEVICE_OFLAG_RDWR));
 
-    spi_read_reg8(gyro_spi_dev, BMI088_CHIP_ID_ADDR, &gyro_id);
-    if (gyro_id != BMI088_GRRO_CHIP_ID) {
+    uint8_t i;
+    for (i = 0; i < RETRY_TIMES; i++) {
+        spi_read_reg8(gyro_spi_dev, BMI088_CHIP_ID_ADDR, &gyro_id);
+        if (gyro_id == BMI088_GRRO_CHIP_ID) {
+            break;
+        }
+    }
+    if (i >= RETRY_TIMES) {
         DRV_DBG("Warning: not found BMI088 gyro id: %02x\n", gyro_id);
         return RT_ERROR;
     }
 
     /* soft reset */
     RT_TRY(spi_write_reg8(gyro_spi_dev, BMI088_BGW_SOFT_RST_ADDR, 0xB6));
-    systime_udelay(35000); // > 30ms delay
+    systime_udelay(35000);              // > 30ms delay
 
     RT_TRY(gyro_set_range(2000));       /* 2000dps */
     RT_TRY(gyro_set_sample_rate(1000)); /* OSR 1000KHz, Filter BW: 116Hz */
@@ -527,8 +535,14 @@ static rt_err_t accelerometer_init(void)
     RT_TRY(spi_read_reg8(accel_spi_dev, BMI088_ACC_BGW_CHIPID, &accel_id));
 
     /* read accel id */
-    RT_TRY(spi_read_reg8(accel_spi_dev, BMI088_ACC_BGW_CHIPID, &accel_id));
-    if (accel_id != BMI088_ACC_BGW_CHIPID_VALUE) {
+    uint8_t i;
+    for (i = 0; i < RETRY_TIMES; i++) {
+        RT_TRY(spi_read_reg8(accel_spi_dev, BMI088_ACC_BGW_CHIPID, &accel_id));
+        if (accel_id == BMI088_ACC_BGW_CHIPID_VALUE) {
+            break;
+        }
+    }
+    if (i >= RETRY_TIMES) {
         DRV_DBG("Warning: not found BMI088 accel id: %02x\n", accel_id);
         return RT_ERROR;
     }
@@ -558,8 +572,8 @@ static rt_err_t accel_read_raw(int16_t acc[3])
 {
     uint8_t buffer[7];
 
-    /* In case of read operations of the accelerometer part, the requested data is not sent 
-    immediately, but instead first a dummy byte is sent, and after this dummy byte the actual 
+    /* In case of read operations of the accelerometer part, the requested data is not sent
+    immediately, but instead first a dummy byte is sent, and after this dummy byte the actual
     reqested register content is transmitted. */
     RT_TRY(spi_read_multi_reg8(accel_spi_dev, BMI088_ACCD_X_LSB, buffer, 7));