diff --git a/conf/airframes/ENAC/rover_ostrich.xml b/conf/airframes/ENAC/rover_ostrich.xml
index 664a6ed801..21b28f145e 100644
--- a/conf/airframes/ENAC/rover_ostrich.xml
+++ b/conf/airframes/ENAC/rover_ostrich.xml
@@ -10,17 +10,22 @@
-
+
-
+
+
+
-
-
+
+
+
+
+
@@ -67,12 +72,12 @@
-
-
-
+
+
+
-
-
+
+
@@ -99,6 +104,10 @@
+
+
diff --git a/conf/flight_plans/ENAC/dw1000_basic.xml b/conf/flight_plans/ENAC/dw1000_basic.xml
new file mode 100644
index 0000000000..87606cf96e
--- /dev/null
+++ b/conf/flight_plans/ENAC/dw1000_basic.xml
@@ -0,0 +1,97 @@
+
+
+
+
+#include "subsystems/datalink/datalink.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/rover_optitrack_PIR.xml b/conf/flight_plans/rover_optitrack_PIR.xml
new file mode 100644
index 0000000000..e5031e0d44
--- /dev/null
+++ b/conf/flight_plans/rover_optitrack_PIR.xml
@@ -0,0 +1,72 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/dw1000_arduino.xml b/conf/modules/dw1000_arduino.xml
index be04aff343..4e5893bfc6 100644
--- a/conf/modules/dw1000_arduino.xml
+++ b/conf/modules/dw1000_arduino.xml
@@ -8,9 +8,13 @@
Decawave DW1000 modules (http://www.decawave.com/products/dwm1000-module) are Ultra-Wide-Band devices that can be used for communication and ranging.
Especially, using 3 modules as anchors can provide data for a localization system based on trilateration.
The DW1000 is using a SPI connection, but an arduino-compatible board can be used with the library https://github.com/thotro/arduino-dw1000 to hyde the low level drivers and provide direct ranging informations.
+
+ See https://hal-enac.archives-ouvertes.fr/hal-01936955 for more information on the EKF filtering.
+
+
@@ -20,8 +24,24 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -33,8 +53,32 @@
+
+
+
+
+
+
+ ifeq (,$(findstring $(DW1000_USE_AS_GPS),0 FALSE))
+ ifdef SECONDARY_GPS
+ ifneq (,$(findstring $(SECONDARY_GPS), dw1000))
+ # this is the secondary GPS
+ $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"modules/decawave/dw1000_arduino.h\"
+ $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_DW1000
+ else
+ $(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/decawave/dw1000_arduino.h\"
+ $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_DW1000
+ endif
+ else
+ # plain old single GPS usage
+ $(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/decawave/dw1000_arduino.h\"
+ endif
+ endif
+
+
+
diff --git a/conf/modules/jevois.xml b/conf/modules/jevois.xml
index 0b5fc7548f..ea3e3677dc 100644
--- a/conf/modules/jevois.xml
+++ b/conf/modules/jevois.xml
@@ -9,6 +9,7 @@
+
@@ -22,6 +23,7 @@
+
diff --git a/conf/modules/shift_tracking.xml b/conf/modules/shift_tracking.xml
new file mode 100644
index 0000000000..dd362fce5e
--- /dev/null
+++ b/conf/modules/shift_tracking.xml
@@ -0,0 +1,43 @@
+
+
+
+
+
+ Pilot nav shift variable to track an offset in navigation
+
+ Typical use is to Control a fixed-wing to use both GPS and
+ local position (from Decawave modules for instance) to land it
+ with precision or in a net.
+
+ Data are coming from POSITION_ESTIMATE ABI message.
+
+ See https://hal-enac.archives-ouvertes.fr/hal-01936955 for more information on UAV landing in a net.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/telemetry_transparent.xml b/conf/modules/telemetry_transparent.xml
index 9a67b26d53..0f78445515 100644
--- a/conf/modules/telemetry_transparent.xml
+++ b/conf/modules/telemetry_transparent.xml
@@ -44,5 +44,9 @@
+
+
+
+
diff --git a/conf/modules/telemetry_transparent_udp.xml b/conf/modules/telemetry_transparent_udp.xml
index 6a896b59e7..e22d993333 100644
--- a/conf/modules/telemetry_transparent_udp.xml
+++ b/conf/modules/telemetry_transparent_udp.xml
@@ -53,5 +53,9 @@
+
+
+
+
diff --git a/conf/modules/telemetry_transparent_usb.xml b/conf/modules/telemetry_transparent_usb.xml
index 9a28be8e1e..b33d6ddc9d 100644
--- a/conf/modules/telemetry_transparent_usb.xml
+++ b/conf/modules/telemetry_transparent_usb.xml
@@ -45,5 +45,9 @@
+
+
+
+
diff --git a/conf/settings/estimation/ins_sonar.xml b/conf/settings/estimation/ins_sonar.xml
index e709d2c6f5..dddf144a52 100644
--- a/conf/settings/estimation/ins_sonar.xml
+++ b/conf/settings/estimation/ins_sonar.xml
@@ -3,7 +3,7 @@
-
+
diff --git a/conf/telemetry/default_rover.xml b/conf/telemetry/default_rover.xml
index dd94af5c00..9e9abea45e 100644
--- a/conf/telemetry/default_rover.xml
+++ b/conf/telemetry/default_rover.xml
@@ -26,6 +26,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/filters/pid.h b/sw/airborne/filters/pid.h
new file mode 100644
index 0000000000..3d72f28633
--- /dev/null
+++ b/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
+ * .
+ */
+
+/** @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
+
diff --git a/sw/airborne/modules/ctrl/shift_tracking.c b/sw/airborne/modules/ctrl/shift_tracking.c
new file mode 100644
index 0000000000..084491427e
--- /dev/null
+++ b/sw/airborne/modules/ctrl/shift_tracking.c
@@ -0,0 +1,166 @@
+/*
+ * 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
+ * .
+ */
+
+/**
+ * @file "modules/ctrl/shift_tracking.h"
+ * @author Gautier Hattenberger
+ *
+ * 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);
+}
diff --git a/sw/airborne/modules/ctrl/shift_tracking.h b/sw/airborne/modules/ctrl/shift_tracking.h
new file mode 100644
index 0000000000..2dc0b763f9
--- /dev/null
+++ b/sw/airborne/modules/ctrl/shift_tracking.h
@@ -0,0 +1,69 @@
+/*
+ * 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
+ * .
+ */
+
+/**
+ * @file "modules/ctrl/shift_tracking.h"
+ * @author Gautier Hattenberger
+ *
+ * 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
+
diff --git a/sw/airborne/modules/decawave/dw1000_arduino.c b/sw/airborne/modules/decawave/dw1000_arduino.c
index c9d51dd755..435f070a8b 100644
--- a/sw/airborne/modules/decawave/dw1000_arduino.c
+++ b/sw/airborne/modules/decawave/dw1000_arduino.c
@@ -39,6 +39,21 @@
#include
#include
+/** TRUE if dw1000 are used as local position estimate */
+#ifndef DW1000_USE_AS_LOCAL_POS
+#define DW1000_USE_AS_LOCAL_POS TRUE
+#endif
+
+/** TRUE if dw1000 are used as GPS */
+#ifndef DW1000_USE_AS_GPS
+#define DW1000_USE_AS_GPS FALSE
+#endif
+
+/** TRUE if EKF range filter is use */
+#ifndef DW1000_USE_EKF
+#define DW1000_USE_EKF TRUE
+#endif
+
/** Number of anchors
*
* using standard trilateration algorithm, only 3 anchors are required/supported
@@ -49,12 +64,12 @@
#define DW1000_NB_ANCHORS 3
#endif
-/** default offset, applied to final result not to individual distances */
+/** default offset, applied to individual distances */
#ifndef DW1000_OFFSET
#define DW1000_OFFSET { 0.f, 0.f, 0.f }
#endif
-/** default scale factor, applied to final result not to individual distances */
+/** default scale factor, applied to individual distances */
#ifndef DW1000_SCALE
#define DW1000_SCALE { 1.f, 1.f, 1.f }
#endif
@@ -69,6 +84,75 @@
#define DW1000_TIMEOUT 500
#endif
+/** DW1000 Noise */
+#ifndef DW1000_NOISE_X
+#define DW1000_NOISE_X 0.1f
+#endif
+
+#ifndef DW1000_NOISE_Y
+#define DW1000_NOISE_Y 0.1f
+#endif
+
+#ifndef DW1000_NOISE_Z
+#define DW1000_NOISE_Z 0.1f
+#endif
+
+#ifndef DW1000_VEL_NOISE_X
+#define DW1000_VEL_NOISE_X 0.1f
+#endif
+
+#ifndef DW1000_VEL_NOISE_Y
+#define DW1000_VEL_NOISE_Y 0.1f
+#endif
+
+#ifndef DW1000_VEL_NOISE_Z
+#define DW1000_VEL_NOISE_Z 0.1f
+#endif
+
+#if DW1000_USE_EKF
+#include "modules/decawave/ekf_range.h"
+#include "filters/median_filter.h"
+
+#define DW1000_EKF_UNINIT 0
+#define DW1000_EKF_POS_INIT 1
+#define DW1000_EKF_RUNNING 2
+
+#ifndef DW1000_EKF_P0_POS
+#define DW1000_EKF_P0_POS 1.0f
+#endif
+
+#ifndef DW1000_EKF_P0_SPEED
+#define DW1000_EKF_P0_SPEED 1.0f
+#endif
+
+#ifndef DW1000_EKF_Q
+#define DW1000_EKF_Q 4.0f
+#endif
+
+#ifndef DW1000_EKF_R_DIST
+#define DW1000_EKF_R_DIST 0.1f
+#endif
+
+#ifndef DW1000_EKF_R_SPEED
+#define DW1000_EKF_R_SPEED 0.1f
+#endif
+
+#endif // USE_EKF
+
+/** waypoints to use as anchors in simulation
+ */
+#if SITL
+#ifndef DW1000_ANCHOR_SIM_WP
+#define DW1000_ANCHOR_SIM_WP { WP_ANCHOR_1, WP_ANCHOR_2, WP_ANCHOR_3 }
+#endif
+#endif
+
+/** ChibiOS SD logger */
+#if DW1000_LOG
+#include "modules/loggers/sdlog_chibios.h"
+static bool log_started;
+#endif
+
/** frame sync byte */
#define DW_STX 0xFE
@@ -85,17 +169,37 @@ struct DW1000 {
uint8_t ck; ///< checksum
uint8_t state; ///< parser state
float initial_heading; ///< initial heading correction
- struct Anchor anchors[DW1000_NB_ANCHORS]; ///buf);
if (dw->anchors[i].id == id) {
- dw->anchors[i].distance = float_from_buf(dw->buf + 2);
+ dw->raw_dist[i] = float_from_buf(dw->buf + 2);
+ // median filter for EKF
+ const float dist = scale[i] * (dw->raw_dist[i] - offset[i]);
+ // store scaled distance
+ dw->anchors[i].distance = update_median_filter_f(&dw->mf[i], dist);
dw->anchors[i].time = get_sys_time_float();
- dw->updated = true;
+ dw->anchors[i].updated = true;
+ dw->updated = true; // at least one of the anchor is updated
break;
}
}
@@ -153,18 +262,23 @@ static void dw1000_arduino_parse(struct DW1000 *dw, uint8_t c)
}
dw->state = DW_WAIT_STX;
break;
+
+ default:
+ dw->state = DW_WAIT_STX;
}
}
+#endif // !SITL
+#if DW1000_USE_AS_GPS
static void send_gps_dw1000_small(struct DW1000 *dw)
{
// rotate and convert to cm integer
float x = dw->pos.x * cosf(dw->initial_heading) - dw->pos.y * sinf(dw->initial_heading);
float y = dw->pos.x * sinf(dw->initial_heading) + dw->pos.y * cosf(dw->initial_heading);
struct EnuCoor_i enu_pos;
- enu_pos.x = (int32_t) (x * 100);
- enu_pos.y = (int32_t) (y * 100);
- enu_pos.z = (int32_t) (dw->pos.z * 100);
+ enu_pos.x = (int32_t) (x * 100.f);
+ enu_pos.y = (int32_t) (y * 100.f);
+ enu_pos.z = (int32_t) (dw->pos.z * 100.f);
// Convert the ENU coordinates to ECEF
ecef_of_enu_point_i(&(dw->gps_dw1000.ecef_pos), &(dw->ltp_def), &enu_pos);
@@ -173,9 +287,31 @@ static void send_gps_dw1000_small(struct DW1000 *dw)
lla_of_ecef_i(&(dw->gps_dw1000.lla_pos), &(dw->gps_dw1000.ecef_pos));
SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_POS_LLA_BIT);
+ // Convert ENU speed to ECEF
+ struct EnuCoor_i enu_speed;
+ enu_speed.x = (int32_t) (dw->speed.x * 100.f);
+ enu_speed.y = (int32_t) (dw->speed.y * 100.f);
+ enu_speed.z = (int32_t) (dw->speed.z * 100.f);
+
+ VECT3_NED_OF_ENU(dw->gps_dw1000.ned_vel, enu_speed);
+ SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_VEL_NED_BIT);
+
+ ecef_of_enu_vect_i(&dw->gps_dw1000.ecef_vel , &(dw->ltp_def) , &enu_speed);
+ SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_VEL_ECEF_BIT);
+
+ dw->gps_dw1000.gspeed = (int16_t)FLOAT_VECT2_NORM(enu_speed);
+ dw->gps_dw1000.speed_3d = (int16_t)FLOAT_VECT3_NORM(enu_speed);
+
+ // HMSL
dw->gps_dw1000.hmsl = dw->ltp_def.hmsl + enu_pos.z * 10;
SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_HMSL_BIT);
+#if defined(SECONDARY_GPS) && AHRS_USE_GPS_HEADING
+ // a second GPS is used to get heading
+ // ugly hack: it is a datalink GPS
+ dw->gps_dw1000.course = gps_datalink.course;
+#endif
+
dw->gps_dw1000.num_sv = 7;
dw->gps_dw1000.tow = get_sys_time_msec();
dw->gps_dw1000.fix = GPS_FIX_3D; // set 3D fix to true
@@ -190,6 +326,139 @@ static void send_gps_dw1000_small(struct DW1000 *dw)
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgGPS(GPS_DW1000_ID, now_ts, &(dw->gps_dw1000));
}
+#endif
+
+#if DW1000_USE_AS_LOCAL_POS
+static void send_pos_estimate(struct DW1000 *dw)
+{
+ uint32_t now_ts = get_sys_time_usec();
+ // send POSITION_ESTIMATE type message
+ AbiSendMsgPOSITION_ESTIMATE(GPS_DW1000_ID, now_ts,
+ dw->pos.x, dw->pos.y, dw->pos.z,
+ DW1000_NOISE_X, DW1000_NOISE_Y, DW1000_NOISE_Z);
+ // send VELOCITY_ESTIMATE type message if EKF is running
+ if (dw1000_use_ekf && dw->ekf_running) {
+ AbiSendMsgVELOCITY_ESTIMATE(GPS_DW1000_ID, now_ts,
+ dw->speed.x, dw->speed.y, dw->speed.z,
+ DW1000_VEL_NOISE_X, DW1000_VEL_NOISE_Y, DW1000_VEL_NOISE_Z);
+ }
+}
+#endif
+
+/** check timeout for each anchor
+ * @param dw DW1000 struct
+ * @param timeout timeout in seconds
+ * @return true if one has reach timeout
+ */
+static bool check_anchor_timeout(struct DW1000 *dw, float timeout)
+{
+ const float now = get_sys_time_float();
+ for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
+ if (now - dw->anchors[i].time > timeout) {
+ return true;
+ }
+ }
+ return false;
+}
+
+/** check new data and compute with the proper algorithm
+ * @return true if processing is succesful
+ */
+static inline bool check_and_compute_data(struct DW1000 *dw)
+{
+ const float timeout = (float)DW1000_TIMEOUT / 1000.;
+ if (dw1000_use_ekf) {
+ if (dw->ekf_running) {
+ // filter is running
+ if (check_anchor_timeout(dw, 5.f*timeout)) {
+ // no more valid data for a long time
+ dw->ekf_running = false;
+ return false;
+ } else {
+ // run filter on each updated anchor
+ for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
+ if (dw->anchors[i].updated) {
+ ekf_range_update_dist(&dw->ekf_range, dw->anchors[i].distance,
+ dw->anchors[i].pos);
+ dw->anchors[i].updated = false;
+ }
+ }
+ dw->pos = ekf_range_get_pos(&dw->ekf_range);
+ dw->speed = ekf_range_get_speed(&dw->ekf_range);
+ return true;
+ }
+ } else {
+ // filter is currently not running,
+ // waiting for a new valid initial position
+ if (check_anchor_timeout(dw, timeout)) {
+ // no valid data
+ return false;
+ } else {
+ if (trilateration_compute(dw->anchors, &(dw->pos)) == 0) {
+ // got valid initial pos
+ struct EnuCoor_f speed = { 0.f, 0.f, 0.f };
+ ekf_range_set_state(&dw->ekf_range, dw->pos, speed);
+ dw->ekf_running = true;
+ return true;
+ } else {
+ // trilateration failed
+ return false;
+ }
+ }
+ }
+ } else {
+ // Direct trilateration only
+ // if no timeout on anchors, run trilateration algorithm
+ return (check_anchor_timeout(dw, timeout) == false &&
+ trilateration_compute(dw->anchors, &(dw->pos)) == 0);
+ }
+}
+
+static void process_data(struct DW1000 *dw) {
+ // process if new data
+ if (dw->updated) {
+ // send result if process returns true
+ if (check_and_compute_data(dw)) {
+#if DW1000_USE_AS_GPS
+ // send fake GPS message for INS filters
+ send_gps_dw1000_small(dw);
+#endif
+#if DW1000_USE_AS_LOCAL_POS
+ // send a local postion estimate
+ send_pos_estimate(dw);
+#endif
+ }
+#if DW1000_LOG
+ if (log_started) {
+ struct EnuCoor_f pos = *stateGetPositionEnu_f();
+ struct EnuCoor_f speed = *stateGetSpeedEnu_f();
+ struct FloatRates *rates = stateGetBodyRates_f();
+ struct FloatRMat *ned_to_body = stateGetNedToBodyRMat_f();
+ float omega_z = -rates->p * MAT33_ELMT(*ned_to_body, 2, 0)
+ + rates->q * MAT33_ELMT(*ned_to_body, 2, 1)
+ + rates->r * MAT33_ELMT(*ned_to_body, 2, 2);
+ sdLogWriteLog(pprzLogFile, "%.3f %.3f %.3f %3.f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
+ dw1000.anchors[0].distance,
+ dw1000.anchors[0].time,
+ dw1000.anchors[1].distance,
+ dw1000.anchors[1].time,
+ dw1000.anchors[2].distance,
+ dw1000.anchors[2].time,
+ dw1000.pos.x,
+ dw1000.pos.y,
+ dw1000.pos.z,
+ pos.x,
+ pos.y,
+ pos.z,
+ speed.x,
+ speed.y,
+ speed.z,
+ omega_z);
+ }
+#endif
+ dw->updated = false;
+ }
+}
void dw1000_reset_heading_ref(void)
{
@@ -198,22 +467,50 @@ void dw1000_reset_heading_ref(void)
dw1000_arduino_dw1000_reset_heading_ref_status = MODULES_STOP;
}
-/// init arrays from airframe file
-static const uint16_t ids[] = DW1000_ANCHORS_IDS;
-static const float pos_x[] = DW1000_ANCHORS_POS_X;
-static const float pos_y[] = DW1000_ANCHORS_POS_Y;
-static const float pos_z[] = DW1000_ANCHORS_POS_Z;
-static const float offset[] = DW1000_OFFSET;
-static const float scale[] = DW1000_SCALE;
+#if SITL
+static const uint8_t wp_ids[] = DW1000_ANCHOR_SIM_WP;
-static void scale_position(struct DW1000 *dw)
+#define DW1000_SITL_SYNC FALSE
+
+static void compute_anchors_dist_from_wp(struct DW1000 *dw)
{
- dw->pos.x = scale[0] * (dw->raw_pos.x - offset[0]);
- dw->pos.y = scale[1] * (dw->raw_pos.y - offset[1]);
- dw->pos.z = scale[2] * (dw->raw_pos.z - offset[2]);
-}
+#if !DW1000_SITL_SYNC
+ static int i = 0; // async data update (more realistic)
+#endif
+ // position of the aircraft
+ struct FloatVect3 *pos = (struct FloatVect3 *) (stateGetPositionEnu_f());
+ float time = get_sys_time_float();
+ // compute distance to each WP/anchor
+#if DW1000_SITL_SYNC
+ for (uint8_t i = 0; i < DW1000_NB_ANCHORS; i++) {
+#endif
+ struct FloatVect3 a_pos = {
+ WaypointX(wp_ids[i]),
+ WaypointY(wp_ids[i]),
+ WaypointAlt(wp_ids[i])
+ };
+ struct FloatVect3 diff;
+ VECT3_DIFF(diff, (*pos), a_pos);
+ dw->anchors[i].distance = float_vect3_norm(&diff);
+ dw->anchors[i].time = time;
+ dw->anchors[i].updated = true;
+ dw->updated = true;
-void dw1000_arduino_init()
+ i = (i+1)%DW1000_NB_ANCHORS;
+#if DW1000_SITL_SYNC
+ }
+#endif
+ struct EnuCoor_f t_pos;
+ // direct trilat for debug
+ if (trilateration_compute(dw->anchors, &t_pos) == 0) {
+ dw->raw_dist[0] = t_pos.x;
+ dw->raw_dist[1] = t_pos.y;
+ dw->raw_dist[2] = t_pos.z;
+ }
+}
+#endif // SITL
+
+void dw1000_arduino_init(void)
{
// init DW1000 structure
dw1000.idx = 0;
@@ -223,11 +520,16 @@ void dw1000_arduino_init()
dw1000.pos.x = 0.f;
dw1000.pos.y = 0.f;
dw1000.pos.z = 0.f;
+ dw1000.speed.x = 0.f;
+ dw1000.speed.y = 0.f;
+ dw1000.speed.z = 0.f;
dw1000.updated = false;
for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
+ dw1000.raw_dist[i] = 0.f;
dw1000.anchors[i].distance = 0.f;
dw1000.anchors[i].time = 0.f;
dw1000.anchors[i].id = ids[i];
+ dw1000.anchors[i].updated = false;
dw1000.anchors[i].pos.x = pos_x[i];
dw1000.anchors[i].pos.y = pos_y[i];
dw1000.anchors[i].pos.z = pos_z[i];
@@ -251,63 +553,95 @@ void dw1000_arduino_init()
// init trilateration algorithm
trilateration_init(dw1000.anchors);
+ dw1000_use_ekf = DW1000_USE_EKF;
+ dw1000_ekf_q = DW1000_EKF_Q;
+ dw1000_ekf_r_dist = DW1000_EKF_R_DIST;
+ dw1000_ekf_r_speed = DW1000_EKF_R_SPEED;
+ dw1000.ekf_running = false;
+ ekf_range_init(&dw1000.ekf_range, DW1000_EKF_P0_POS, DW1000_EKF_P0_SPEED,
+ DW1000_EKF_Q, DW1000_EKF_R_DIST, DW1000_EKF_R_SPEED, 0.1f);
+ for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
+ init_median_filter_f(&dw1000.mf[i], 3);
+ }
}
-void dw1000_arduino_periodic()
+void dw1000_arduino_periodic(void)
{
- // Check for timeout
+ if (dw1000_use_ekf) {
+ if (dw1000.ekf_running) {
+ ekf_range_predict(&dw1000.ekf_range);
+ dw1000.pos = ekf_range_get_pos(&dw1000.ekf_range);
+ dw1000.speed = ekf_range_get_speed(&dw1000.ekf_range);
+ }
+ } else {
+ dw1000.ekf_running = false; // init sequence will be required at next run
+ }
+#if SITL
+ // compute position from WP for simulation
+ compute_anchors_dist_from_wp(&dw1000);
+ process_data(&dw1000);
+#endif
+#if DW1000_USE_AS_GPS
+ // Check for GPS timeout
gps_periodic_check(&(dw1000.gps_dw1000));
+#endif
+#if DW1000_LOG
+ if (pprzLogFile != -1) {
+ if (!log_started) {
+ sdLogWriteLog(pprzLogFile,
+ "d1 t1 d2 t2 d3 t3 x y z gps_x gps_y gps_z vx vy vz omega\n");
+ log_started = true;
+ }
+ }
+#endif
}
-void dw1000_arduino_report()
+void dw1000_arduino_report(void)
{
- float buf[9];
+ float buf[12];
buf[0] = dw1000.anchors[0].distance;
buf[1] = dw1000.anchors[1].distance;
buf[2] = dw1000.anchors[2].distance;
- buf[3] = dw1000.raw_pos.x;
- buf[4] = dw1000.raw_pos.y;
- buf[5] = dw1000.raw_pos.z;
+ buf[3] = dw1000.raw_dist[0];
+ buf[4] = dw1000.raw_dist[1];
+ buf[5] = dw1000.raw_dist[2];
buf[6] = dw1000.pos.x;
buf[7] = dw1000.pos.y;
buf[8] = dw1000.pos.z;
- DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 9, buf);
+ buf[9] = dw1000.speed.x;
+ buf[10] = dw1000.speed.y;
+ buf[11] = dw1000.speed.z;
+ DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 12, buf);
}
-/** check timeout for each anchor
- * @return true if one has reach timeout
- */
-static bool check_anchor_timeout(struct DW1000 *dw)
-{
- const float now = get_sys_time_float();
- const float timeout = (float)DW1000_TIMEOUT / 1000.;
- for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
- if (now - dw->anchors[i].time > timeout) {
- return true;
- }
- }
- return false;
-}
-
-void dw1000_arduino_event()
+void dw1000_arduino_event(void)
{
+#if !SITL
// Look for data on serial link and send to parser
while (uart_char_available(&DW1000_ARDUINO_DEV)) {
uint8_t ch = uart_getch(&DW1000_ARDUINO_DEV);
dw1000_arduino_parse(&dw1000, ch);
- // process if new data
- if (dw1000.updated) {
- // if no timeout on anchors, run trilateration algorithm
- if (check_anchor_timeout(&dw1000) == false &&
- trilateration_compute(dw1000.anchors, &dw1000.raw_pos) == 0) {
- // apply scale and neutral corrections
- scale_position(&dw1000);
- // send fake GPS message for INS filters
- send_gps_dw1000_small(&dw1000);
- }
- dw1000.updated = false;
- }
+ process_data(&dw1000);
}
+#endif
}
+void dw1000_arduino_update_ekf_q(float v)
+{
+ dw1000_ekf_q = v;
+ ekf_range_update_noise(&dw1000.ekf_range, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed);
+}
+
+void dw1000_arduino_update_ekf_r_dist(float v)
+{
+ dw1000_ekf_r_dist = v;
+ ekf_range_update_noise(&dw1000.ekf_range, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed);
+}
+
+void dw1000_arduino_update_ekf_r_speed(float v)
+{
+ dw1000_ekf_r_speed = v;
+ ekf_range_update_noise(&dw1000.ekf_range, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed);
+}
+
diff --git a/sw/airborne/modules/decawave/dw1000_arduino.h b/sw/airborne/modules/decawave/dw1000_arduino.h
index fd2e6076e8..731317a663 100644
--- a/sw/airborne/modules/decawave/dw1000_arduino.h
+++ b/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
diff --git a/sw/airborne/modules/decawave/ekf_range.c b/sw/airborne/modules/decawave/ekf_range.c
new file mode 100644
index 0000000000..2aceb65e5e
--- /dev/null
+++ b/sw/airborne/modules/decawave/ekf_range.c
@@ -0,0 +1,198 @@
+/*
+ * 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
+ * .
+ */
+/**
+ * @file "modules/decawave/ekf_range.c"
+ * @author Gautier Hattenberger
+ * EKF based on range measurements algorithm
+ */
+
+#include "ekf_range.h"
+#include
+
+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
+}
+
diff --git a/sw/airborne/modules/decawave/ekf_range.h b/sw/airborne/modules/decawave/ekf_range.h
new file mode 100644
index 0000000000..b4054b3ad8
--- /dev/null
+++ b/sw/airborne/modules/decawave/ekf_range.h
@@ -0,0 +1,131 @@
+/*
+ * 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
+ * .
+ */
+/**
+ * @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
+
+
diff --git a/sw/airborne/modules/decawave/trilateration.c b/sw/airborne/modules/decawave/trilateration.c
index c2a796333e..5a45793729 100644
--- a/sw/airborne/modules/decawave/trilateration.c
+++ b/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;
}
diff --git a/sw/airborne/modules/decawave/trilateration.h b/sw/airborne/modules/decawave/trilateration.h
index c96a2861a5..254d511472 100644
--- a/sw/airborne/modules/decawave/trilateration.h
+++ b/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
diff --git a/sw/airborne/modules/sensors/cameras/jevois.c b/sw/airborne/modules/sensors/cameras/jevois.c
index e0bb311279..fc9be329da 100644
--- a/sw/airborne/modules/sensors/cameras/jevois.c
+++ b/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
#include
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);
}
diff --git a/sw/airborne/modules/sensors/cameras/jevois.h b/sw/airborne/modules/sensors/cameras/jevois.h
index 18afe2acdb..39f60bfc8a 100644
--- a/sw/airborne/modules/sensors/cameras/jevois.h
+++ b/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
diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c
index 0fd6670461..c7c527efe1 100644
--- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c
+++ b/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);
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index 61192d861f..d9173a6dc9 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit 61192d861fba6dd56c79af0471b93377028bbba8
+Subproject commit d9173a6dc9cf66f97da7d4fab2bdf5d198777419