add i2c transaction cue to the linux arch (#2254)

* add i2c transaction cue to the linux arch
* add thread name to most pthreads
* split each i2c to different thread and replace polling with signaling
* code cleanup and fix code style

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

@@ -1,6 +1,7 @@
 /*
  *
  * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
+ *               2018 Kirk Scheper <kirkscheper@gmail.com>
  *
  * This file is part of paparazzi.
  *
@@ -34,6 +35,35 @@
 #include <linux/i2c-dev.h>
 #include <errno.h>
 
+#ifndef _GNU_SOURCE
+// for pthread_setname_np
+#define _GNU_SOURCE
+#endif
+#include <pthread.h>
+#include "rt_priority.h"
+
+#ifndef I2C_THREAD_PRIO
+#define I2C_THREAD_PRIO 10
+#endif
+
+static void *i2c_thread(void *thread_data);
+
+// private I2C init structure
+struct i2c_thread_t {
+  pthread_mutex_t mutex;
+  pthread_cond_t condition;
+};
+
+static void i2c_arch_init(struct i2c_periph *p)
+{
+  pthread_t tid;
+  if (pthread_create(&tid, NULL, i2c_thread, (void *)p) != 0) {
+    fprintf(stderr, "i2c_arch_init: Could not create I2C thread.\n");
+    return;
+  }
+  pthread_setname_np(tid, "pprz_i2c_thread");
+}
+
 void i2c_event(void)
 {
 }
@@ -47,71 +77,132 @@ bool i2c_idle(struct i2c_periph *p __attribute__((unused)))
   return true;
 }
 
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wcast-qual"
 bool i2c_submit(struct i2c_periph *p, struct i2c_transaction *t)
 {
-  int file = (int)p->reg_addr;
+  // get mutex lock and condition
+  pthread_mutex_t *mutex = &(((struct i2c_thread_t *)(p->init_struct))->mutex);
+  pthread_cond_t *condition = &(((struct i2c_thread_t *)(p->init_struct))->condition);
 
+  uint8_t next_idx;
+  pthread_mutex_lock(mutex);
+  next_idx = (p->trans_insert_idx + 1) % I2C_TRANSACTION_QUEUE_LEN;
+  if (next_idx == p->trans_extract_idx) {
+    // queue full
+    p->errors->queue_full_cnt++;
+    t->status = I2CTransFailed;
+    pthread_mutex_unlock(mutex);
+    return false;
+  }
+
+  t->status = I2CTransPending;
+
+  /* put transaction in queue */
+  p->trans[p->trans_insert_idx] = t;
+  p->trans_insert_idx = next_idx;
+
+  /* wake handler thread */
+  pthread_cond_signal(condition);
+  pthread_mutex_unlock(mutex);
+
+  return true;
+}
+
+/*
+ * Transactions handler thread
+ */
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wcast-qual"
+static void *i2c_thread(void *data)
+{
   struct i2c_msg trx_msgs[2];
   struct i2c_rdwr_ioctl_data trx_data = {
     .msgs = trx_msgs,
     .nmsgs = 2
   };
-  // Switch the different transaction types
-  switch (t->type) {
-      // Just transmitting
-    case I2CTransTx:
-      // Set the slave address, converted to 7 bit
-      ioctl(file, I2C_SLAVE, t->slave_addr >> 1);
-      if (write(file, (uint8_t *)t->buf, t->len_w) < 0) {
-        /* if write failed, increment error counter queue_full_cnt */
-        p->errors->queue_full_cnt++;
-        t->status = I2CTransFailed;
-        return true;
-      }
-      break;
-      // Just reading
-    case I2CTransRx:
-      // Set the slave address, converted to 7 bit
-      ioctl(file, I2C_SLAVE, t->slave_addr >> 1);
-      if (read(file, (uint8_t *)t->buf, t->len_r) < 0) {
-        /* if read failed, increment error counter ack_fail_cnt */
-        p->errors->ack_fail_cnt++;
-        t->status = I2CTransFailed;
-        return true;
-      }
-      break;
-      // First Transmit and then read with repeated start
-    case I2CTransTxRx:
-      trx_msgs[0].addr = t->slave_addr >> 1;
-      trx_msgs[0].flags = 0; /* tx */
-      trx_msgs[0].len = t->len_w;
-      trx_msgs[0].buf = (void*) t->buf;
-      trx_msgs[1].addr = t->slave_addr >> 1;
-      trx_msgs[1].flags = I2C_M_RD;
-      trx_msgs[1].len = t->len_r;
-      trx_msgs[1].buf = (void*) t->buf;
-      if (ioctl(file, I2C_RDWR, &trx_data) < 0) {
-        /* if write/read failed, increment error counter miss_start_stop_cnt */
-        p->errors->miss_start_stop_cnt++;
-        t->status = I2CTransFailed;
-        return true;
-      }
-      break;
-    default:
-      break;
-  }
 
-  // Successfull transfer
-  t->status = I2CTransSuccess;
-  return true;
+  get_rt_prio(I2C_THREAD_PRIO);
+
+  struct i2c_periph *p = (struct i2c_periph *)data;
+  pthread_mutex_t *mutex = &(((struct i2c_thread_t *)(p->init_struct))->mutex);
+  pthread_cond_t *condition = &(((struct i2c_thread_t *)(p->init_struct))->condition);
+
+  while (1) {
+    /* wait for data to transfer */
+    pthread_mutex_lock(mutex);
+    if (p->trans_insert_idx == p->trans_extract_idx) {
+      pthread_cond_wait(condition, mutex);
+    }
+
+    int fd = (int)p->reg_addr;
+    struct i2c_transaction *t = p->trans[p->trans_extract_idx];
+    pthread_mutex_unlock(mutex);
+
+    // Switch the different transaction types
+    switch (t->type) {
+      // Just transmitting
+      case I2CTransTx:
+        // Set the slave address, converted to 7 bit
+        ioctl(fd, I2C_SLAVE, t->slave_addr >> 1);
+        if (write(fd, (uint8_t *)t->buf, t->len_w) < 0) {
+          /* if write failed, increment error counter ack_fail_cnt */
+          pthread_mutex_lock(mutex);
+          p->errors->ack_fail_cnt++;
+          pthread_mutex_unlock(mutex);
+          t->status = I2CTransFailed;
+        } else {
+          t->status = I2CTransSuccess;
+        }
+        break;
+      // Just reading
+      case I2CTransRx:
+        // Set the slave address, converted to 7 bit
+        ioctl(fd, I2C_SLAVE, t->slave_addr >> 1);
+        if (read(fd, (uint8_t *)t->buf, t->len_r) < 0) {
+          /* if read failed, increment error counter arb_lost_cnt */
+          pthread_mutex_lock(mutex);
+          p->errors->arb_lost_cnt++;
+          pthread_mutex_unlock(mutex);
+          t->status = I2CTransFailed;
+        } else {
+          t->status = I2CTransSuccess;
+        }
+        break;
+      // First Transmit and then read with repeated start
+      case I2CTransTxRx:
+        trx_msgs[0].addr = t->slave_addr >> 1;
+        trx_msgs[0].flags = 0; /* tx */
+        trx_msgs[0].len = t->len_w;
+        trx_msgs[0].buf = (void *) t->buf;
+        trx_msgs[1].addr = t->slave_addr >> 1;
+        trx_msgs[1].flags = I2C_M_RD;
+        trx_msgs[1].len = t->len_r;
+        trx_msgs[1].buf = (void *) t->buf;
+        if (ioctl(fd, I2C_RDWR, &trx_data) < 0) {
+          /* if write/read failed, increment error counter miss_start_stop_cnt */
+          pthread_mutex_lock(mutex);
+          p->errors->miss_start_stop_cnt++;
+          pthread_mutex_unlock(mutex);
+          t->status = I2CTransFailed;
+        } else {
+          t->status = I2CTransSuccess;
+        }
+        break;
+      default:
+        t->status = I2CTransFailed;
+        break;
+    }
+
+    pthread_mutex_lock(mutex);
+    p->trans_extract_idx = (p->trans_extract_idx + 1) % I2C_TRANSACTION_QUEUE_LEN;
+    pthread_mutex_unlock(mutex);
+  }
+  return NULL;
 }
 #pragma GCC diagnostic pop
 
-
 #if USE_I2C0
 struct i2c_errors i2c0_errors;
+struct i2c_thread_t i2c0_thread;
 
 void i2c0_hw_init(void)
 {
@@ -120,11 +211,18 @@ void i2c0_hw_init(void)
 
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c0_errors);
+
+  pthread_mutex_init(&i2c0_thread.mutex, NULL);
+  pthread_cond_init(&i2c0_thread.condition, NULL);
+  i2c0.init_struct = (void *)(&i2c0_thread);
+
+  i2c_arch_init(&i2c0);
 }
 #endif
 
 #if USE_I2C1
 struct i2c_errors i2c1_errors;
+struct i2c_thread_t i2c1_thread;
 
 void i2c1_hw_init(void)
 {
@@ -133,11 +231,18 @@ void i2c1_hw_init(void)
 
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c1_errors);
+
+  pthread_mutex_init(&i2c1_thread.mutex, NULL);
+  pthread_cond_init(&i2c1_thread.condition, NULL);
+  i2c1.init_struct = (void *)(&i2c1_thread);
+
+  i2c_arch_init(&i2c1);
 }
 #endif
 
 #if USE_I2C2
 struct i2c_errors i2c2_errors;
+struct i2c_thread_t i2c2_thread;
 
 void i2c2_hw_init(void)
 {
@@ -146,11 +251,18 @@ void i2c2_hw_init(void)
 
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c2_errors);
+
+  pthread_mutex_init(&i2c2_thread.mutex, NULL);
+  pthread_cond_init(&i2c2_thread.condition, NULL);
+  i2c2.init_struct = (void *)(&i2c2_thread);
+
+  i2c_arch_init(&i2c2);
 }
 #endif
 
 #if USE_I2C3
 struct i2c_errors i2c3_errors;
+struct i2c_thread_t i2c3_thread;
 
 void i2c3_hw_init(void)
 {
@@ -159,5 +271,11 @@ void i2c3_hw_init(void)
 
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c3_errors);
+
+  pthread_mutex_init(&i2c3_thread.mutex, NULL);
+  pthread_cond_init(&i2c3_thread.condition, NULL);
+  i2c3.init_struct = (void *)(&i2c3_thread);
+
+  i2c_arch_init(&i2c3);
 }
 #endif

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

@@ -26,6 +26,10 @@
 
 #include "mcu_periph/sys_time.h"
 #include <stdio.h>
+#ifndef _GNU_SOURCE
+// for pthread_setname_np
+#define _GNU_SOURCE
+#endif
 #include <pthread.h>
 #include <sys/timerfd.h>
 #include <time.h>
@@ -39,7 +43,6 @@
 #define SYS_TIME_THREAD_PRIO 29
 #endif
 
-pthread_t sys_time_thread;
 static struct timespec startup_time;
 
 static void sys_tick_handler(void);
@@ -77,8 +80,8 @@ void *sys_time_thread_main(void *data)
   while (1) {
     unsigned long long missed;
     /* Wait for the next timer event. If we have missed any the
-	   number is written to "missed" */
-	int r = read(fd, &missed, sizeof(missed));
+     number is written to "missed" */
+    int r = read(fd, &missed, sizeof(missed));
     if (r == -1) {
       perror("Couldn't read timer!");
     }
@@ -98,11 +101,13 @@ void sys_time_arch_init(void)
 
   clock_gettime(CLOCK_MONOTONIC, &startup_time);
 
-  int ret = pthread_create(&sys_time_thread, NULL, sys_time_thread_main, NULL);
+  pthread_t tid;
+  int ret = pthread_create(&tid, NULL, sys_time_thread_main, NULL);
   if (ret) {
     perror("Could not setup sys_time_thread");
     return;
   }
+  pthread_setname_np(tid, "pprz_sys_time_thread");
 }
 
 static void sys_tick_handler(void)
@@ -165,7 +170,7 @@ uint32_t get_sys_time_usec(void)
     d_sec -= 1;
     d_nsec += 1000000000L;
   }
-  return d_sec * 1000000 + d_nsec/1000;
+  return d_sec * 1000000 + d_nsec / 1000;
 }
 
 /**
@@ -187,5 +192,5 @@ uint32_t get_sys_time_msec(void)
     d_sec -= 1;
     d_nsec += 1000000000L;
   }
-  return d_sec * 1000 + d_nsec/1000000;
+  return d_sec * 1000 + d_nsec / 1000000;
 }

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

@@ -37,6 +37,10 @@
 #include "serial_port.h"
 #include "rt_priority.h"
 
+#ifndef _GNU_SOURCE
+// for pthread_setname_np
+#define _GNU_SOURCE
+#endif
 #include <pthread.h>
 #include <sys/select.h>
 
@@ -60,6 +64,7 @@ void uart_arch_init(void)
     fprintf(stderr, "uart_arch_init: Could not create UART reading thread.\n");
     return;
   }
+  pthread_setname_np(tid, "pprz_uart_thread");
 }
 
 static void *uart_thread(void *data __attribute__((unused)))
@@ -74,13 +79,13 @@ static void *uart_thread(void *data __attribute__((unused)))
   /* clear the fd list */
   FD_ZERO(&fds_master);
   /* add used fds */
-  int __attribute__ ((unused)) fd;
+  int __attribute__((unused)) fd;
 #if USE_UART0
   if (uart0.reg_addr != NULL) {
     fd = ((struct SerialPort *)uart0.reg_addr)->fd;
     FD_SET(fd, &fds_master);
     if (fd > fdmax) {
-      fdmax =fd;
+      fdmax = fd;
     }
   }
 #endif
@@ -89,7 +94,7 @@ static void *uart_thread(void *data __attribute__((unused)))
     fd = ((struct SerialPort *)uart1.reg_addr)->fd;
     FD_SET(fd, &fds_master);
     if (fd > fdmax) {
-      fdmax =fd;
+      fdmax = fd;
     }
   }
 #endif
@@ -98,7 +103,7 @@ static void *uart_thread(void *data __attribute__((unused)))
     fd = ((struct SerialPort *)uart2.reg_addr)->fd;
     FD_SET(fd, &fds_master);
     if (fd > fdmax) {
-      fdmax =fd;
+      fdmax = fd;
     }
   }
 #endif
@@ -107,7 +112,7 @@ static void *uart_thread(void *data __attribute__((unused)))
     fd = ((struct SerialPort *)uart3.reg_addr)->fd;
     FD_SET(fd, &fds_master);
     if (fd > fdmax) {
-      fdmax =fd;
+      fdmax = fd;
     }
   }
 #endif
@@ -116,7 +121,7 @@ static void *uart_thread(void *data __attribute__((unused)))
     fd = ((struct SerialPort *)uart4.reg_addr)->fd;
     FD_SET(fd, &fds_master);
     if (fd > fdmax) {
-      fdmax =fd;
+      fdmax = fd;
     }
   }
 #endif
@@ -125,7 +130,7 @@ static void *uart_thread(void *data __attribute__((unused)))
     fd = ((struct SerialPort *)uart5.reg_addr)->fd;
     FD_SET(fd, &fds_master);
     if (fd > fdmax) {
-      fdmax =fd;
+      fdmax = fd;
     }
   }
 #endif
@@ -134,7 +139,7 @@ static void *uart_thread(void *data __attribute__((unused)))
     fd = ((struct SerialPort *)uart6.reg_addr)->fd;
     FD_SET(fd, &fds_master);
     if (fd > fdmax) {
-      fdmax =fd;
+      fdmax = fd;
     }
   }
 #endif
@@ -148,8 +153,7 @@ static void *uart_thread(void *data __attribute__((unused)))
 
     if (select(fdmax + 1, &fds, NULL, NULL, NULL) < 0) {
       fprintf(stderr, "uart_thread: select failed!");
-    }
-    else {
+    } else {
 #if USE_UART0
       if (uart0.reg_addr != NULL) {
         fd = ((struct SerialPort *)uart0.reg_addr)->fd;
@@ -275,9 +279,9 @@ void uart_put_byte(struct uart_periph *periph, long fd __attribute__((unused)),
   struct SerialPort *port = (struct SerialPort *)(periph->reg_addr);
 
   int ret = 0;
-  do{
+  do {
     ret = write((int)(port->fd), &data, 1);
-  } while(ret < 1 && errno == EAGAIN); //FIXME: max retry
+  } while (ret < 1 && errno == EAGAIN); //FIXME: max retry
 
   if (ret < 1) {
     TRACE("uart_put_byte: write %d failed [%d: %s]\n", data, ret, strerror(errno));
@@ -285,7 +289,7 @@ void uart_put_byte(struct uart_periph *periph, long fd __attribute__((unused)),
 }
 
 
-static void __attribute__ ((unused)) uart_receive_handler(struct uart_periph *periph)
+static void __attribute__((unused)) uart_receive_handler(struct uart_periph *periph)
 {
   unsigned char c = 'D';
 
@@ -303,8 +307,7 @@ static void __attribute__ ((unused)) uart_receive_handler(struct uart_periph *pe
     if (temp != periph->rx_extract_idx) {
       periph->rx_buf[periph->rx_insert_idx] = c;
       periph->rx_insert_idx = temp;  // update insert index
-    }
-    else {
+    } else {
       TRACE("uart_receive_handler: rx_buf full! discarding received byte: %x %c\n", c, c);
     }
   }

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

@@ -29,6 +29,10 @@
 #include <stdio.h>
 #include <errno.h>
 
+#ifndef _GNU_SOURCE
+// for pthread_setname_np
+#define _GNU_SOURCE
+#endif
 #include <pthread.h>
 #include <sys/select.h>
 
@@ -60,6 +64,7 @@ void udp_arch_init(void)
     fprintf(stderr, "udp_arch_init: Could not create UDP reading thread.\n");
     return;
   }
+  pthread_setname_np(tid, "pprz_udp_thread");
 }
 
 /**
@@ -108,8 +113,8 @@ uint8_t udp_getch(struct udp_periph *p)
  */
 void udp_receive(struct udp_periph *p)
 {
-  if (p == NULL) return;
-  if (p->network == NULL) return;
+  if (p == NULL) { return; }
+  if (p->network == NULL) { return; }
 
   int16_t i;
   int16_t available = UDP_RX_BUFFER_SIZE - udp_char_available(p);
@@ -141,8 +146,8 @@ void udp_receive(struct udp_periph *p)
  */
 void udp_send_message(struct udp_periph *p, long fd __attribute__((unused)))
 {
-  if (p == NULL) return;
-  if (p->network == NULL) return;
+  if (p == NULL) { return; }
+  if (p->network == NULL) { return; }
 
   struct UdpSocket *sock = (struct UdpSocket *) p->network;
 
@@ -152,8 +157,7 @@ void udp_send_message(struct udp_periph *p, long fd __attribute__((unused)))
     if (bytes_sent != p->tx_insert_idx) {
       if (bytes_sent < 0) {
         perror("udp_send_message failed");
-      }
-      else {
+      } else {
         fprintf(stderr, "udp_send_message: only sent %d bytes instead of %d\n",
                 (int)bytes_sent, p->tx_insert_idx);
       }
@@ -167,8 +171,8 @@ void udp_send_message(struct udp_periph *p, long fd __attribute__((unused)))
  */
 void udp_send_raw(struct udp_periph *p, long fd __attribute__((unused)), uint8_t *buffer, uint16_t size)
 {
-  if (p == NULL) return;
-  if (p->network == NULL) return;
+  if (p == NULL) { return; }
+  if (p->network == NULL) { return; }
 
   struct UdpSocket *sock = (struct UdpSocket *) p->network;
   ssize_t test __attribute__((unused)) = sendto(sock->sockfd, buffer, size, MSG_DONTWAIT,
@@ -195,21 +199,21 @@ static void *udp_thread(void *data __attribute__((unused)))
   fd = ((struct UdpSocket *)udp0.network)->sockfd;
   FD_SET(fd, &socks_master);
   if (fd > fdmax) {
-    fdmax =fd;
+    fdmax = fd;
   }
 #endif
 #if USE_UDP1
   fd = ((struct UdpSocket *)udp1.network)->sockfd;
   FD_SET(fd, &socks_master);
   if (fd > fdmax) {
-    fdmax =fd;
+    fdmax = fd;
   }
 #endif
 #if USE_UDP2
   fd = ((struct UdpSocket *)udp2.network)->sockfd;
   FD_SET(fd, &socks_master);
   if (fd > fdmax) {
-    fdmax =fd;
+    fdmax = fd;
   }
 #endif
 
@@ -222,8 +226,7 @@ static void *udp_thread(void *data __attribute__((unused)))
 
     if (select(fdmax + 1, &socks, NULL, NULL, NULL) < 0) {
       fprintf(stderr, "udp_thread: select failed!");
-    }
-    else {
+    } else {
 #if USE_UDP0
       fd = ((struct UdpSocket *)udp0.network)->sockfd;
       if (FD_ISSET(fd, &socks)) {

sw/airborne/boards/ardrone/navdata.c

@@ -39,6 +39,10 @@
 #include <math.h>
 #include <errno.h>
 #include <assert.h>
+#ifndef _GNU_SOURCE
+// for pthread_setname_np
+#define _GNU_SOURCE
+#endif
 #include <pthread.h>
 
 #include "std.h"
@@ -236,6 +240,7 @@ bool navdata_init()
     printf("[navdata] Could not create navdata reading thread!\n");
     return false;
   }
+  pthread_setname_np(navdata_thread, "pprz_navdata_thread");
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ARDRONE_NAVDATA, send_navdata);

sw/airborne/boards/swing/baro_board.c

@@ -32,6 +32,10 @@
 #include <string.h>
 #include <fcntl.h>
 #include <unistd.h>
+#ifndef _GNU_SOURCE
+// for pthread_setname_np
+#define _GNU_SOURCE
+#endif
 #include <pthread.h>
 #include <linux/input.h>
 
@@ -81,6 +85,7 @@ void baro_init(void)
   if (pthread_create(&baro_thread, NULL, baro_read, NULL) != 0) {
     printf("[swing_board] Could not create baro reading thread!\n");
   }
+  pthread_setname_np(baro_thread, "pprz_baro_thread");
 
 }
 

sw/airborne/boards/swing/board.c

@@ -32,6 +32,10 @@
 #include <string.h>
 #include <fcntl.h>
 #include <unistd.h>
+#ifndef _GNU_SOURCE
+// for pthread_setname_np
+#define _GNU_SOURCE
+#endif
 #include <pthread.h>
 #include <linux/input.h>
 #include "subsystems/electrical.h"
@@ -48,11 +52,10 @@ static void *bat_read(void *data __attribute__((unused)))
     /* Open the command for reading. */
     fp = popen("cat /sys/devices/platform/p6-spi.2/spi2.0/vbat", "r");
     if (fp == NULL) {
-      printf("Failed to read battery\n" );
-    }
-    else {
+      printf("Failed to read battery\n");
+    } else {
       /* Read the output a line at a time - output it. */
-      while (fgets(path, sizeof(path)-1, fp) != NULL) {
+      while (fgets(path, sizeof(path) - 1, fp) != NULL) {
         int raw_bat = atoi(path);
         // convert to decivolt
         // from /bin/mcu_vbat.sh: MILLIVOLTS_VALUE=$(( ($RAW_VALUE * 4250) / 1023 ))
@@ -116,12 +119,14 @@ void board_init(void)
   if (pthread_create(&bat_thread, NULL, bat_read, NULL) != 0) {
     printf("[swing_board] Could not create battery reading thread!\n");
   }
+  pthread_setname_np(bat_thread, "pprz_bat_thread");
 
   /* Start button reading thread */
   pthread_t button_thread;
   if (pthread_create(&button_thread, NULL, button_read, NULL) != 0) {
     printf("[swing_board] Could not create button reading thread!\n");
   }
+  pthread_setname_np(button_thread, "pprz_button_thread");
 
 }
 

sw/airborne/mcu_periph/i2c.c

@@ -247,6 +247,7 @@ void i2c_init(struct i2c_periph *p)
   p->trans_insert_idx = 0;
   p->trans_extract_idx = 0;
   p->status = I2CIdle;
+  p->reg_addr = NULL;
 
 #if PERIODIC_TELEMETRY
   // the first to register do it for the others
@@ -256,7 +257,7 @@ void i2c_init(struct i2c_periph *p)
 
 
 bool i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t,
-                    uint8_t s_addr, uint8_t len)
+                  uint8_t s_addr, uint8_t len)
 {
   t->type = I2CTransTx;
   t->slave_addr = s_addr;
@@ -266,7 +267,7 @@ bool i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t,
 }
 
 bool i2c_receive(struct i2c_periph *p, struct i2c_transaction *t,
-                   uint8_t s_addr, uint16_t len)
+                 uint8_t s_addr, uint16_t len)
 {
   t->type = I2CTransRx;
   t->slave_addr = s_addr;
@@ -276,7 +277,7 @@ bool i2c_receive(struct i2c_periph *p, struct i2c_transaction *t,
 }
 
 bool i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t,
-                      uint8_t s_addr, uint8_t len_w, uint16_t len_r)
+                    uint8_t s_addr, uint8_t len_w, uint16_t len_r)
 {
   t->type = I2CTransTxRx;
   t->slave_addr = s_addr;

sw/airborne/modules/computer_vision/video_thread.c

@@ -50,6 +50,10 @@
 #endif
 
 // Threaded computer vision
+#ifndef _GNU_SOURCE
+// for pthread_setname_np
+#define _GNU_SOURCE
+#endif
 #include <pthread.h>
 #include "rt_priority.h"
 
@@ -235,6 +239,7 @@ static void start_video_thread(struct video_config_t *camera)
       printf("[viewvideo] Could not create streaming thread for camera %s: Reason: %d.\n", camera->dev_name, errno);
       return;
     }
+    pthread_setname_np(tid, "pprz_camera_thread");
   }
 }
 

sw/airborne/modules/sonar/sonar_bebop.c

@@ -37,6 +37,10 @@
 #include "mcu_periph/adc.h"
 #include "mcu_periph/spi.h"
 #include "subsystems/abi.h"
+#ifndef _GNU_SOURCE
+// for pthread_setname_np
+#define _GNU_SOURCE
+#endif
 #include <pthread.h>
 #include "subsystems/datalink/downlink.h"
 
@@ -90,9 +94,6 @@ struct SonarBebop sonar_bebop;
 
 static struct spi_transaction sonar_bebop_spi_t;
 void *sonar_bebop_read(void *data);
-#ifdef USE_SONAR
-static pthread_t sonar_bebop_thread;
-#endif
 
 void sonar_bebop_init(void)
 {
@@ -111,7 +112,9 @@ void sonar_bebop_init(void)
   sonar_bebop_spi_t.input_length  = 0;
 
 #if USE_SONAR
-  pthread_create(&sonar_bebop_thread, NULL, sonar_bebop_read, NULL);
+  pthread_t tid;
+  pthread_create(&tid, NULL, sonar_bebop_read, NULL);
+  pthread_setname_np(tid, "pprz_sonar_thread");
 #endif
 
   init_median_filter_f(&sonar_filt, 3);