Threads I2C and SPI (#3471)

* [threads] Add binary semaphore timeout wait.

* [i2c] Update blocking functions to be RTOS aware.

* [SHT25] Refactor SHT25 driver as threaded I2C example.

* [spi] Update blocking function to be RTOS aware.

* [AMT22] add AMT22 driver using SPI blocking mode.

---------

Co-authored-by: Fabien-B <Fabien-B@github.com>

conf/modules/encoder_amt22.xml

@@ -0,0 +1,33 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+<module name="encoder_amt22" dir="sensors" task="sensors">
+  <doc>
+    <description>Driver for AMT22 encoder from CUI devices.</description>
+  </doc>
+  <dep>
+    <depends>spi_master</depends>
+  </dep>
+  <header>
+    <file name="encoder_amt22.h"/>
+  </header>
+  <init fun="encoder_amt22_init()"/>
+  <periodic fun="encoder_amt22_periodic()" freq="50.0" autorun="TRUE"/>
+  <makefile target="ap">
+    <configure name="AMT22_SPI_DEV" default="SPI2" case="upper|lower"/>
+    <configure name="AMT22_SPI_SLAVE_IDX" default="SPI_SLAVE0"/>
+    <define name="USE_$(AMT22_SPI_DEV_UPPER)"/>
+    <define name="USE_$(AMT22_SPI_SLAVE_IDX)"/>
+    <define name="AMT22_SPI_DEV" value="$(AMT22_SPI_DEV_LOWER)"/>
+    <define name="AMT22_SPI_SLAVE_IDX" value="$(AMT22_SPI_SLAVE_IDX)"/>
+    <file name="encoder_amt22.c"/>
+    <file name="amt22.c" dir="peripherals"/>
+    <test>
+      <define name="AMT22_SPI_DEV" value="spi1" />
+      <define name="USE_SPI1" />
+      <define name="AMT22_SPI_SLAVE_IDX" value="0" />
+      <define name="SPI_MASTER"/>
+      <define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
+      <define name="DOWNLINK_DEVICE" value="uart0"/>
+      <define name="USE_UART0" />
+    </test>
+  </makefile>
+</module>

conf/modules/humid_sht_i2c.xml

@@ -3,18 +3,14 @@
 <module name="humid_sht_i2c" dir="meteo">
   <doc>
     <description>Sensirion SHT25 humidity sensor (I2C)</description>
-    <define name="SCP_I2C_DEV" value="i2cX" description="select i2c peripheral to use (default i2c0)"/>
+    <define name="SHT_I2C_DEV" value="i2cX" description="select i2c peripheral to use (default i2c0)"/>
   </doc>
   <header>
     <file name="humid_sht_i2c.h"/>
   </header>
   <init fun="humid_sht_init_i2c()"/>
   <periodic fun="humid_sht_periodic_i2c()" freq="4" delay="0."/>
-  <periodic fun="humid_sht_p_temp()" freq="4" delay="0.4"/>
-  <periodic fun="humid_sht_p_humid()"  freq="4" delay="0.6"/>
-  <event fun="humid_sht_event_i2c()"/>
   <makefile target="ap">
     <file name="humid_sht_i2c.c"/>
   </makefile>
 </module>
-

sw/airborne/arch/chibios/mcu_periph/i2c_arch.c

@@ -235,6 +235,7 @@ static void handle_i2c_thd(struct i2c_periph *p)
   }
 
   i2cReleaseBus((I2CDriver *)p->reg_addr);
+  pprz_bsem_signal(&t->bsem);
 }
 
 /**
@@ -440,6 +441,7 @@ static bool i2c_chibios_submit(struct i2c_periph *p, struct i2c_transaction *t)
   p->trans_insert_idx = temp;
 
   chSysUnlock();
+  pprz_bsem_init(&t->bsem, true);
   chSemSignal(&((struct i2c_init *)p->init_struct)->sem);
   // transaction submitted
   return TRUE;

sw/airborne/arch/chibios/mcu_periph/spi_arch.c

@@ -389,6 +389,7 @@ static void handle_spi_thd(struct spi_periph *p)
     t->after_cb(t);
   }
 
+  pprz_bsem_signal(&t->bsem);
 }
 
 /**
@@ -552,6 +553,7 @@ bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
   p->trans_insert_idx = idx;
 
   chSysUnlock();
+  pprz_bsem_init(&t->bsem, true);
   chSemSignal(&((struct spi_init *)p->init_struct)->sem);
   // transaction submitted
   return TRUE;

sw/airborne/arch/chibios/modules/core/threads_arch.c

@@ -44,6 +44,16 @@ void pprz_bsem_wait(pprz_bsem_t* bsem) {
   chBSemWait(&bsem->bsem);
 }
 
+int pprz_bsem_wait_timeout(pprz_bsem_t* bsem, float timeout) {
+  sysinterval_t chtimeout;
+  if(timeout < 0.002) {
+    chtimeout = chTimeUS2I(timeout*1e6);
+  } else {
+    chtimeout = chTimeMS2I(timeout*1e3);
+  }
+  return chBSemWaitTimeout(&bsem->bsem, chtimeout);
+}
+
 void pprz_bsem_signal(pprz_bsem_t* bsem) {
   chBSemSignal(&bsem->bsem);
 }

sw/airborne/arch/linux/mcu_periph/i2c_arch.c

@@ -104,6 +104,7 @@ static bool i2c_linux_submit(struct i2c_periph *p, struct i2c_transaction *t)
   p->trans[p->trans_insert_idx] = t;
   p->trans_insert_idx = next_idx;
 
+  pprz_bsem_init(&t->bsem, true);
   /* wake handler thread */
   pthread_cond_signal(condition);
   pthread_mutex_unlock(mutex);
@@ -199,6 +200,7 @@ static void *i2c_thread(void *data)
     pthread_mutex_lock(mutex);
     p->trans_extract_idx = (p->trans_extract_idx + 1) % I2C_TRANSACTION_QUEUE_LEN;
     pthread_mutex_unlock(mutex);
+    pprz_bsem_signal(&t->bsem);
   }
   return NULL;
 }

sw/airborne/arch/linux/modules/core/threads_arch.c

@@ -15,6 +15,7 @@
 #include "stdbool.h"
 #include <semaphore.h>
 #include <stdlib.h>
+#include <time.h>
 
 
 int pprz_mtx_init(pprz_mutex_t* mtx) {
@@ -44,6 +45,16 @@ void pprz_bsem_wait(pprz_bsem_t* bsem) {
   sem_wait(&bsem->sem);
 }
 
+int pprz_bsem_wait_timeout(pprz_bsem_t* bsem, float timeout) {
+  struct timespec ts;
+  clock_gettime(CLOCK_REALTIME, &ts);
+  time_t timeout_secs = (time_t)timeout;
+  long timeout_nsecs = (timeout - timeout_secs) * 1e9;
+  ts.tv_sec += timeout_secs;
+  ts.tv_nsec += timeout_nsecs;
+  return sem_timedwait(&bsem->sem, &ts);
+}
+
 void pprz_bsem_signal(pprz_bsem_t* bsem) {
   int val;
   pthread_mutex_lock(&bsem->mtx);

sw/airborne/arch/stm32/modules/core/threads_arch.c

@@ -51,6 +51,18 @@ void pprz_bsem_wait(pprz_bsem_t* bsem) {
   bsem->value = 0;
 }
 
+int pprz_bsem_wait_timeout(pprz_bsem_t* bsem, float timeout) {
+  float time_end = get_sys_time_float() + timeout;
+  while(get_sys_time_float() - time_end > 0) {
+    // active wait
+    if(bsem->value) {
+      bsem->value = 0;
+      return 0;
+    }
+  }
+  return -1;
+}
+
 void pprz_bsem_signal(pprz_bsem_t* bsem) {
   bsem->value = 1;
 }

sw/airborne/boards/bebop/actuators.c

@@ -74,7 +74,7 @@ void actuators_bebop_commit(void)
 {
   // Receive the status
   actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_GET_OBS_DATA;
-  i2c_blocking_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13);
+  i2c_blocking_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13, 0.5);
 
   // Update status
   electrical.vsupply = (float)(actuators_bebop.i2c_trans.buf[9] + (actuators_bebop.i2c_trans.buf[8] << 8)) / 1000.f;
@@ -94,7 +94,7 @@ void actuators_bebop_commit(void)
   if (actuators_bebop.i2c_trans.buf[10] != 4 && actuators_bebop.i2c_trans.buf[10] != 2 && autopilot_get_motors_on()) {
     // Reset the error
     actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_CLEAR_ERROR;
-    i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
+    i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 0.5);
 
     // Start the motors
     actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_START_PROP;
@@ -104,12 +104,12 @@ void actuators_bebop_commit(void)
 #else
     actuators_bebop.i2c_trans.buf[1] = 0b00000101;
 #endif
-    i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2);
+    i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2, 0.5);
   }
   // Stop the motors
   else if (actuators_bebop.i2c_trans.buf[10] == 4 && !autopilot_get_motors_on()) {
     actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_STOP_PROP;
-    i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
+    i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 0.5);
   } else if (actuators_bebop.i2c_trans.buf[10] == 4 && autopilot_get_motors_on()) {
     // Send the commands
     actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_SET_REF_SPEED;
@@ -126,14 +126,14 @@ void actuators_bebop_commit(void)
 #pragma GCC diagnostic ignored "-Wcast-qual"
     actuators_bebop.i2c_trans.buf[10] = actuators_bebop_checksum((uint8_t *)actuators_bebop.i2c_trans.buf, 9);
 #pragma GCC diagnostic pop
-    i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 11);
+    i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 11, 0.5);
   }
 
   // Update the LEDs
   if (actuators_bebop.led != (led_hw_values & 0x3)) {
     actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_TOGGLE_GPIO;
     actuators_bebop.i2c_trans.buf[1] = (led_hw_values & 0x3);
-    i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2);
+    i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2, 0.5);
 
     actuators_bebop.led = led_hw_values & 0x3;
   }

sw/airborne/boards/bebop/mt9f002.c

@@ -194,7 +194,7 @@ static void write_reg(struct mt9f002_t *mt, uint16_t addr, uint32_t val, uint8_t
   }
 
   // Transmit the buffer
-  i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, len + 2);
+  i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, len + 2, 0.5);
 }
 
 /**
@@ -207,7 +207,7 @@ static uint32_t read_reg(struct mt9f002_t *mt, uint16_t addr, uint8_t len)
   mt->i2c_trans.buf[1] = addr & 0xFF;
 
   // Transmit the buffer and receive back
-  i2c_blocking_transceive(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, 2, len);
+  i2c_blocking_transceive(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, 2, len, 0.5);
 
   /* Fix signdness */
   for (uint8_t i = 0; i < len; i++) {

sw/airborne/boards/bebop/mt9v117.c

@@ -231,7 +231,7 @@ static void write_reg(struct mt9v117_t *mt, uint16_t addr, uint32_t val, uint16_
   }
 
   // Transmit the buffer
-  i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, MT9V117_ADDRESS, len + 2);
+  i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, MT9V117_ADDRESS, len + 2, 0.5);
 }
 
 /**
@@ -244,7 +244,7 @@ static uint32_t read_reg(struct mt9v117_t *mt, uint16_t addr, uint16_t len)
   mt->i2c_trans.buf[1] = addr & 0xFF;
 
   // Transmit the buffer and receive back
-  i2c_blocking_transceive(mt->i2c_periph, &mt->i2c_trans, MT9V117_ADDRESS, 2, len);
+  i2c_blocking_transceive(mt->i2c_periph, &mt->i2c_trans, MT9V117_ADDRESS, 2, len, 0.5);
 
   /* Fix sigdness */
   for (uint8_t i = 0; i < len; i++) {
@@ -302,7 +302,7 @@ static inline void mt9v117_write_patch(struct mt9v117_t *mt)
     }
 
     // Transmit the buffer
-    i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, mt->i2c_trans.slave_addr, mt9v117_patch_lines[i].len);
+    i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, mt->i2c_trans.slave_addr, mt9v117_patch_lines[i].len, 0.5);
   }
 
   write_reg(mt, MT9V117_LOGICAL_ADDRESS_ACCESS, 0x0000, 2);

sw/airborne/boards/disco/actuators.c

@@ -119,7 +119,7 @@ void actuators_disco_commit(void)
 
   // Receive the status
   actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_GET_OBS_DATA;
-  i2c_blocking_transceive(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, sizeof(obs_data));
+  i2c_blocking_transceive(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, sizeof(obs_data), 0.5);
   // copy data from buffer
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wcast-qual"
@@ -149,17 +149,17 @@ void actuators_disco_commit(void)
       actuators_disco.motor_rpm > DISCO_BLDC_START_MOTOR_THRESHOLD) {
     // Reset the error
     actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_CLEAR_ERROR;
-    i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1);
+    i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, 0.5);
 
     // Start the motors
     actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_START_PROP;
-    i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1);
+    i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, 0.5);
   }
   // Stop the motors
   else if ((bldc_status == DISCO_BLDC_STATUS_RUNNING || bldc_status == DISCO_BLDC_STATUS_RAMPUP) &&
       actuators_disco.motor_rpm < DISCO_BLDC_START_MOTOR_THRESHOLD) {
     actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_STOP_PROP;
-    i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1);
+    i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, 0.5);
   } else if (bldc_status == DISCO_BLDC_STATUS_RUNNING) {
     // Send the commands
     actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_SET_REF_SPEED;
@@ -170,7 +170,7 @@ void actuators_disco_commit(void)
 #pragma GCC diagnostic ignored "-Wcast-qual"
     actuators_disco.i2c_trans.buf[4] = actuators_disco_checksum((uint8_t *)actuators_disco.i2c_trans.buf, 3);
 #pragma GCC diagnostic pop
-    i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 11);
+    i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 11, 0.5);
   }
 
   // Send ABI message

sw/airborne/mcu_periph/i2c.c

@@ -229,73 +229,45 @@ bool i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t,
   return i2c_submit(p, t);
 }
 
-/** Default timeout for blocking I2C transactions */
-#ifndef I2C_BLOCKING_TIMEOUT
-#define I2C_BLOCKING_TIMEOUT 1.f
-#endif
+static enum I2CTransactionStatus i2c_blocking_submit(struct i2c_periph *p, struct i2c_transaction *t, float timeout) {
+  if (!i2c_submit(p, t)) {
+    return I2CTransFailed;
+  }
 
-bool i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t,
-                           uint8_t s_addr, uint8_t len)
+  // Wait for transaction to complete
+  if(pprz_bsem_wait_timeout(&t->bsem, timeout) == 0) {
+    return t->status;
+  } else {
+    return I2CTransFailed;
+  }
+}
+
+enum I2CTransactionStatus i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t,
+                           uint8_t s_addr, uint8_t len, float timeout)
 {
   t->type = I2CTransTx;
   t->slave_addr = s_addr;
   t->len_w = len;
   t->len_r = 0;
-  if (!i2c_submit(p, t)) {
-    return false;
-  }
-
-  // Wait for transaction to complete
-  float start_t = get_sys_time_float();
-  while (t->status == I2CTransPending || t->status == I2CTransRunning) {
-    if (p->spin) p->spin(p);
-    if (get_sys_time_float() - start_t > I2C_BLOCKING_TIMEOUT) {
-      break;  // timeout after 1 second
-    }
-  }
-  return true;
+  return i2c_blocking_submit(p, t, timeout);
 }
 
-bool i2c_blocking_receive(struct i2c_periph *p, struct i2c_transaction *t,
-                          uint8_t s_addr, uint16_t len)
+enum I2CTransactionStatus i2c_blocking_receive(struct i2c_periph *p, struct i2c_transaction *t,
+                          uint8_t s_addr, uint16_t len, float timeout)
 {
   t->type = I2CTransRx;
   t->slave_addr = s_addr;
   t->len_w = 0;
   t->len_r = len;
-  if (!i2c_submit(p, t)) {
-    return false;
-  }
-
-  // Wait for transaction to complete
-  float start_t = get_sys_time_float();
-  while (t->status == I2CTransPending || t->status == I2CTransRunning) {
-    if (p->spin) p->spin(p);
-    if (get_sys_time_float() - start_t > I2C_BLOCKING_TIMEOUT) {
-      break;  // timeout after 1 second
-    }
-  }
-  return true;
+  return i2c_blocking_submit(p, t, timeout);
 }
 
-bool i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t,
-                             uint8_t s_addr, uint8_t len_w, uint16_t len_r)
+enum I2CTransactionStatus i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t,
+                             uint8_t s_addr, uint8_t len_w, uint16_t len_r, float timeout)
 {
   t->type = I2CTransTxRx;
   t->slave_addr = s_addr;
   t->len_w = len_w;
   t->len_r = len_r;
-  if (!i2c_submit(p, t)) {
-    return false;
-  }
-
-  // Wait for transaction to complete
-  float start_t = get_sys_time_float();
-  while (t->status == I2CTransPending || t->status == I2CTransRunning) {
-    if (p->spin) p->spin(p);
-    if (get_sys_time_float() - start_t > I2C_BLOCKING_TIMEOUT) {
-      break;  // timeout after 1 second
-    }
-  }
-  return true;
+  return i2c_blocking_submit(p, t, timeout);
 }

sw/airborne/mcu_periph/i2c.h

@@ -33,6 +33,7 @@
 #include "std.h"
 
 #include "mcu_periph/i2c_arch.h"
+#include "modules/core/threads.h"
 
 /**
  * @addtogroup mcu_periph
@@ -124,6 +125,10 @@ struct i2c_transaction {
   /** Transaction status.
    */
   volatile enum I2CTransactionStatus status;
+
+  /** binary semaphore for blocking functions
+   */
+  pprz_bsem_t bsem;
 };
 
 /** I2C transaction queue length.
@@ -328,10 +333,11 @@ extern bool i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t,
  * @param t i2c transaction
  * @param s_addr slave address
  * @param len number of bytes to transmit
- * @return TRUE if insertion to the transaction queue succeeded
+ * @param timeout timeout in seconds after which the transaction is considered failed
+ * @return the status of the transaction, or I2CTransFailed if insertion to the transaction queue failed
  */
-bool i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t,
-                           uint8_t s_addr, uint8_t len);
+enum I2CTransactionStatus i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t,
+                           uint8_t s_addr, uint8_t len, float timeout);
 
 /** Submit a read only transaction and wait for it to complete.
  * Convenience function which is usually preferred over i2c_submit,
@@ -340,10 +346,11 @@ bool i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t,
  * @param t i2c transaction
  * @param s_addr slave address
  * @param len number of bytes to receive
- * @return TRUE if insertion to the transaction queue succeeded
+ * @param timeout timeout in seconds after which the transaction is considered failed
+ * @return the status of the transaction, or I2CTransFailed if insertion to the transaction queue failed
  */
-bool i2c_blocking_receive(struct i2c_periph *p, struct i2c_transaction *t,
-                          uint8_t s_addr, uint16_t len);
+enum I2CTransactionStatus i2c_blocking_receive(struct i2c_periph *p, struct i2c_transaction *t,
+                          uint8_t s_addr, uint16_t len, float timeout);
 
 /** Submit a write/read transaction and wait for it to complete.
  * Convenience function which is usually preferred over i2c_submit,
@@ -353,10 +360,11 @@ bool i2c_blocking_receive(struct i2c_periph *p, struct i2c_transaction *t,
  * @param s_addr slave address
  * @param len_w number of bytes to transmit
  * @param len_r number of bytes to receive
- * @return TRUE if insertion to the transaction queue succeeded
+ * @param timeout timeout in seconds after which the transaction is considered failed
+ * @return the status of the transaction, or I2CTransFailed if insertion to the transaction queue failed
  */
-bool i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t,
-                             uint8_t s_addr, uint8_t len_w, uint16_t len_r);
+enum I2CTransactionStatus i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t,
+                             uint8_t s_addr, uint8_t len_w, uint16_t len_r, float timeout);
 /** @}*/
 /** @}*/
 

sw/airborne/mcu_periph/spi.c

@@ -164,3 +164,17 @@ extern void spi_slave_init(struct spi_periph *p)
 
 #endif /* SPI_SLAVE */
 
+
+
+enum SPITransactionStatus spi_blocking_transceive(struct spi_periph *p, struct spi_transaction *t, float timeout) {
+  if (!spi_submit(p, t)) {
+    return SPITransFailed;
+  }
+
+  // Wait for transaction to complete
+  if(pprz_bsem_wait_timeout(&t->bsem, timeout) == 0) {
+    return t->status;
+  } else {
+    return SPITransFailed;
+  }
+}

sw/airborne/mcu_periph/spi.h

@@ -35,11 +35,7 @@
 
 #include "mcu_periph/spi_arch.h"
 #include "mcu_periph/sys_time.h"
-
-#ifndef SPI_BLOCKING_TIMEOUT
-#define SPI_BLOCKING_TIMEOUT 1.f
-#endif
-
+#include "modules/core/threads.h"
 
 /**
  * @addtogroup mcu_periph
@@ -160,6 +156,7 @@ struct spi_transaction {
   SPICallback before_cb;        ///< NULL or function called before the transaction
   SPICallback after_cb;         ///< NULL or function called after the transaction
   volatile enum SPITransactionStatus status;
+  pprz_bsem_t bsem;
 };
 
 /** SPI transaction queue length.
@@ -294,21 +291,10 @@ extern bool spi_submit(struct spi_periph *p, struct spi_transaction *t);
 /** Perform a spi transaction (blocking).
  * @param p spi peripheral to be used
  * @param t spi transaction
- * @return TRUE if transaction completed (success or failure)
+ * @param timeout timeout in seconds after which the transaction is considered failed
+ * @return the status of the transaction, or SPITransFailed if insertion to the transaction queue failed
  */
-static inline bool spi_blocking_transceive(struct spi_periph *p, struct spi_transaction *t) {
-  if (!spi_submit(p, t)) {
-    return false;
-  }
-  // Wait for transaction to complete
-  float start_t = get_sys_time_float();
-  while (t->status == SPITransPending || t->status == SPITransRunning) {
-    if (get_sys_time_float() - start_t > SPI_BLOCKING_TIMEOUT) {
-      break;
-    }
-  }
-  return true;
-}
+enum SPITransactionStatus spi_blocking_transceive(struct spi_periph *p, struct spi_transaction *t, float timeout);
 
 /** Select a slave.
  * @param slave slave id

sw/airborne/modules/core/threads.h

@@ -34,6 +34,14 @@ int pprz_mtx_unlock(pprz_mutex_t* mtx);
 
 void pprz_bsem_init(pprz_bsem_t* bsem, bool taken);
 void pprz_bsem_wait(pprz_bsem_t* bsem);
+
+/**
+ * @brief Wait on semaphore no more than timeout.
+ * @param timeout in seconds
+ * @returns 0 on success
+ */
+int pprz_bsem_wait_timeout(pprz_bsem_t* bsem, float timeout);
+
 void pprz_bsem_signal(pprz_bsem_t* bsem);
 
 /**

sw/airborne/modules/meteo/humid_sht_i2c.c

@@ -27,9 +27,6 @@
 
 
 #include "modules/meteo/humid_sht_i2c.h"
-
-#include "mcu_periph/i2c.h"
-#include "mcu_periph/uart.h"
 #include "pprzlink/messages.h"
 #include "modules/datalink/downlink.h"
 
@@ -40,14 +37,146 @@
 
 #define SHT_SLAVE_ADDR 0x80
 
-struct i2c_transaction sht_trans;
-uint8_t sht_status;
-uint8_t sht_serial[8] = {0};
-uint32_t sht_serial1 = 0, sht_serial2 = 0;
-uint16_t humidsht_i2c, tempsht_i2c;
-float fhumidsht_i2c, ftempsht_i2c;
+#define SHT2_WRITE_USER          0xE6
+#define SHT2_READ_USER           0xE7
+#define SHT2_TRIGGER_TEMP        0xF3
+#define SHT2_TRIGGER_HUMID       0xF5
+#define SHT2_SOFT_RESET          0xFE
 
-int8_t humid_sht_crc(volatile uint8_t *data)
+static void humid_sht_thd(void* arg);
+static void sht_read_serial(struct sht_humid_t* sht);
+static int sht_read_temp(struct sht_humid_t* sht);
+static int sht_read_humid(struct sht_humid_t* sht);
+static int8_t humid_sht_crc(volatile uint8_t *data);
+
+
+
+static struct sht_humid_t sht;
+
+
+void humid_sht_init_i2c(void)
+{
+  sht.sht_status = SHT2_UNINIT;
+  pprz_bsem_init(&sht.bsem_sht_status, true);
+  pprz_thread_create(&sht.thd_handle, 512, "humid_sht25", PPRZ_NORMAL_PRIO+1, humid_sht_thd, &sht);
+}
+
+void humid_sht_periodic_i2c(void)
+{
+  if(sht.sht_status == SHT2_READING) {
+    /* send serial number every 30 seconds */
+      RunOnceEvery((4 * 30), DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht.sht_serial1, &sht.sht_serial2));
+  }
+
+  if(pprz_bsem_wait_timeout(&sht.bsem_sht_status, 0) == 0) {
+    DOWNLINK_SEND_SHT_I2C_STATUS(DefaultChannel, DefaultDevice, &sht.humidsht_i2c, &sht.tempsht_i2c, &sht.fhumidsht_i2c, &sht.ftempsht_i2c);
+  }
+}
+
+
+static void humid_sht_thd(void* arg) {
+  sys_time_usleep(100000);
+  struct sht_humid_t* sht = (struct sht_humid_t*)arg;
+
+  /* soft reset sensor */
+  sht->sht_trans.buf[0] = SHT2_SOFT_RESET;
+  while(i2c_blocking_transmit(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 1, 0.5) != I2CTransSuccess) {
+    sys_time_usleep(100000);
+  }
+
+  /* read serial number */
+  sht_read_serial(sht);
+  
+  sht->sht_status = SHT2_READING;
+
+  while(true) {
+    /* read temperature */
+    if(sht_read_temp(sht)) {continue;}
+    /* read humidity */
+    if(sht_read_humid(sht)) {continue;}
+    /* signal semaphore to handle data */
+    pprz_bsem_signal(&sht->bsem_sht_status);
+
+    sys_time_usleep(500000);
+  }
+}
+
+
+static void sht_read_serial(struct sht_humid_t* sht) {
+  uint8_t sht_serial[8] = {0};
+
+  /* request serial number part 1 */
+  sht->sht_trans.buf[0] = 0xFA;
+  sht->sht_trans.buf[1] = 0x0F;
+  while(i2c_blocking_transceive(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 2, 8, 0.5) != I2CTransSuccess) {
+    sys_time_usleep(10000);
+  }
+  /* read serial number part 1 */
+  sht_serial[5] = sht->sht_trans.buf[0];
+  sht_serial[4] = sht->sht_trans.buf[2];
+  sht_serial[3] = sht->sht_trans.buf[4];
+  sht_serial[2] = sht->sht_trans.buf[6];
+  
+  /* request serial number part 2 */
+  sht->sht_trans.buf[0] = 0xFC;
+  sht->sht_trans.buf[1] = 0xC9;
+  while(i2c_blocking_transceive(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 2, 6, 0.5) != I2CTransSuccess) {
+    sys_time_usleep(10000);
+  }
+  /* read serial number part 2 */
+  sht_serial[1] = sht->sht_trans.buf[0];
+  sht_serial[0] = sht->sht_trans.buf[1];
+  sht_serial[7] = sht->sht_trans.buf[3];
+  sht_serial[6] = sht->sht_trans.buf[4];
+
+  sht->sht_serial1 = sht_serial[7] << 24 | sht_serial[6] << 16 | sht_serial[5] << 8 | sht_serial[4];
+  sht->sht_serial2 = sht_serial[3] << 24 | sht_serial[2] << 16 | sht_serial[1] << 8 | sht_serial[0];
+}
+
+static int sht_read_temp(struct sht_humid_t* sht) {
+  /* trigger temp measurement, no master hold */
+  sht->sht_trans.buf[0] = SHT2_TRIGGER_TEMP;
+  if(i2c_blocking_transmit(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 1, 0.5) != I2CTransSuccess ) {
+    return -1;
+  }
+  /* needs 85ms delay from temp trigger measurement */
+  sys_time_usleep(85000);
+  /* read temperature */
+  if(i2c_blocking_receive(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 3, 0.5) != I2CTransSuccess) {
+    return -1;
+  }
+  if (humid_sht_crc(sht->sht_trans.buf) != 0) {
+    /* checksum error*/
+    return -1;
+  }
+
+  sht->tempsht_i2c = ((sht->sht_trans.buf[0] << 8) | sht->sht_trans.buf[1]) & 0xFFFC;
+  sht->ftempsht_i2c = -46.85 + 175.72 / 65536. * sht->tempsht_i2c;
+  return 0;
+}
+
+static int sht_read_humid(struct sht_humid_t* sht) {
+  /* trigger humid measurement, no master hold */
+  sht->sht_trans.buf[0] = SHT2_TRIGGER_HUMID;
+  if(i2c_blocking_transmit(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 1, 0.5) != I2CTransSuccess) {
+    return -1;
+  }
+  /* needs 29ms delay from humid trigger measurement */
+  sys_time_usleep(29000);
+  /* read humidity */
+  if(i2c_blocking_receive(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 3, 0.5) != I2CTransSuccess) {
+    return -1;
+  }
+  if (humid_sht_crc(sht->sht_trans.buf) != 0) {
+    /* checksum error */
+    return -1;
+  }
+  sht->humidsht_i2c = ((sht->sht_trans.buf[0] << 8) | sht->sht_trans.buf[1]) & 0xFFFC;
+  sht->fhumidsht_i2c = -6. + 125. / 65536. * sht->humidsht_i2c;
+  return 0;
+}
+
+static int8_t humid_sht_crc(volatile uint8_t *data)
 {
   uint8_t i, bit, crc = 0;
 
@@ -67,147 +196,3 @@ int8_t humid_sht_crc(volatile uint8_t *data)
     return 0;
   }
 }
-
-void humid_sht_init_i2c(void)
-{
-  sht_status = SHT2_UNINIT;
-}
-
-void humid_sht_periodic_i2c(void)
-{
-  switch (sht_status) {
-
-    case SHT2_UNINIT:
-      /* do soft reset, then wait at least 15ms */
-      sht_status = SHT2_RESET;
-      sht_trans.buf[0] = SHT2_SOFT_RESET;
-      i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1);
-      break;
-
-    case SHT2_SERIAL:
-      /* get serial number part 1 */
-      sht_status = SHT2_SERIAL1;
-      sht_trans.buf[0] = 0xFA;
-      sht_trans.buf[1] = 0x0F;
-      i2c_transceive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 2, 8);
-      break;
-
-    case SHT2_SERIAL1:
-    case SHT2_SERIAL2:
-      break;
-
-    default:
-      /* trigger temp measurement, no master hold */
-      sht_trans.buf[0] = SHT2_TRIGGER_TEMP;
-      sht_status = SHT2_TRIG_TEMP;
-      i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1);
-      /* send serial number every 30 seconds */
-      RunOnceEvery((4 * 30), DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2));
-      break;
-  }
-}
-
-/* needs 85ms delay from temp trigger measurement */
-void humid_sht_p_temp(void)
-{
-  if (sht_status == SHT2_GET_TEMP) {
-    /* get temp */
-    sht_status = SHT2_READ_TEMP;
-    i2c_receive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 3);
-  }
-}
-
-/* needs 29ms delay from humid trigger measurement */
-void humid_sht_p_humid(void)
-{
-  if (sht_status == SHT2_GET_HUMID) {
-    /* read humid */
-    sht_status = SHT2_READ_HUMID;
-    i2c_receive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 3);
-  }
-}
-
-void humid_sht_event_i2c(void)
-{
-  if (sht_trans.status == I2CTransSuccess) {
-    switch (sht_status) {
-
-      case SHT2_TRIG_TEMP:
-        sht_status = SHT2_GET_TEMP;
-        sht_trans.status = I2CTransDone;
-        break;
-
-      case SHT2_READ_TEMP:
-        /* read temperature */
-        tempsht_i2c = (sht_trans.buf[0] << 8) | sht_trans.buf[1];
-        tempsht_i2c &= 0xFFFC;
-        if (humid_sht_crc(sht_trans.buf) == 0) {
-          /* trigger humid measurement, no master hold */
-          sht_trans.buf[0] = SHT2_TRIGGER_HUMID;
-          sht_status = SHT2_TRIG_HUMID;
-          i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1);
-        } else {
-          /* checksum error, restart */
-          sht_status = SHT2_IDLE;
-          sht_trans.status = I2CTransDone;
-        }
-        break;
-
-      case SHT2_TRIG_HUMID:
-        sht_status = SHT2_GET_HUMID;
-        sht_trans.status = I2CTransDone;
-        break;
-
-      case SHT2_READ_HUMID:
-        /* read humidity */
-        humidsht_i2c = (sht_trans.buf[0] << 8) | sht_trans.buf[1];
-        humidsht_i2c &= 0xFFFC;
-        fhumidsht_i2c = -6. + 125. / 65536. * humidsht_i2c;
-        ftempsht_i2c = -46.85 + 175.72 / 65536. * tempsht_i2c;
-
-        sht_status = SHT2_IDLE;
-        sht_trans.status = I2CTransDone;
-
-        if (humid_sht_crc(sht_trans.buf) == 0) {
-          DOWNLINK_SEND_SHT_I2C_STATUS(DefaultChannel, DefaultDevice, &humidsht_i2c, &tempsht_i2c, &fhumidsht_i2c, &ftempsht_i2c);
-        }
-        break;
-
-      case SHT2_RESET:
-        sht_status = SHT2_SERIAL;
-        sht_trans.status = I2CTransDone;
-        break;
-
-      case SHT2_SERIAL1:
-        /* read serial number part 1 */
-        sht_serial[5] = sht_trans.buf[0];
-        sht_serial[4] = sht_trans.buf[2];
-        sht_serial[3] = sht_trans.buf[4];
-        sht_serial[2] = sht_trans.buf[6];
-        /* get serial number part 2 */
-        sht_status = SHT2_SERIAL2;
-        sht_trans.buf[0] = 0xFC;
-        sht_trans.buf[1] = 0xC9;
-        i2c_transceive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 2, 6);
-        break;
-
-      case SHT2_SERIAL2:
-        /* read serial number part 2 */
-        sht_serial[1] = sht_trans.buf[0];
-        sht_serial[0] = sht_trans.buf[1];
-        sht_serial[7] = sht_trans.buf[3];
-        sht_serial[6] = sht_trans.buf[4];
-        sht_serial1 = sht_serial[7] << 24 | sht_serial[6] << 16 | sht_serial[5] << 8 | sht_serial[4];
-        sht_serial2 = sht_serial[3] << 24 | sht_serial[2] << 16 | sht_serial[1] << 8 | sht_serial[0];
-        DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2);
-        sht_status = SHT2_IDLE;
-        sht_trans.status = I2CTransDone;
-        break;
-
-      default:
-        sht_trans.status = I2CTransDone;
-        break;
-    }
-  }
-}
-

sw/airborne/modules/meteo/humid_sht_i2c.h

@@ -1,39 +1,32 @@
 #ifndef HUMID_SHT_I2C_H
 #define HUMID_SHT_I2C_H
 
+#include "mcu_periph/i2c.h"
+#include "modules/core/threads.h"
 #include "std.h"
 
-#define SHT2_WRITE_USER          0xE6
-#define SHT2_READ_USER           0xE7
-#define SHT2_TRIGGER_TEMP        0xF3
-#define SHT2_TRIGGER_HUMID       0xF5
-#define SHT2_SOFT_RESET          0xFE
-
 enum sht_stat_i2c {
   SHT2_UNINIT,
-  SHT2_IDLE,
-  SHT2_RESET,
-  SHT2_SERIAL,
-  SHT2_SERIAL1,
-  SHT2_SERIAL2,
-  SHT2_SET_CONFIG,
-  SHT2_READ_SERIAL,
-  SHT2_TRIG_TEMP,
-  SHT2_GET_TEMP,
-  SHT2_READ_TEMP,
-  SHT2_TRIG_HUMID,
-  SHT2_GET_HUMID,
-  SHT2_READ_HUMID
+  SHT2_READING,
 };
 
-int8_t humid_sht_crc(volatile uint8_t *data);
+
+struct sht_humid_t {
+  struct i2c_transaction sht_trans;
+  enum sht_stat_i2c sht_status;
+  uint32_t sht_serial1;
+  uint32_t sht_serial2;
+  uint16_t humidsht_i2c;
+  uint16_t tempsht_i2c;
+  float fhumidsht_i2c;
+  float ftempsht_i2c;
+
+  pprz_thread_t thd_handle;
+  pprz_bsem_t bsem_sht_status;
+};
+
+
 void humid_sht_init_i2c(void);
 void humid_sht_periodic_i2c(void);
-void humid_sht_p_temp(void);
-void humid_sht_p_humid(void);
-void humid_sht_event_i2c(void);
-
-extern uint16_t humidsht_i2c, tempsht_i2c;
-extern float fhumidsht_i2c, ftempsht_i2c;
 
 #endif

sw/airborne/modules/sensors/encoder_amt22.c

@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) 2025 Flo&Fab <name.surname@enac.fr>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/sensors/encoder_amt22.c"
+ * @author Flo&Fab <name.surname@enac.fr>
+ * Driver for AMT22 encoder from CUI devices.
+ */
+
+#include "modules/sensors/encoder_amt22.h"
+#include "peripherals/amt22.h"
+#include "modules/datalink/downlink.h"
+
+static amt22_t amt22;
+static amt22_config_t amt22_conf = {
+  .p = &AMT22_SPI_DEV,
+  .slave_idx = AMT22_SPI_SLAVE_IDX,
+  .type = AMT22_12_SINGLE,
+};
+
+void encoder_amt22_init(void)
+{
+  amt22_init(&amt22, &amt22_conf);
+}
+
+void encoder_amt22_periodic(void)
+{
+  amt22_periodic(&amt22);
+  float f[2] = {amt22_get_position(&amt22), amt22_get_turns(&amt22)};
+  DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 2, f);
+}

sw/airborne/modules/sensors/encoder_amt22.h

@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2025 Flo&Fab <name.surname@enac.fr>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/sensors/encoder_amt22.h"
+ * @author Flo&Fab <name.surname@enac.fr>
+ * Driver for AMT22 encoder from CUI devices.
+ */
+
+#ifndef ENCODER_AMT22_H
+#define ENCODER_AMT22_H
+
+extern void encoder_amt22_init(void);
+extern void encoder_amt22_periodic(void);
+
+#endif  // ENCODER_AMT22_H

sw/airborne/peripherals/amt22.c

@@ -0,0 +1,133 @@
+#include "amt22.h"
+
+
+#ifndef AMT22_SPI_CDIV
+#define AMT22_SPI_CDIV SPIDiv256
+#endif
+
+static bool amt22_checkbit(uint8_t p0, uint8_t p1);
+
+static void amt22_thd(void* arg);
+
+void amt22_init(amt22_t* amt, amt22_config_t* conf) {
+  amt->config = conf;
+  // Set up SPI peripheral and transaction
+  amt->trans.slave_idx = amt->config->slave_idx;
+  amt->trans.input_buf = amt->spi_input_buf;
+  amt->trans.output_buf = amt->spi_output_buf;
+  amt->trans.select = SPISelectUnselect;
+  amt->trans.cpol = SPICpolIdleLow;
+  amt->trans.cpha = SPICphaEdge1;
+  amt->trans.dss = SPIDss8bit;
+  amt->trans.bitorder = SPIMSBFirst;
+  amt->trans.cdiv = AMT22_SPI_CDIV;
+  amt->trans.before_cb = NULL;
+  amt->trans.after_cb = NULL;
+  amt->trans.status = SPITransDone;
+  amt->position = 0;
+  amt->turns = 0;
+
+
+  if(amt->config->type == AMT22_12_SINGLE || amt->config->type == AMT22_14_SINGLE) {
+    //single turn
+    amt->trans.output_buf[0] = 0x00;
+    amt->trans.output_buf[1] = 0x00;
+    amt->trans.output_length = 2;
+    amt->trans.input_length = 2;
+  } else {
+    //multi_turn
+    amt->trans.output_buf[0] = 0x00;
+    amt->trans.output_buf[1] = 0xA0;
+    amt->trans.output_buf[2] = 0x00;
+    amt->trans.output_buf[3] = 0x00;
+    amt->trans.output_length = 4;
+    amt->trans.input_length = 4;
+  }
+
+  pprz_bsem_init(&amt->bsem_amt22_read, true);
+  pprz_mtx_init(&amt->mtx);
+  pprz_thread_create(&amt->thd_handle, 512, "amt22", PPRZ_NORMAL_PRIO+1, amt22_thd, amt);
+}
+
+void amt22_periodic(amt22_t* amt) {
+  // trigger read
+  pprz_bsem_signal(&amt->bsem_amt22_read);
+}
+
+
+bool amt22_read(amt22_t* amt) {
+  if(spi_blocking_transceive(amt->config->p, &amt->trans, 0.5) != SPITransSuccess) {
+    return false;
+  }
+
+  uint8_t p0 = amt->trans.input_buf[0];
+  uint8_t p1 = amt->trans.input_buf[1];
+
+  if(!amt22_checkbit(p0,p1)) {
+    return false;
+  }
+
+  uint16_t position = (p0 << 8 | p1) & 0x3fff;
+  int16_t turns = 0;
+
+  // 12 bits so shift 2
+  if(amt->config->type == AMT22_12_SINGLE || amt->config->type == AMT22_12_MULTI) {
+    position >>= 2;
+  }
+
+  if(amt->config->type == AMT22_12_MULTI || amt->config->type == AMT22_14_MULTI) {
+    uint8_t t0 = amt->trans.input_buf[2];
+    uint8_t t1 = amt->trans.input_buf[3];
+    turns = (t0 << 8 | t1);
+  }
+
+  pprz_mtx_lock(&amt->mtx);
+  amt->position = position;
+  amt->turns = turns;
+  pprz_mtx_unlock(&amt->mtx);
+  return true;
+}
+
+static bool amt22_checkbit(uint8_t p0, uint8_t p1) {
+  uint16_t data = ((p0 << 8 | p1) & 0x3fff) >> 2;
+  uint8_t odd = 0;
+  uint8_t even = 0;
+  while (data)
+  {
+    even ^= data & 1;
+    data >>= 1;
+    odd ^= data & 1;
+    data >>= 1;
+  }
+  even = !even;
+  odd = !odd;
+  if((even == ((p0 & 0x40) >> 6)) && (odd == ((p0 & 0x80) >> 7))) {
+    return 1;
+  } else {
+    return 0;
+  }
+}
+
+uint16_t amt22_get_position(amt22_t* amt) {
+  pprz_mtx_lock(&amt->mtx);
+  uint16_t pos = amt->position;
+  pprz_mtx_unlock(&amt->mtx);
+  return pos;
+}
+
+int16_t amt22_get_turns(amt22_t* amt) {
+  pprz_mtx_lock(&amt->mtx);
+  int16_t turns = amt->turns;
+  pprz_mtx_unlock(&amt->mtx);
+  return turns;
+}
+
+
+static void amt22_thd(void* arg) {
+  amt22_t* amt = (amt22_t*)arg;
+
+  while(true) {
+    pprz_bsem_wait(&amt->bsem_amt22_read);
+    amt22_read(amt);
+  }
+}

sw/airborne/peripherals/amt22.h

@@ -0,0 +1,38 @@
+#pragma once
+
+#include "mcu_periph/spi.h"
+#include "modules/core/threads.h"
+
+enum amt22_type {
+  AMT22_12_SINGLE,  ///< 12-bits, single-turn
+  AMT22_14_SINGLE,  ///< 14-bits, single-turn
+  AMT22_12_MULTI,   ///< 12-bits, multi-turn
+  AMT22_14_MULTI,   ///< 14-bits, multi-turn
+};
+
+typedef struct {
+  struct spi_periph *p;
+  uint8_t slave_idx;
+  enum amt22_type type;
+} amt22_config_t;
+
+typedef struct {
+  amt22_config_t *config;
+
+  // private
+  struct spi_transaction trans;
+  volatile uint8_t spi_input_buf[4];
+  volatile uint8_t spi_output_buf[4];
+
+  uint16_t position;
+  int16_t turns;
+
+  pprz_thread_t thd_handle;
+  pprz_bsem_t bsem_amt22_read;
+  pprz_mutex_t mtx;
+} amt22_t;
+
+void amt22_init(amt22_t* amt22, amt22_config_t* conf);
+void amt22_periodic(amt22_t* amt22);
+uint16_t amt22_get_position(amt22_t* amt22);
+int16_t amt22_get_turns(amt22_t* amt22);

sw/airborne/peripherals/pca95xx.c

@@ -50,7 +50,7 @@ bool pca95xx_configure(struct pca95xx *dev, uint8_t val, bool blocking)
   dev->i2c_trans.buf[0] = PCA95XX_CONFIG_REG;
   dev->i2c_trans.buf[1] = val;
   if (blocking) {
-    return i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2);
+    return i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2, 0.5) == I2CTransSuccess;
   } else {
     return i2c_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2);
   }
@@ -68,7 +68,7 @@ bool pca95xx_set_output(struct pca95xx *dev, uint8_t mask, bool blocking)
   dev->i2c_trans.buf[0] = PCA95XX_OUTPUT_REG;
   dev->i2c_trans.buf[1] = mask;
   if (blocking) {
-    return i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2);
+    return i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2, 0.5) == I2CTransSuccess;
   } else {
     return i2c_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2);
   }

sw/airborne/peripherals/vl53l1_platform.c

@@ -47,7 +47,7 @@ int8_t VL53L1_WriteMulti(VL53L1_DEV dev, uint16_t index, uint8_t *pdata, uint32_
   dev->i2c_trans.buf[1] = (index & 0x00FF);
   memcpy((uint8_t *) dev->i2c_trans.buf + 2, pdata, count);
   return !i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans,
-                                dev->i2c_trans.slave_addr, 2 + count);
+                                dev->i2c_trans.slave_addr, 2 + count, 1.f);
 }
 
 int8_t VL53L1_ReadMulti(VL53L1_DEV dev, uint16_t index, uint8_t *pdata, uint32_t count)