Merge pull request #2447 from enacuavlab/enac_student_projects-integration

Enac student projects integration

sw/airborne/filters/pid.h

@@ -0,0 +1,338 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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 filters/pid.h
+ *  @brief Several forms of PID controllers
+ *
+ */
+
+#ifndef PID_H
+#define PID_H
+
+/** Simple PID structure
+ *  floating point
+ *
+ * u_k = Kp * e_k + Kd * (e_k - e_k-1) / dt + Ki * (sum (e_k * dt))
+ *
+ * with:
+ *  u = outputs
+ *  e = inputs
+ *  Kp = proportional gain
+ *  Kd = derivative gain
+ *  Ki = integral gain
+ *  dt = time since last input
+ */
+struct PID_f {
+  float u;        ///< output
+  float e[2];     ///< input
+  float sum;      ///< integral of input
+  float g[3];     ///< controller gains (Kp, Kd, Ki)
+  float max_sum;  ///< windup protection, max of Ki * sum(e_k * dt)
+};
+
+static inline void init_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki, float max_sum)
+{
+  *pid = (struct PID_f) {
+    0.f,
+    { 0.f , 0.f },
+    0.f,
+    { Kp, Kd, Ki },
+    max_sum
+  };
+}
+
+/** Update PID with a new value and return new command.
+ *
+ * @param pid pointer to PID structure
+ * @param value new input value of the PID
+ * @param dt time since last input (in seconds)
+ * @return new output command
+ */
+static inline float update_pid_f(struct PID_f *pid, float value, float dt)
+{
+  pid->e[1] = pid->e[0];
+  pid->e[0] = value;
+  float integral = pid->g[2] * (pid->sum + value);
+  if (integral > pid->max_sum) {
+    integral = pid->max_sum;
+  } else if (integral < -pid->max_sum) {
+    integral = -pid->max_sum;
+  } else {
+    pid->sum += value;
+  }
+  pid->u = pid->g[0] * pid->e[0] + pid->g[1] * (pid->e[0] - pid->e[1]) / dt + integral;
+  return pid->u;
+}
+
+/** Get current value of the PID command.
+ *
+ * @param pid pointer to PID structure
+ * @return current value of PID command
+ */
+static inline float get_pid_f(struct PID_f *pid)
+{
+  return pid->u;
+}
+
+/** Reset PID struture, gains left unchanged.
+ *
+ * @param pid pointer to PID structure
+ */
+static inline void reset_pid_f(struct PID_f *pid)
+{
+  pid->u = 0.f;
+  pid->e[0] = 0.f;
+  pid->e[1] = 0.f;
+  pid->sum = 0.f;
+}
+
+/** Set gains of the PID struct.
+ *
+ * @param pid pointer to PID structure
+ * @param Kp proportional gain
+ * @param Kd derivative gain
+ * @param Ki integral gain
+ */
+static inline void set_gains_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki)
+{
+  pid->g[0] = Kp;
+  pid->g[1] = Kd;
+  pid->g[2] = Ki;
+}
+
+/** Set integral part, can be used to reset.
+ *  The new sum of errors is calculated from current gains and bounds.
+ *
+ * @param pid pointer to PID structure
+ * @param value integral part of the PID control, 0. will reset it
+ */
+static inline void set_integral_pid_f(struct PID_f *pid, float value)
+{
+  float integral = value;
+  if (integral < -pid->max_sum) {
+    integral = -pid->max_sum;
+  } else if (integral > pid->max_sum) {
+    integral = pid->max_sum;
+  }
+  if (fabsf(pid->g[2]) < 1e-6) {
+    pid->sum = 0.f; // integral gain is too low, prevent division by zero, just reset sum
+  } else {
+    pid->sum = integral / pid->g[2];
+  }
+}
+
+
+
+/** Distcrete time PID structure.
+ *  floating point, fixed frequency.
+ *
+ * u_k = u_k-1 + a * e_k  + b * e_k-1 + c * e_k-2
+ *
+ * with:
+ *  u = outputs
+ *  e = inputs
+ *  a = Kp + Ki Ts/2 + Kd/Ts
+ *  b = -Kp + Ki Ts/2 - 2 Kd/Ts
+ *  c = Kd/Ts
+ *  Kp = proportional gain
+ *  Kd = derivative gain
+ *  Ki = integral gain
+ *  Ts = sampling frequency
+ *
+ */
+struct PID_df {
+  float u[2]; ///< output
+  float e[3]; ///< input
+  float g[3]; ///< controller gains
+};
+
+/** Init PID struct.
+ *
+ * @param pid pointer to PID structure
+ * @param Kp proportional gain
+ * @param Kd derivative gain
+ * @param Ki integral gain
+ * @param Ts sampling time
+ */
+static inline void init_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
+{
+  *pid = (struct PID_df) {
+    { 0.f, 0.f },
+    { 0.f ,0.f , 0.f },
+    { Kp + Ki * Ts / 2.f + Kd / Ts,
+     -Kp + Ki * Ts / 2.f - 2.f * Kd / Ts,
+      Kd / Ts }
+  };
+}
+
+/** Update PID with a new value and return new command.
+ *
+ * @param pid pointer to PID structure
+ * @param value new input value of the PID
+ * @return new output command
+ */
+static inline float update_pid_df(struct PID_df *pid, float value)
+{
+  pid->e[2] = pid->e[1];
+  pid->e[1] = pid->e[0];
+  pid->e[0] = value;
+  pid->u[1] = pid->u[0];
+  pid->u[0] = pid->u[1] + pid->g[0] * pid->e[0] + pid->g[1] * pid->e[1] + pid->g[2] * pid->e[2];
+  return pid->u[0];
+}
+
+/** Get current value of the PID command.
+ *
+ * @param pid pointer to PID structure
+ * @return current value of PID command
+ */
+static inline float get_pid_df(struct PID_df *pid)
+{
+  return pid->u[0];
+}
+
+/** Reset PID struture, gains left unchanged.
+ *
+ * @param pid pointer to PID structure
+ */
+static inline void reset_pid_df(struct PID_df *pid)
+{
+  pid->u[0] = 0.f;
+  pid->u[1] = 0.f;
+  pid->e[0] = 0.f;
+  pid->e[1] = 0.f;
+  pid->e[2] = 0.f;
+}
+
+/** Set gains of the PID struct.
+ *
+ * @param pid pointer to PID structure
+ * @param Kp proportional gain
+ * @param Kd derivative gain
+ * @param Ki integral gain
+ * @param Ts sampling time
+ */
+static inline void set_gains_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
+{
+  pid->g[0] = Kp + Ki * Ts / 2.f + Kd / Ts;
+  pid->g[1] = -Kp + Ki * Ts / 2.f - 2.f * Kd / Ts;
+  pid->g[2] = Kd / Ts;
+}
+
+/** Distcrete time PI-D structure.
+ *  derivative term is directly provided as input
+ *  as it may be available directly from a sensor
+ *  or estimated separately.
+ *  floating point, fixed frequency.
+ *
+ * u_k = u_k-1 + a * e_k  + b * e_k-1 + Kd * d_k
+ *
+ * with:
+ *  u = outputs
+ *  e = inputs
+ *  d = derivative of input
+ *  a = Kp + Ki Ts/2
+ *  b = -Kp + Ki Ts/2
+ *  Kp = proportional gain
+ *  Kd = derivative gain
+ *  Ki = integral gain
+ *  Ts = sampling frequency
+ *
+ */
+struct PI_D_df {
+  float u[2]; ///< output
+  float e[2]; ///< input
+  float g[3]; ///< controller gains
+};
+
+/** Init PI-D struct.
+ *
+ * @param pid pointer to PID structure
+ * @param Kp proportional gain
+ * @param Kd derivative gain
+ * @param Ki integral gain
+ * @param Ts sampling time
+ */
+static inline void init_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
+{
+  *pid = (struct PI_D_df) {
+    { 0.f, 0.f },
+    { 0.f ,0.f },
+    { Kp + Ki * Ts / 2.f,
+     -Kp + Ki * Ts / 2.f,
+      Kd }
+  };
+}
+
+/** Update PI-D with a new value and return new command.
+ *
+ * @param pid pointer to PI-D structure
+ * @param value new input value of the PI-D
+ * @param deriv new input derivative
+ * @return new output command
+ */
+static inline float update_pi_d_df(struct PI_D_df *pid, float value, float deriv)
+{
+  pid->e[1] = pid->e[0];
+  pid->e[0] = value;
+  pid->u[1] = pid->u[0];
+  pid->u[0] = pid->u[1] + pid->g[0] * pid->e[0] + pid->g[1] * pid->e[1] + pid->g[2] * deriv;
+  return pid->u[0];
+}
+
+/** Get current value of the PI-D command.
+ *
+ * @param pid pointer to PI-D structure
+ * @return current value of PI-D command
+ */
+static inline float get_pi_d_df(struct PI_D_df *pid)
+{
+  return pid->u[0];
+}
+
+/** Reset PI-D struture, gains left unchanged.
+ *
+ * @param pid pointer to PI-D structure
+ */
+static inline void reset_pi_d_df(struct PI_D_df *pid)
+{
+  pid->u[0] = 0.f;
+  pid->u[1] = 0.f;
+  pid->e[0] = 0.f;
+  pid->e[1] = 0.f;
+}
+
+/** Set gains PI-D struct.
+ *
+ * @param pid pointer to PID structure
+ * @param Kp proportional gain
+ * @param Kd derivative gain
+ * @param Ki integral gain
+ * @param Ts sampling time
+ */
+static inline void set_gains_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
+{
+  pid->g[0] = Kp + Ki * Ts / 2.f;
+  pid->g[1] = -Kp + Ki * Ts / 2.f;
+  pid->g[2] = Kd;
+}
+
+#endif
+

sw/airborne/modules/ctrl/shift_tracking.c

@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@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/ctrl/shift_tracking.h"
+ * @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * pilot the nav shift variable to track an offset trajectory
+ * based on the POSITION_ESTIMATE ABI message
+ */
+
+#include "modules/ctrl/shift_tracking.h"
+
+#include "firmwares/fixedwing/nav.h"
+#include "autopilot.h"
+#include "subsystems/abi.h"
+#include "math/pprz_algebra_float.h"
+#include "generated/airframe.h"
+
+#include "filters/pid.h"
+
+// ABI message binding ID
+#ifndef SHIFT_TRACKING_ID
+#define SHIFT_TRACKING_ID ABI_BROADCAST
+#endif
+
+// Send debug message
+#ifndef SHIFT_TRACKING_DEBUG
+#define SHIFT_TRACKING_DEBUG FALSE
+#endif
+
+#if SHIFT_TRACKING_DEBUG
+#include "subsystems/datalink/downlink.h"
+#include "pprzlink/messages.h"
+#include "mcu_periph/uart.h"
+#endif
+
+#ifndef SHIFT_TRACKING_DIR
+#define SHIFT_TRACKING_DIR { -1.0f, 0.f, 0.f }
+#endif
+
+#ifndef SHIFT_TRACKING_KP
+#define SHIFT_TRACKING_KP 1.5f
+#endif
+
+#ifndef SHIFT_TRACKING_KI
+#define SHIFT_TRACKING_KI 0.5f
+#endif
+
+#ifndef SHIFT_TRACKING_KD
+#define SHIFT_TRACKING_KD 1.f
+#endif
+
+#ifndef SHIFT_TRACKING_MAXSHIFT
+#define SHIFT_TRACKING_MAXSHIFT 30.f
+#endif
+
+struct shift_tracking_t shift_tracking;
+
+// base time on NAV freq
+static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
+
+// internal structure
+struct shift_tracking_private {
+  struct FloatVect3 pos;  ///< last position report
+  struct FloatVect3 dir;  ///< tracking direction
+  struct PID_f pid;       ///< PID controller
+  float *shift;           ///< keep track of the shift variable to change
+  abi_event pos_ev;
+};
+
+static struct shift_tracking_private stp;
+
+static const float dir[] = SHIFT_TRACKING_DIR;
+
+// callback on follow target message
+static void get_pos(uint8_t sender_id __attribute__((unused)),
+    uint32_t id __attribute__((unused)),
+    float x,
+    float y,
+    float z,
+    float noise_x __attribute__((unused)),
+    float noise_y __attribute__((unused)),
+    float noise_z __attribute__((unused)))
+{
+	stp.pos.x = x;
+	stp.pos.y = y;
+	stp.pos.z = z;
+}
+
+void shift_tracking_init(void)
+{
+  shift_tracking.kp = SHIFT_TRACKING_KP;
+  shift_tracking.ki = SHIFT_TRACKING_KI;
+  shift_tracking.kd = SHIFT_TRACKING_KD;
+  shift_tracking.shift = 0.f;
+
+  FLOAT_VECT3_ZERO(stp.pos);
+  stp.dir.x = dir[0];
+  stp.dir.y = dir[1];
+  stp.dir.z = dir[2];
+  float_vect3_normalize(&stp.dir); // normalize direction
+  init_pid_f(&stp.pid, shift_tracking.kp, shift_tracking.kd, shift_tracking.ki, 30.f);
+  stp.shift = NULL;
+
+  // Bind to position message
+  AbiBindMsgPOSITION_ESTIMATE(SHIFT_TRACKING_ID, &stp.pos_ev, get_pos);
+}
+
+void shift_tracking_reset(void)
+{
+  reset_pid_f(&stp.pid);
+  shift_tracking.shift = 0.f;
+  if (stp.shift) {
+    *stp.shift = 0.f;
+  }
+}
+
+void shift_tracking_run(float *shift)
+{
+  // store external shift variable pointer
+  stp.shift = shift;
+
+  // compute value parameter from pos and dir
+  //
+  // FIXME
+  // for now, assume that Z is normal to the ground
+  // and shift is computed from 2D (X,Y)
+  // the vertical axis in local from should be defined as well
+  // for a correct 3D computation
+  //
+  // normal to dir = (-dir.y, dir.x) where dir is normalized
+  // offset is scalar between pos and normal
+  float value = (- stp.dir.y * stp.pos.x) + (stp.dir.x * stp.pos.y);
+
+  // shift calculation
+  shift_tracking.shift = update_pid_f(&stp.pid, value, nav_dt);
+  BoundAbs(shift_tracking.shift, SHIFT_TRACKING_MAXSHIFT);
+
+  // pilot actual value
+  if (stp.shift) {
+    *stp.shift = shift_tracking.shift;
+  }
+}
+
+void shift_tracking_update_gains(void)
+{
+  set_gains_pid_f(&stp.pid, shift_tracking.kp, shift_tracking.kd, shift_tracking.ki);
+}

sw/airborne/modules/ctrl/shift_tracking.h

@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@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/ctrl/shift_tracking.h"
+ * @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * pilot the nav shift variable to track an offset trajectory
+ * based on the POSITION_ESTIMATE ABI message
+ */
+
+#ifndef SHIFT_TRACKING_H
+#define SHIFT_TRACKING_H
+
+#include "std.h"
+
+struct shift_tracking_t {
+  float kp;     ///< proportional gain
+  float kd;     ///< derivative gain
+  float ki;     ///< integral gain
+  float shift;  ///< shift command
+};
+
+extern struct shift_tracking_t shift_tracking;
+
+/** init function
+ */
+extern void shift_tracking_init(void);
+
+/** run function
+ *
+ *  should be called in flight plan pre_call
+ *
+ *  @param[out] shift pointer to the navigation shift to control
+ */
+extern void shift_tracking_run(float *shift);
+
+/** reset function
+ *
+ *  reset integral and offset command
+ */
+extern void shift_tracking_reset(void);
+
+/** hndlers for gains update
+ */
+#define shift_tracking_SetKp(_v) { shift_tracking.kp = _v; shift_tracking_update_gains(); }
+#define shift_tracking_SetKd(_v) { shift_tracking.kd = _v; shift_tracking_update_gains(); }
+#define shift_tracking_SetKi(_v) { shift_tracking.ki = _v; shift_tracking_update_gains(); }
+extern void shift_tracking_update_gains(void);
+
+#endif
+

sw/airborne/modules/decawave/dw1000_arduino.h

@@ -26,6 +26,20 @@
 #ifndef DW1000_ARDUINO_H
 #define DW1000_ARDUINO_H
 
+#include "std.h"
+
+/** enable EKF filtering */
+extern bool dw1000_use_ekf;
+
+/** process and measurements noise */
+extern float dw1000_ekf_q;
+extern float dw1000_ekf_r_dist;
+extern float dw1000_ekf_r_speed;
+/**  settings handler */
+extern void dw1000_arduino_update_ekf_q(float v);
+extern void dw1000_arduino_update_ekf_r_dist(float v);
+extern void dw1000_arduino_update_ekf_r_speed(float v);
+
 extern void dw1000_arduino_init(void);
 extern void dw1000_arduino_periodic(void);
 extern void dw1000_arduino_report(void);
@@ -36,5 +50,10 @@ extern void dw1000_arduino_event(void);
  */
 extern void dw1000_reset_heading_ref(void);
 
+// when used as a GPS
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS GPS_DW1000
+#endif
+
 #endif
 

sw/airborne/modules/decawave/ekf_range.c

@@ -0,0 +1,198 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@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/decawave/ekf_range.c"
+ * @author Gautier Hattenberger
+ * EKF based on range measurements algorithm
+ */
+
+#include "ekf_range.h"
+#include <math.h>
+
+void ekf_range_init(struct EKFRange *ekf_range, float P0_pos, float P0_speed, float Q_sigma2, float R_dist, float R_speed, float dt)
+{
+  int i,j;
+  const float dt2 = dt * dt;
+  const float dt3 = dt2 * dt / 2.f;
+  const float dt4 = dt2 * dt2 / 4.f;
+  for (i = 0; i < EKF_RANGE_DIM; i++) {
+    ekf_range->state[i] = 0.f; // don't forget to call set_state before running the filter for better results
+    for (j = 0; j < EKF_RANGE_DIM; j++) {
+      ekf_range->P[i][j] = 0.f;
+      ekf_range->Q[i][j] = 0.f;
+    }
+  }
+  for (i = 0; i < EKF_RANGE_DIM; i += 2) {
+    ekf_range->P[i][i] = P0_pos;
+    ekf_range->P[i+1][i+1] = P0_speed;
+    ekf_range->Q[i][i] = Q_sigma2 * dt4;
+    ekf_range->Q[i+1][i] = Q_sigma2 * dt3;
+    ekf_range->Q[i][i+1] = Q_sigma2 * dt3;
+    ekf_range->Q[i+1][i+1] = Q_sigma2 * dt2;
+  }
+  ekf_range->R_dist = R_dist;
+  ekf_range->R_speed = R_speed;
+  ekf_range->dt = dt;
+}
+
+void ekf_range_set_state(struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed)
+{
+  ekf_range->state[0] = pos.x;
+  ekf_range->state[1] = speed.x;
+  ekf_range->state[2] = pos.y;
+  ekf_range->state[3] = speed.y;
+  ekf_range->state[4] = pos.z;
+  ekf_range->state[5] = speed.z;
+}
+
+void ekf_range_get_state(struct EKFRange *ekf_range, struct EnuCoor_f *pos, struct EnuCoor_f *speed)
+{
+  pos->x = ekf_range->state[0];
+  pos->y = ekf_range->state[2];
+  pos->z = ekf_range->state[4];
+  speed->x = ekf_range->state[1];
+  speed->y = ekf_range->state[3];
+  speed->z = ekf_range->state[5];
+}
+
+struct EnuCoor_f ekf_range_get_pos(struct EKFRange *ekf_range)
+{
+  struct EnuCoor_f pos;
+  pos.x = ekf_range->state[0];
+  pos.y = ekf_range->state[2];
+  pos.z = ekf_range->state[4];
+  return pos;
+}
+
+struct EnuCoor_f ekf_range_get_speed(struct EKFRange *ekf_range)
+{
+  struct EnuCoor_f speed;
+  speed.x = ekf_range->state[1];
+  speed.y = ekf_range->state[3];
+  speed.z = ekf_range->state[5];
+  return speed;
+}
+
+void ekf_range_update_noise(struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed)
+{
+  int i;
+  const float dt = ekf_range->dt;
+  const float dt2 = dt * dt;
+  const float dt3 = dt2 * dt / 2.f;
+  const float dt4 = dt2 * dt2 / 4.f;
+  for (i = 0; i < EKF_RANGE_DIM; i += 2) {
+    ekf_range->Q[i][i] = Q_sigma2 * dt4;
+    ekf_range->Q[i+1][i] = Q_sigma2 * dt3;
+    ekf_range->Q[i][i+1] = Q_sigma2 * dt3;
+    ekf_range->Q[i+1][i+1] = Q_sigma2 * dt2;
+  }
+  ekf_range->R_dist = R_dist;
+  ekf_range->R_speed = R_speed;
+}
+
+/** propagate dynamic model
+ *
+ * F = [ 1 dt 0 0  0 0
+ *       0 1  0 0  0 0
+ *       0 0  1 dt 0 0
+ *       0 0  0 1  0 0
+ *       0 0  0 0  1 dt
+ *       0 0  0 0  0 1  ]
+ */
+void ekf_range_predict(struct EKFRange *ekf_range)
+{
+  int i;
+  for (i = 0; i < EKF_RANGE_DIM; i += 2) {
+    // kinematic equation of the dynamic model X = F*X
+    ekf_range->state[i] += ekf_range->state[i+1] * ekf_range->dt;
+    // propagate covariance P = F*P*Ft + Q
+    // since F is diagonal by block, P can be updated by block here as well
+    // let's unroll the matrix operations as it is simple
+    const float d_dt = ekf_range->P[i+1][i+1] * ekf_range->dt;
+    ekf_range->P[i][i] += ekf_range->P[i+1][i] * ekf_range->dt
+      + ekf_range->dt * (ekf_range->P[i][i+1] + d_dt) + ekf_range->Q[i][i];
+    ekf_range->P[i][i+1] += d_dt + ekf_range->Q[i][i+1];
+    ekf_range->P[i+1][i] += d_dt + ekf_range->Q[i+1][i];
+    ekf_range->P[i+1][i+1] += ekf_range->Q[i+1][i+1];
+  }
+}
+
+/** correction step
+ *
+ * K = PHt(HPHt+R)^1
+ * X = X + K(z-h(X))
+ * P = (I-KH)P
+ */
+void ekf_range_update_dist(struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
+{
+  const float dx = ekf_range->state[0] - anchor.x;
+  const float dy = ekf_range->state[2] - anchor.y;
+  const float dz = ekf_range->state[4] - anchor.z;
+  const float norm = sqrtf(dx * dx + dy * dy + dz * dz);
+  // build measurement error
+  const float res = dist - norm;
+  // build Jacobian of observation model for anchor i
+  float Hi[] = { dx / norm, 0.f, dy / norm, 0.f, dz / norm, 0.f };
+  // compute kalman gain K = P*Ht (H*P*Ht + R)^-1
+  // S = H*P*Ht + R
+  const float S =
+    Hi[0] * Hi[0] * ekf_range->P[0][0] +
+    Hi[2] * Hi[2] * ekf_range->P[2][2] +
+    Hi[4] * Hi[4] * ekf_range->P[4][4] +
+    Hi[0] * Hi[2] * (ekf_range->P[0][2] + ekf_range->P[2][0]) +
+    Hi[0] * Hi[4] * (ekf_range->P[0][4] + ekf_range->P[4][0]) +
+    Hi[2] * Hi[4] * (ekf_range->P[2][4] + ekf_range->P[4][2]) +
+    ekf_range->R_dist;
+  if (fabsf(S) < 1e-5) {
+    return; // don't inverse S if it is too small
+  }
+  // finally compute gain and correct state
+  float K[6];
+  for (int i = 0; i < 6; i++) {
+    K[i] = (Hi[0] * ekf_range->P[i][0] + Hi[2] * ekf_range->P[i][2] + Hi[4] * ekf_range->P[i][4]) / S;
+    ekf_range->state[i] += K[i] * res;
+  }
+  // precompute K*H and store current P
+  float KH_tmp[6][6];
+  float P_tmp[6][6];
+  for (int i = 0; i < 6; i++) {
+    for (int j = 0; j < 6; j++) {
+      KH_tmp[i][j] = K[i] * Hi[j];
+      P_tmp[i][j] = ekf_range->P[i][j];
+    }
+  }
+  // correct covariance P = (I-K*H)*P = P - K*H*P
+  for (int i = 0; i < 6; i++) {
+    for (int j = 0; j < 6; j++) {
+      for (int k = 0; k < 6; k++) {
+        ekf_range->P[i][j] -= KH_tmp[i][k] * P_tmp[k][j];
+      }
+    }
+  }
+}
+
+void ekf_range_update_speed(struct EKFRange *ekf_range, float speed, uint8_t type)
+{
+  (void) ekf_range;
+  (void) speed;
+  (void) type;
+  // TODO
+}
+

sw/airborne/modules/decawave/ekf_range.h

@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@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/decawave/ekf_range.h"
+ * @author Gautier Hattenberger
+ * EKF based on range measurements algorithm
+ */
+
+#ifndef EKF_RANGE_H
+#define EKF_RANGE_H
+
+#include "std.h"
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_geodetic_float.h"
+
+#define EKF_RANGE_DIM 6
+
+/** EKF_range structure
+ *
+ * state vector: X = [ x xd y yd z zd ]'
+ * command vector: U = 0 (constant velocity model)
+ * dynamic model: basic kinematic model x_k+1 = x_k + xd_k * dt
+ * measures: distance between (fixed and known) anchors and UAV
+ *
+ * */
+struct EKFRange {
+  float state[EKF_RANGE_DIM];             ///< state vector
+  float P[EKF_RANGE_DIM][EKF_RANGE_DIM];  ///< covariance matrix
+  float Q[EKF_RANGE_DIM][EKF_RANGE_DIM];  ///< process noise matrix
+  float R_dist;                           ///< measurement noise on distances (assumed the same for all anchors)
+  float R_speed;                          ///< measurement noise on speed (assumed the same for all axis)
+  float dt;                               ///< prediction step (in seconds)
+};
+
+/** Init EKF_range internal struct
+ *
+ * @param[in] ekf_range EKFRange structure
+ * @param[in] P0_pos initial covariance on position
+ * @param[in] P0_speed initial covariance on speed
+ * @param[in] Q_sigma2 process noise
+ * @param[in] R_dist measurement noise on distance
+ * @param[in] R_speed measurement noise on speed
+ * @param[in] dt prediction time step in seconds
+ */
+extern void ekf_range_init(struct EKFRange *ekf_range, float P0_pos, float P0_speed, float Q_sigma2, float R_dist, float R_speed, float dt);
+
+/** Set initial state vector
+ *
+ * This function should be called after initialization of the ekf struct and before
+ * running the filter for better results and faster convergence
+ *
+ * @param[in] ekf_range EKFRange structure
+ * @param[in] pos initial position
+ * @param[in] speed initial speed
+ */
+extern void ekf_range_set_state(struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed);
+
+/** Get current state
+ *
+ * @param[in] ekf_range EKFRange structure
+ * @param[out] pos current position
+ * @param[out] speed current speed
+ */
+extern void ekf_range_get_state(struct EKFRange *ekf_range, struct EnuCoor_f *pos, struct EnuCoor_f *speed);
+
+/** Get current pos
+ *
+ * @param[in] ekf_range EKFRange structure
+ * @return current position
+ */
+extern struct EnuCoor_f ekf_range_get_pos(struct EKFRange *ekf_range);
+
+/** Get current speed
+ *
+ * @param[in] ekf_range EKFRange structure
+ * @return current speed
+ */
+extern struct EnuCoor_f ekf_range_get_speed(struct EKFRange *ekf_range);
+
+/** Update process and measurement noises
+ *
+ * @param[in] ekf_range EKFRange structure
+ * @param[in] Q_sigma2 process noise
+ * @param[in] R_dist measurement noise on distance
+ * @param[in] R_speed measurement noise on speed
+ */
+extern void ekf_range_update_noise(struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed);
+
+/** Prediction step
+ *
+ * @param[in] ekf_range EKFRange structure
+ */
+extern void ekf_range_predict(struct EKFRange *ekf_range);
+
+/** Update step based on each new distance data
+ *
+ * @param[in] ekf_range EKFRange structure
+ * @param[in] dist new distance measurement
+ * @param[in] anchor position of the anchor from which the distance is measured
+ */
+extern void ekf_range_update_dist(struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor);
+
+/** Update step based on speed measure
+ *
+ * @param[in] ekf_range EKFRange structure
+ * @param[in] speed new speed measurement
+ * @param[in] type 1: horizontal ground speed norm, 2: vertical ground speed norm, 3: 3D ground speed norm
+ */
+extern void ekf_range_update_speed(struct EKFRange *ekf_range, float speed, uint8_t type);
+
+
+#endif
+
+

sw/airborne/modules/decawave/trilateration.c

@@ -110,6 +110,7 @@ int trilateration_compute(struct Anchor *anchors, struct EnuCoor_f *pos)
   pos->x = P[0][0] + tmp[0] * Ex[0] + tmp[1] * Ey[0] + tmp[2] * Ez[0];
   pos->y = P[0][1] + tmp[0] * Ex[1] + tmp[1] * Ey[1] + tmp[2] * Ez[1];
   pos->z = P[0][2] + tmp[0] * Ex[2] + tmp[1] * Ey[2] + tmp[2] * Ez[2];
+  pos->z = fabsf(pos->z); // in case the base is not matching, keep positive z
   return 0;
 }
 

sw/airborne/modules/decawave/trilateration.h

@@ -36,6 +36,7 @@ struct Anchor {
   float time;           ///< time of the last received data
   struct EnuCoor_f pos; ///< position of the anchor
   uint16_t id;          ///< anchor ID
+  bool updated;         ///< new data available
 };
 
 /** Init internal trilateration structures

sw/airborne/modules/sensors/cameras/jevois.c

@@ -29,6 +29,8 @@
 #include "mcu_periph/uart.h"
 #include "subsystems/abi.h"
 #include "math/pprz_algebra_float.h"
+#include "subsystems/datalink/downlink.h"
+#include <string.h>
 #include <stdio.h>
 
 int jevois_mapping_setting;
@@ -72,10 +74,42 @@ struct jevois_t {
   uint8_t idx; // temp buffer index
   uint8_t n; // temp coordinates/dimension index
   struct jevois_msg_t msg; // last decoded message
+  bool data_available; // new data to report
 };
 
 struct jevois_t jevois;
 
+// reporting function, send telemetry message
+void jevois_report(void)
+{
+  if (jevois.data_available == false) {
+    // no new data, return
+    return;
+  }
+
+  float quat[4] = {
+    jevois.msg.quat.qi,
+    jevois.msg.quat.qx,
+    jevois.msg.quat.qy,
+    jevois.msg.quat.qz
+  };
+  uint8_t len = strlen(jevois.msg.id);
+  char none[] = "None";
+  char *id = jevois.msg.id;
+  if (len == 0) {
+    id = none;
+    len = 4;
+  }
+  DOWNLINK_SEND_JEVOIS(DefaultChannel, DefaultDevice,
+      &jevois.msg.type,
+      len, id,
+      &jevois.msg.nb,
+      Max(jevois.msg.nb,1), jevois.msg.coord,
+      jevois.msg.dim,
+      quat);
+  jevois.data_available = false;
+}
+
 // initialization
 void jevois_init(void)
 {
@@ -86,12 +120,17 @@ void jevois_init(void)
   jevois.state = JV_SYNC;
   jevois.idx = 0;
   jevois.n = 0;
+  jevois.data_available = false;
   memset(jevois.buf, 0, JEVOIS_MAX_LEN);
 }
 
 // send specific message if requested
 static void jevois_send_message(void)
 {
+#if JEVOIS_SEND_MSG
+  // send pprzlink JEVOIS message
+  jevois_report();
+#endif
 #if JEVOIS_SEND_FOLLOW_TARGET
   float cam_heading = (JEVOIS_HFOV / (2.f * JEVOIS_NORM)) * (float)(jevois.msg.coord[0]);
   float cam_height = (JEVOIS_VFOV / (2.f * JEVOIS_NORM)) * (float)(jevois.msg.coord[1]);
@@ -200,7 +239,7 @@ static void jevois_parse(struct jevois_t *jv, char c)
       if (JEVOIS_CHECK_DELIM(c)) {
         jv->buf[jv->idx] = '\0'; // end string
         jv->msg.coord[jv->n++] = (int16_t)atoi(jv->buf); // store value
-        if (jv->n == 2 * jv->msg.nb) {
+        if (jv->n == jv->msg.nb) {
           // got all coordinates, go to next state
           jv->n = 0; // reset number of received elements
           jv->idx = 0; // reset index
@@ -257,10 +296,10 @@ static void jevois_parse(struct jevois_t *jv, char c)
     case JV_QUAT:
       if (JEVOIS_CHECK_DELIM(c)) {
         jv->buf[jv->idx] = '\0';
-        float q = 0.f;//(float) atof(jv->buf);
+        float q = (float)atof(jv->buf);
         switch (jv->n) {
           case 0:
-            jv->msg.quat.qi = q; // TODO check quaternion order
+            jv->msg.quat.qi = q;
             break;
           case 1:
             jv->msg.quat.qx = q;
@@ -310,6 +349,7 @@ static void jevois_parse(struct jevois_t *jv, char c)
           jv->msg.extra);
       // also send specific messages if needed
       jevois_send_message();
+      jv->data_available = true;
       jv->state = JV_SYNC;
       break;
     default:
@@ -331,7 +371,7 @@ void jevois_event(void)
 }
 
 // utility function to send a string
-static void send_string(char *s)
+void jevois_send_string(char *s)
 {
   uint8_t i = 0;
   while (s[i]) {
@@ -344,9 +384,9 @@ void jevois_stream(bool activate)
 {
   jevois_stream_setting = activate;
   if (activate) {
-    send_string("streamon\r\n");
+    jevois_send_string("streamon\r\n");
   } else {
-    send_string("streamoff\r\n");
+    jevois_send_string("streamoff\r\n");
   }
 }
 
@@ -354,13 +394,13 @@ void jevois_setmapping(int number)
 {
   jevois_mapping_setting = number;
   jevois_stream(false);
-  send_string("setmapping ");
+  jevois_send_string("setmapping ");
   char s[4];
 #ifndef SITL
   itoa(number, s, 10);
 #endif
-  send_string(s);
-  send_string("\r\n");
+  jevois_send_string(s);
+  jevois_send_string("\r\n");
   jevois_stream(true);
 }
 

sw/airborne/modules/sensors/cameras/jevois.h

@@ -61,6 +61,13 @@
 
 extern void jevois_init(void);
 extern void jevois_event(void);
+extern void jevois_report(void);
+
+/**
+ * Generic function to send a string command to Jevois
+ * @param[in] s string command to send
+ */
+extern void jevois_send_string(char *s);
 
 /** Start and stop streaming
  * @param[in] activate enable or disable streaming

sw/airborne/subsystems/ins/ins_gps_passthrough.c

@@ -203,6 +203,7 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
   struct Int32Vect3 accel_body, accel_ned;
   struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
   int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
+  stateSetAccelBody_i(&accel_body);
   struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
   int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, &accel_body);
   accel_ned.z += ACCEL_BFP_OF_REAL(9.81);