works on nearly all sensors

1 files changed, 16 insertions, 12 deletions

diff --git a/ports/raspberrypi/common-hal/busio/I2C.c b/ports/raspberrypi/common-hal/busio/I2C.c
index 970b6de89..6886b3815 100644
--- a/ports/raspberrypi/common-hal/busio/I2C.c
+++ b/ports/raspberrypi/common-hal/busio/I2C.c
@@ -98,18 +98,20 @@ void common_hal_busio_i2c_construct(busio_i2c_obj_t *self,
     // Create a bitbangio.I2C object to do short writes.
     // Must be done before setting up the I2C pins, since they will be
     // set up as GPIO by the bitbangio.I2C object.
+    //
+    // Sets pins to open drain, high, and input.
     shared_module_bitbangio_i2c_construct(&self->bitbangio_i2c, scl, sda,
                                           frequency, timeout);
 
-    gpio_set_function(sda->number, GPIO_FUNC_I2C);
-    gpio_set_function(scl->number, GPIO_FUNC_I2C);
-
     self->baudrate = i2c_init(self->peripheral, frequency);
 
-    self->sda_pin = sda->number;
     self->scl_pin = scl->number;
-    claim_pin(sda);
+    self->sda_pin = sda->number;
     claim_pin(scl);
+    claim_pin(sda);
+
+    gpio_set_function(self->scl_pin, GPIO_FUNC_I2C);
+    gpio_set_function(self->sda_pin, GPIO_FUNC_I2C);
 }
 
 bool common_hal_busio_i2c_deinited(busio_i2c_obj_t *self) {
@@ -154,21 +156,23 @@ void common_hal_busio_i2c_unlock(busio_i2c_obj_t *self) {
 uint8_t common_hal_busio_i2c_write(busio_i2c_obj_t *self, uint16_t addr,
                                    const uint8_t *data, size_t len, bool transmit_stop_bit) {
     if (len <= 2) {
-        // Use a bitbangio.I2C to do the write.
+        // The RP2040 I2C peripheral will not do writes 2 bytes or less long.
+        // So use bitbangio.I2C to do the write.
 
-        gpio_set_function(self->sda_pin, GPIO_FUNC_SIO);
         gpio_set_function(self->scl_pin, GPIO_FUNC_SIO);
-        gpio_set_dir(self->sda_pin, GPIO_IN);
-        gpio_put(self->sda_pin, true);
+        gpio_set_function(self->sda_pin, GPIO_FUNC_SIO);
         gpio_set_dir(self->scl_pin, GPIO_IN);
-        gpio_put(self->scl_pin, true);
+        gpio_set_dir(self->sda_pin, GPIO_IN);
+        gpio_put(self->scl_pin, false);
+        gpio_put(self->sda_pin, false);
 
         uint8_t status = shared_module_bitbangio_i2c_write(&self->bitbangio_i2c,
                                                            addr, data, len, transmit_stop_bit);
 
-        mp_hal_delay_us(20);
-        gpio_set_function(self->sda_pin, GPIO_FUNC_I2C);
+        // The pins must be set back to GPIO_FUNC_I2C in the order given here,
+        // SCL first, otherwise reads will hang.
         gpio_set_function(self->scl_pin, GPIO_FUNC_I2C);
+        gpio_set_function(self->sda_pin, GPIO_FUNC_I2C);
 
         return status;
     }