Merge branch 'linux_sys_time_thread' of github.com:paparazzi/paparazzi into linux_video_speedup_without_thread

CHANGELOG.md

@@ -3,6 +3,8 @@ Paparazzi 5.5_devel
 
 currently ongoing development, changes so far (no particular order, nor complete)
 
+- arch/linux: change the sys timer to a multi threaded implementation
+  [#1117] (https://github.com/paparazzi/paparazzi/pull/1117)
 - python: generate paparazzi math wrappers with SWIG
   [#1066] (https://github.com/paparazzi/paparazzi/pull/1066)
 - ground segment: use pkg-config for ivy-c if available

conf/Makefile.linux

@@ -56,7 +56,7 @@ CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused
 #CFLAGS += -Wa,-adhlns=$(OBJDIR)/$(notdir $(subst $(suffix $<),.lst,$<))
 #CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
 
-CFLAGS += -lm
+CFLAGS += -lm -pthread
 
 # flags only for C
 CFLAGS += -Wstrict-prototypes -Wmissing-declarations
@@ -65,7 +65,7 @@ CFLAGS += $(CSTANDARD)
 CFLAGS += $($(TARGET).CFLAGS)
 CFLAGS += $(USER_CFLAGS)
 
-LDFLAGS += -lm
+LDFLAGS += -lm -pthread
 
 CXXFLAGS += -O$(OPT) -fPIC
 CXXFLAGS += -pipe -fshow-column -ffast-math

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

@@ -25,45 +25,86 @@
  */
 
 #include "mcu_periph/sys_time.h"
-#include <sys/time.h>
-#include <signal.h>
-#include <string.h>
 #include <stdio.h>
+#include <pthread.h>
+#include <sys/timerfd.h>
+#include "rt_priority.h"
 
 #ifdef SYS_TIME_LED
 #include "led.h"
 #endif
 
+#ifndef SYS_TIME_THREAD_PRIO
+#define SYS_TIME_THREAD_PRIO 29
+#endif
+
+pthread_t sys_time_thread;
+static void sys_tick_handler(void);
+void *sys_time_thread_main(void *data);
+
+#define NSEC_OF_SEC(sec) ((sec) * 1e9)
+
+void *sys_time_thread_main(void *data)
+{
+  int fd;
+
+  /* Create the timer */
+  fd = timerfd_create(CLOCK_MONOTONIC, 0);
+  if (fd == -1) {
+    perror("Could not set up timer.");
+    return NULL;
+  }
+
+  get_rt_prio(SYS_TIME_THREAD_PRIO);
+
+  /* Make the timer periodic */
+  struct itimerspec timer;
+  /* timer expires after sys_time.resolution sec */
+  timer.it_value.tv_sec = 0;
+  timer.it_value.tv_nsec = NSEC_OF_SEC(sys_time.resolution);
+  /* and every SYS_TIME_RESOLUTION sec after that */
+  timer.it_interval.tv_sec = 0;
+  timer.it_interval.tv_nsec = NSEC_OF_SEC(sys_time.resolution);
+
+  if (timerfd_settime(fd, 0, &timer, NULL) == -1) {
+    perror("Could not set up timer.");
+    return NULL;
+  }
+
+  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));
+    if (r == -1) {
+      perror("Couldn't read timer!");
+    }
+    if (missed > 1) {
+      fprintf(stderr, "Missed %lld timer events!\n", missed);
+    }
+    /* advance sys_time, in case we missed some events: call it more than once */
+    unsigned int i;
+    for (i = 0; i < missed; i++) {
+      sys_tick_handler();
+    }
+  }
+  return NULL;
+}
 
 void sys_time_arch_init(void)
 {
-
   sys_time.cpu_ticks_per_sec = 1e6;
   sys_time.resolution_cpu_ticks = (uint32_t)(sys_time.resolution * sys_time.cpu_ticks_per_sec + 0.5);
 
-  struct sigaction sa;
-  struct itimerval timer;
-
-  memset(&sa, 0, sizeof(sa));
-  sa.sa_handler = &sys_tick_handler;
-  if (sigaction(SIGALRM, &sa, NULL) == -1) {
-    printf("Couldn't set up sys_time timer!\n");
+  int ret = pthread_create(&sys_time_thread, NULL, sys_time_thread_main, NULL);
+  if (ret) {
+    perror("Could not setup sys_time_thread");
     return;
   }
-
-  // timer expires after sys_time.resolution sec
-  timer.it_value.tv_sec = 0;
-  timer.it_value.tv_usec = USEC_OF_SEC(sys_time.resolution);
-  // and every SYS_TIME_RESOLUTION sec after that
-  timer.it_interval.tv_sec = 0;
-  timer.it_interval.tv_usec = USEC_OF_SEC(sys_time.resolution);
-
-  setitimer(ITIMER_REAL, &timer, NULL);
 }
 
-void sys_tick_handler(int signum)
+static void sys_tick_handler(void)
 {
-
   sys_time.nb_tick++;
   sys_time.nb_sec_rem += sys_time.resolution_cpu_ticks;;
   if (sys_time.nb_sec_rem >= sys_time.cpu_ticks_per_sec) {

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

@@ -31,8 +31,6 @@
 #include "std.h"
 #include <unistd.h>
 
-extern void sys_tick_handler(int signum);
-
 /**
  * Get the time in microseconds since startup.
  * WARNING: overflows after 71min34seconds!

sw/airborne/arch/linux/rt_priority.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
+ *
+ * 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 rt_priority.h
+ * Function to obtain rt priority.
+ */
+
+#ifndef RT_PRIORITY_H
+#define RT_PRIORITY_H
+
+#include <pthread.h>
+#include <stdio.h>
+
+static inline int get_rt_prio(int prio)
+{
+  struct sched_param param;
+  int policy;
+  pthread_getschedparam(pthread_self(), &policy, &param);
+  printf("Current schedparam: policy %d, prio %d\n", policy, param.sched_priority);
+
+  //SCHED_RR, SCHED_FIFO, SCHED_OTHER (POSIX scheduling policies)
+  int sched = SCHED_FIFO;
+  int min = sched_get_priority_min(sched);
+  int max = sched_get_priority_max(sched);
+  param.sched_priority = prio;
+  if (prio > max || prio < min) {
+    printf("Requested prio %d outside current min/max prios: %d/%d\n", prio, min, max);
+    if (prio > max) {
+      param.sched_priority = max;
+    } else {
+      param.sched_priority = min;
+    }
+  }
+
+  if (pthread_setschedparam(pthread_self(), sched, &param)) {
+    perror("setschedparam failed!");
+    return -1;
+  }
+  else {
+    pthread_getschedparam(pthread_self(), &policy, &param);
+    printf("New schedparam: policy %d, prio %d\n", policy, param.sched_priority);
+  }
+
+  return 0;
+}
+
+#endif /* RT_PRIORITY_H */

sw/airborne/boards/ardrone/baro_board.c

@@ -57,15 +57,15 @@ void baro_periodic(void) {}
  */
 static inline int32_t baro_apply_calibration(int32_t raw)
 {
-  int32_t b6 = ((int32_t)baro_calibration.b5) - 4000L;
-  int32_t x1 = (((int32_t)baro_calibration.b2) * (b6 * b6 >> 12)) >> 11;
-  int32_t x2 = ((int32_t)baro_calibration.ac2) * b6 >> 11;
+  int32_t b6 = ((int32_t)navdata.bmp180_calib.b5) - 4000L;
+  int32_t x1 = (((int32_t)navdata.bmp180_calib.b2) * (b6 * b6 >> 12)) >> 11;
+  int32_t x2 = ((int32_t)navdata.bmp180_calib.ac2) * b6 >> 11;
   int32_t x3 = x1 + x2;
-  int32_t b3 = (((((int32_t)baro_calibration.ac1) * 4 + x3) << BMP180_OSS) + 2) / 4;
-  x1 = ((int32_t)baro_calibration.ac3) * b6 >> 13;
-  x2 = (((int32_t)baro_calibration.b1) * (b6 * b6 >> 12)) >> 16;
+  int32_t b3 = (((((int32_t)navdata.bmp180_calib.ac1) * 4 + x3) << BMP180_OSS) + 2) / 4;
+  x1 = ((int32_t)navdata.bmp180_calib.ac3) * b6 >> 13;
+  x2 = (((int32_t)navdata.bmp180_calib.b1) * (b6 * b6 >> 12)) >> 16;
   x3 = ((x1 + x2) + 2) >> 2;
-  uint32_t b4 = (((int32_t)baro_calibration.ac4) * (uint32_t)(x3 + 32768L)) >> 15;
+  uint32_t b4 = (((int32_t)navdata.bmp180_calib.ac4) * (uint32_t)(x3 + 32768L)) >> 15;
   uint32_t b7 = (raw - b3) * (50000L >> BMP180_OSS);
   int32_t p = b7 < 0x80000000L ? (b7 * 2) / b4 : (b7 / b4) * 2;
   x1 = (p >> 8) * (p >> 8);
@@ -83,26 +83,26 @@ static inline int32_t baro_apply_calibration(int32_t raw)
  */
 static inline int32_t baro_apply_calibration_temp(int32_t tmp_raw)
 {
-  int32_t x1 = ((tmp_raw - ((int32_t)baro_calibration.ac6)) * ((int32_t)baro_calibration.ac5)) >> 15;
-  int32_t x2 = (((int32_t)baro_calibration.mc) << 11) / (x1 + ((int32_t)baro_calibration.md));
-  baro_calibration.b5 = x1 + x2;
-  return (baro_calibration.b5 + 8) >> 4;
+  int32_t x1 = ((tmp_raw - ((int32_t)navdata.bmp180_calib.ac6)) * ((int32_t)navdata.bmp180_calib.ac5)) >> 15;
+  int32_t x2 = (((int32_t)navdata.bmp180_calib.mc) << 11) / (x1 + ((int32_t)navdata.bmp180_calib.md));
+  navdata.bmp180_calib.b5 = x1 + x2;
+  return (navdata.bmp180_calib.b5 + 8) >> 4;
 }
 
 void ardrone_baro_event(void)
 {
-  if (navdata_baro_available) {
-    if (baro_calibrated) {
+  if (navdata.baro_available) {
+    if (navdata.baro_calibrated) {
       // first read temperature because pressure calibration depends on temperature
-      float temp_deg = 0.1 * baro_apply_calibration_temp(navdata.temperature_pressure);
+      float temp_deg = 0.1 * baro_apply_calibration_temp(navdata.measure.temperature_pressure);
       AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp_deg);
-      int32_t press_pascal = baro_apply_calibration(navdata.pressure);
+      int32_t press_pascal = baro_apply_calibration(navdata.measure.pressure);
 #if USE_BARO_MEDIAN_FILTER
       press_pascal = update_median_filter(&baro_median, press_pascal);
 #endif
       float pressure = (float)press_pascal;
       AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
     }
-    navdata_baro_available = FALSE;
+    navdata.baro_available = FALSE;
   }
 }

sw/airborne/boards/ardrone/navdata.h

@@ -30,10 +30,13 @@
 #ifndef NAVDATA_H_
 #define NAVDATA_H_
 
-#include <stdint.h>
-#include <sys/types.h>
+#include "std.h"
 
-typedef struct {
+/**
+ * Main navdata structure from the navdata board
+ * This is received from the navdata board at ~200Hz
+ */
+struct navdata_measure_t {
   uint16_t taille;
   uint16_t nu_trame;
 
@@ -74,9 +77,10 @@ typedef struct {
 
   uint16_t chksum;
 
-} __attribute__((packed)) measures_t;
+} __attribute__((packed));
 
-struct bmp180_baro_calibration {
+/* The baro calibration received from the navboard */
+struct bmp180_calib_t {
   int16_t ac1;
   int16_t ac2;
   int16_t ac3;
@@ -93,32 +97,46 @@ struct bmp180_baro_calibration {
   int32_t b5;
 };
 
-#define NAVDATA_BUFFER_SIZE 80
-typedef struct {
-  uint8_t isInitialized;
-  uint16_t bytesRead;
+/* Navdata board defines */
+#define NAVDATA_PACKET_SIZE       60
+#define NAVDATA_START_BYTE        0x3A
+#define NAVDATA_CMD_START         0x01
+#define NAVDATA_CMD_STOP          0x02
+#define NAVDATA_CMD_BARO_CALIB    0x17
+
+#define ARDRONE_GPIO_PORT         0x32524
+#define ARDRONE_GPIO_PIN_NAVDATA  177
+
+/* Main navdata structure */
+struct navdata_t {
+  bool_t is_initialized;                  //< Check if the navdata board is initialized
+  int fd;                                 //< The navdata file pointer
+
   uint32_t totalBytesRead;
   uint32_t packetsRead;
   uint32_t checksum_errors;
   uint32_t lost_imu_frames;
   uint16_t last_packet_number;
-  uint8_t buffer[NAVDATA_BUFFER_SIZE];
-} navdata_port;
 
-extern measures_t navdata;
-extern navdata_port nav_port;
-struct bmp180_baro_calibration baro_calibration;
-navdata_port *port;
-uint16_t navdata_cks;
-uint8_t navdata_imu_available;
-uint8_t navdata_baro_available;
-uint8_t baro_calibrated;
+  volatile uint8_t buffer_idx;            //< Buffer insert index for reading/writing
+  uint8_t buffer[NAVDATA_PACKET_SIZE];    //< Buffer filled in the thread (maximum one navdata packet)
+
+  struct navdata_measure_t measure;       //< Main navdata packet receieved from navboard
+  struct bmp180_calib_t bmp180_calib;     //< BMP180 calibration receieved from navboard
+
+  bool_t baro_calibrated;                 //< Whenever the baro is calibrated
+  bool_t imu_available;                   //< Whenever the imu is available
+  bool_t baro_available;                  //< Whenever the baro is available
+  bool_t imu_lost;                        //< Whenever the imu is lost
+};
+extern struct navdata_t navdata;
+
 
 bool_t navdata_init(void);
-void navdata_read(void);
 void navdata_update(void);
 int16_t navdata_height(void);
 
+/* This should be moved to the uart handling part! */
 ssize_t full_write(int fd, const uint8_t *buf, size_t count);
 ssize_t full_read(int fd, uint8_t *buf, size_t count);
 

sw/airborne/subsystems/imu/imu_ardrone2_raw.h

@@ -89,11 +89,11 @@ static inline void imu_ardrone2_event(void (* _gyro_handler)(void), void (* _acc
 {
   navdata_update();
   //checks if the navboard has a new dataset ready
-  if (navdata_imu_available == TRUE) {
-    navdata_imu_available = FALSE;
-    RATES_ASSIGN(imu.gyro_unscaled, navdata.vx, -navdata.vy, -navdata.vz);
-    VECT3_ASSIGN(imu.accel_unscaled, navdata.ax, 4096 - navdata.ay, 4096 - navdata.az);
-    VECT3_ASSIGN(imu.mag_unscaled, -navdata.mx, -navdata.my, -navdata.mz);
+  if (navdata.imu_available == TRUE) {
+    navdata.imu_available = FALSE;
+    RATES_ASSIGN(imu.gyro_unscaled, navdata.measure.vx, -navdata.measure.vy, -navdata.measure.vz);
+    VECT3_ASSIGN(imu.accel_unscaled, navdata.measure.ax, 4096 - navdata.measure.ay, 4096 - navdata.measure.az);
+    VECT3_ASSIGN(imu.mag_unscaled, -navdata.measure.mx, -navdata.measure.my, -navdata.measure.mz);
 
     _gyro_handler();
     _accel_handler();