fix read

betaflight · Jun 27, 2023 · ede26d8 · ede26d8
1 parent 19e148a
commit ede26d8
Showing 1 changed file with 47 additions and 14 deletions.
diff --git a/src/main/drivers/compass/compass_ist8310.c b/src/main/drivers/compass/compass_ist8310.c
@@ -120,31 +120,64 @@
 #define IST8310_CNTRL2_DRPOL 0x04
 #define IST8310_CNTRL2_DRENA 0x08
 
+/*
+
+
 static bool ist8310Init(magDev_t *magDev)
 {
     extDevice_t *dev = &magDev->dev;
 
     busDeviceRegister(dev);
 
-    busWriteRegister(dev, IST8310_REG_CNTRL1, IST8310_ODR_100_HZ);
-    delay(5);
-
+    // write 0x24 to register 0x41 to set average number to 16
     busWriteRegister(dev, IST8310_REG_AVERAGE, IST8310_AVG_16);
+    delay(6);
+    // write 0xC0 to register 0x42 to set pulse duration to 0.8us
+    busWriteRegister(dev, IST8310_REG_PDCNTL, IST8310_PULSE_DURATION_NORMAL);
+    delay(6);
+
+    return true;
+}
+*/
+
+static bool ist8310Init(magDev_t *magDev)
+{
+    extDevice_t *dev = &magDev->dev;
+    uint8_t regTemp;
+
+    busDeviceRegister(dev);
+
+    // soft-Reset
+    bool ack = busReadRegisterBuffer(dev, IST8310_REG_CNTRL2, &regTemp, 1);
+    regTemp |= IST8310_CNTRL2_RESET;
+    ack = ack && busWriteRegister(dev, IST8310_REG_CNTRL2, regTemp);
+    delay(30);
+
+    // ODR mode
+    ack = ack && busWriteRegister(dev, IST8310_REG_CNTRL1, IST8310_ODR_50_HZ);
     delay(5);
 
-    busWriteRegister(dev, IST8310_REG_PDCNTL, IST8310_PULSE_DURATION_NORMAL);
+    // Init setting : avg16 / pulse mode
+    ack = ack && busWriteRegister(dev, IST8310_REG_AVERAGE, IST8310_AVG_16);
+    ack = ack && busWriteRegister(dev, IST8310_REG_PDCNTL, IST8310_PULSE_DURATION_NORMAL);
     delay(5);
 
-    return true;
+    return ack;
 }
 
+
 static bool ist8310Read(magDev_t * magDev, int16_t *magData)
 {
     uint8_t buf[6];
     uint8_t LSB2FSV = 3; // 3mG - 14 bit
 
     extDevice_t *dev = &magDev->dev;
 
+    // write 0x01 to register 0x0A to set single measure mode
+    busWriteRegister(dev, IST8310_REG_CNTRL1, IST8310_ODR_SINGLE);
+    delay(6);
+
+    // read 6 bytes from register 0x03
     if (!busReadRegisterBuffer(dev, IST8310_REG_DATA, buf, 6)) {
         // set magData to zero for case of failed read
         magData[X] = 0;
@@ -159,23 +192,23 @@ static bool ist8310Read(magDev_t * magDev, int16_t *magData)
     magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
     magData[Z] =  (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV;
 
+    // TODO: do cross axis compensation
+
     return true;
 }
 
-#define DETECTION_MAX_RETRY_COUNT 10
 static bool deviceDetect(magDev_t * magDev)
 {
     extDevice_t *dev = &magDev->dev;
 
-    for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
-        delay(10);
-
-        uint8_t sig = 0;
-        bool ack = busReadRegisterBuffer(dev, IST8310_REG_WHOAMI, &sig, 1);
+    uint8_t sig = 0;
+    bool ack = busReadRegisterBuffer(dev, IST8310_REG_WHOAMI, &sig, 1);
+    ack = busReadRegisterBuffer(dev, IST8310_REG_WHOAMI, &sig, 1);
+    ack = busReadRegisterBuffer(dev, IST8310_REG_WHOAMI, &sig, 1);
 
-        if (ack && sig == IST8310_CHIP_ID) {
-            return true;
-        }
+    if (ack && sig == IST8310_CHIP_ID) {
+        // TODO: set device in standby mode
+        return true;
     }
 
     return false;