diff --git a/conf/modules/dragspeed.xml b/conf/modules/dragspeed.xml
new file mode 100644
index 0000000000..15fcae8c80
--- /dev/null
+++ b/conf/modules/dragspeed.xml
@@ -0,0 +1,114 @@
+
+
+
+
+
+ Estimate the velocity/airspeed of rotorcraft by measuring the drag force.
+
+ @image html images/airborne/modules/dragspeed/dragspeed.png
+
+ During flight, gravity, thrust and drag act on the quadrotor.
+ Gravity is not measured by accelerometers and the thrust only acts along
+ the drone's vertical axis. Therefore, only the drag is measured along the
+ quadrotor's horizontal axes. Using a simple linear drag model, this
+ deceleration can be transformed back into an estimate of the drone's
+ airspeed.
+
+ The advantages of this method are that the acceleration is not integrated
+ over time (the only source of drift is the drift of the sensor itself)
+ and that no knowledge of the quadrotor's attitude or other states is
+ required.
+
+ This module makes the following assumptions:
+ - The drag acting on the quadrotor grows linearly with velocity.
+ - The size and weight of the drone are constant (changes in size and/or
+ weight require re-calibration).
+ - Pitch and roll angles are small (less than ~20 deg).
+ - No wind (when used as ground speed).
+
+
+ Usage instructions
+ ------------------
+ 1. Calibrate zero (recommended before each flight)
+ 1. For the best results, ensure that the INS velocity is as accurate as
+ possible (e.g. use Optitrack and set the INS type to
+ gps_passthrough) and set DRAGSPEED_SEND_ABI_MESSAGE to FALSE.
+ 2. Bring the drone to a stationary hover.
+ 3. In the GCS under settings/dragspeed, set "Calibrate Zero" to 1.
+ 4. Keep the drone stationary until "Calibrate Zero" returns to 0.
+ 5. (Optional) Copy the new Zero_X and Zero_Y values to the
+ DRAGSPEED_ZERO_X and _Y defines in the airframe file.
+ 2. Calibrate drag coefficient (new airframes or after change in weight or
+ shape)
+ 1. For the best results, ensure that the INS velocity is as accurate as
+ possible (e.g. use Optitrack and set the INS type to
+ gps_passthrough) and set DRAGSPEED_SEND_ABI_MESSAGE to FALSE.
+ 2. Ensure the zero measurement is calibrated first. Drag coefficient
+ calibration will not start unless Zero_X and _Y values are
+ available.
+ 3. In the GCS under settings/dragspeed, set "Calibrate Coeff" to 1.
+ 4. Fly the drone around along the body x and y axes. The drag
+ coefficient is updated at speeds larger than 0.5m/s.
+ 5. Calibration is finished when "Calibrate Coeff" returns to 0. Check
+ the results by comparing the estimated and INS velocities in the
+ DRAGSPEED telemetry message.
+ 6. Copy the new Coeff_X and Coeff_Y values to the DRAGSPEED_COEFF_X
+ and _Y defines in the airframe file.
+ 3. By default, the estimated velocity is sent through the
+ VELOCITY_ESTIMATE ABI message to the INS. The estimated velocity can
+ also be read from the dragspeed struct.
+
+
+ Calibration from flight plan
+ ----------------------------
+ Calibration can be started from the flight plan by calling
+ dragspeed_calibrate_zero() or dragspeed_calibrate_coeff(). Progress of the
+ calibration can be monitored through dragspeed_is_calibrating(), which
+ returns TRUE while either calibration routine is in progress.
+
+
+ Example results
+ ---------------
+ The following results were obtained from a flight test performed on an
+ AR.Drone 2.0 inside the TU Delft Cyberzoo. The ground-truth velocities
+ are obtained through the Optitrack system.
+
+ @image html images/airborne/modules/dragspeed/example_result.png
+
+ The following settings were used:
+ - DRAGSPEED_COEFF_X = 0.62
+ - DRAGSPEED_COEFF_Y = 0.85
+ - DRAGSPEED_FILTER = 0.8
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index 8cef08df02..cd56f46813 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -28,6 +28,7 @@
+
diff --git a/doc/images/airborne/modules/dragspeed/dragspeed.png b/doc/images/airborne/modules/dragspeed/dragspeed.png
new file mode 100644
index 0000000000..f87a61858b
Binary files /dev/null and b/doc/images/airborne/modules/dragspeed/dragspeed.png differ
diff --git a/doc/images/airborne/modules/dragspeed/example_result.png b/doc/images/airborne/modules/dragspeed/example_result.png
new file mode 100644
index 0000000000..65d397c4eb
Binary files /dev/null and b/doc/images/airborne/modules/dragspeed/example_result.png differ
diff --git a/sw/airborne/modules/dragspeed/dragspeed.c b/sw/airborne/modules/dragspeed/dragspeed.c
new file mode 100644
index 0000000000..ef84e1c416
--- /dev/null
+++ b/sw/airborne/modules/dragspeed/dragspeed.c
@@ -0,0 +1,264 @@
+/*
+ * Copyright (C) Tom van Dijk
+ *
+ * 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/dragspeed/dragspeed.c"
+ * @author Tom van Dijk
+ * This module estimates the velocity of rotorcraft by measuring the drag force
+ * using the accelerometer.
+ *
+ * During flight, gravity, thrust and drag act on the quadrotor. Only the drag
+ * is measured along the quadrotor's horizontal axes. Using a simple linear drag
+ * model, this deceleration can be transformed back into an estimate of the
+ * drone's airspeed.
+ *
+ * This module makes the following assumptions:
+ * - The drag acting on the quadrotor grows linearly with velocity.
+ * - The size and weight of the drone are constant (changes in size and weight
+ * require re-calibration).
+ * - Pitch and roll angles are small (< ~20 deg).
+ * - No wind (when used as ground speed).
+ */
+
+#include "modules/dragspeed/dragspeed.h"
+
+#include "subsystems/abi.h"
+#include "subsystems/abi_common.h"
+#include "subsystems/datalink/telemetry.h"
+
+#include
+
+#ifndef DRAGSPEED_SEND_ABI_MESSAGE
+#define DRAGSPEED_SEND_ABI_MESSAGE TRUE
+#endif
+
+#ifndef DRAGSPEED_ACCEL_ID
+#define DRAGSPEED_ACCEL_ID ABI_BROADCAST
+#endif
+
+#ifndef DRAGSPEED_COEFF_X
+#define DRAGSPEED_COEFF_X 1.0 /// Drag coefficient (mu/m) of the linear drag model along x axis, where m*a = v*mu
+#endif
+
+#ifndef DRAGSPEED_COEFF_Y
+#define DRAGSPEED_COEFF_Y 1.0 /// Drag coefficient (mu/m) of the linear drag model along y axis, where m*a = v*mu
+#endif
+
+#ifndef DRAGSPEED_R
+#define DRAGSPEED_R 0.25 /// Velocity measurement noise variance [(m/s)^2]
+#endif
+
+#ifndef DRAGSPEED_FILTER
+#define DRAGSPEED_FILTER 0.8 /// First-order low-pass filter strength [0..1]. Pretty high default value as accelero's tend to be noisy.
+#endif
+
+struct dragspeed_t dragspeed;
+
+static abi_event accel_ev;
+static void accel_cb(uint8_t sender_id, uint32_t stamp,
+ struct Int32Vect3 *accel);
+
+static void send_dragspeed(struct transport_tx *trans, struct link_device *dev);
+
+static void calibrate_coeff(struct Int32Vect3 *accel);
+static void calibrate_zero(struct Int32Vect3 *accel);
+
+void dragspeed_init(void)
+{
+ // Set initial values
+ dragspeed.coeff.x = DRAGSPEED_COEFF_X;
+ dragspeed.coeff.y = DRAGSPEED_COEFF_Y;
+ dragspeed.filter = DRAGSPEED_FILTER;
+#ifdef DRAGSPEED_ZERO_X
+ dragspeed.zero.x = DRAGSPEED_ZERO_X;
+#endif
+#ifdef DRAGSPEED_ZERO_Y
+ dragspeed.zero.y = DRAGSPEED_ZERO_Y;
+#endif
+#if defined(DRAGSPEED_ZERO_X) && defined(DRAGSPEED_ZERO_Y)
+ dragspeed.zero_calibrated = TRUE;
+#else
+ dragspeed.zero_calibrated = FALSE;
+#endif
+ // Register callbacks
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DRAGSPEED,
+ send_dragspeed);
+ AbiBindMsgIMU_ACCEL_INT32(DRAGSPEED_ACCEL_ID, &accel_ev, accel_cb);
+}
+
+bool dragspeed_calibrate_coeff(void)
+{
+ dragspeed.calibrate_coeff = TRUE;
+ return FALSE;
+}
+
+bool dragspeed_calibrate_zero(void)
+{
+ dragspeed.calibrate_zero = TRUE;
+ return FALSE;
+}
+
+bool dragspeed_is_calibrating(void)
+{
+ return dragspeed.calibrate_coeff || dragspeed.calibrate_zero;
+}
+
+static void accel_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp,
+ struct Int32Vect3 *accel)
+{
+ // Estimate current velocity
+ float vx = -(ACCEL_FLOAT_OF_BFP(accel->x) - dragspeed.zero.x)
+ / dragspeed.coeff.x;
+ float vy = -(ACCEL_FLOAT_OF_BFP(accel->y) - dragspeed.zero.y)
+ / dragspeed.coeff.y;
+ // Simple low-pass filter
+ dragspeed.vel.x += (1 - dragspeed.filter) * (vx - dragspeed.vel.x);
+ dragspeed.vel.y += (1 - dragspeed.filter) * (vy - dragspeed.vel.y);
+ // Send as ABI VELOCITY_ESTIMATE message
+#if DRAGSPEED_SEND_ABI_MESSAGE
+ if (!dragspeed.calibrate_coeff && !dragspeed.calibrate_zero) {
+ float vel_z = stateGetSpeedNed_f()->z; // Workaround until VELOCITY_ESTIMATE contains flags to specify which axes are estimated.
+ AbiSendMsgVELOCITY_ESTIMATE(VEL_DRAGSPEED_ID, stamp, dragspeed.vel.x,
+ dragspeed.vel.y, vel_z, DRAGSPEED_R);
+ }
+#endif
+ // Perform calibration if required
+ calibrate_coeff(accel);
+ calibrate_zero(accel);
+}
+
+/**
+ * Calibrate drag coefficient by comparing accelerometer measurements to INS
+ * velocities.
+ * Should be performed with VEL_DRAGSPEED_ID set to ABI_DISABLE, but for safety
+ * the ABI messages are also disabled automatically when calibration is active.
+ *
+ * This routine requires the accelerometers to have been zeroed beforehand,
+ * otherwise it will return without changing the drag coefficient!
+ */
+static void calibrate_coeff(struct Int32Vect3 *accel)
+{
+ // Reset when new calibration is started
+ static bool calibrate_prev = FALSE;
+ static struct FloatVect2 coeff;
+ static int num_samples_x = 0;
+ static int num_samples_y = 0;
+ if (dragspeed.calibrate_coeff && !calibrate_prev) {
+ coeff.x = 0.0f;
+ coeff.y = 0.0f;
+ num_samples_x = 0;
+ num_samples_y = 0;
+ }
+ calibrate_prev = dragspeed.calibrate_coeff;
+ // Return when calibration is not active
+ if (!dragspeed.calibrate_coeff) {
+ return;
+ }
+ // Also return if zero calibration has not been performed yet
+ if (!dragspeed.zero_calibrated) {
+#ifdef __linux__
+ fprintf(stderr,
+ "[dragspeed] Error: zero measurement should be calibrated before drag coefficient!\n");
+#endif
+ dragspeed.calibrate_coeff = FALSE;
+ return;
+ }
+
+ // Calculate INS velocity in body frame
+ struct FloatEulers *att = stateGetNedToBodyEulers_f();
+ struct EnuCoor_f *vel_ins = stateGetSpeedEnu_f();
+ struct FloatVect2 vel_ins_body = { cosf(att->psi) * vel_ins->y
+ + sinf(att->psi) * vel_ins->x, -sinf(att->psi) * vel_ins->y
+ + cosf(att->psi) * vel_ins->x };
+ // Calibrate coefficient when velocity is sufficiently high
+ if (fabsf(vel_ins_body.x) > 0.5) {
+ float this_coeff = -(ACCEL_FLOAT_OF_BFP(accel->x) - dragspeed.zero.x)
+ / vel_ins_body.x;
+ coeff.x = (coeff.x * num_samples_x + this_coeff) / (num_samples_x + 1);
+ num_samples_x++;
+ }
+ if (fabsf(vel_ins_body.y) > 0.5) {
+ float this_coeff = -(ACCEL_FLOAT_OF_BFP(accel->y) - dragspeed.zero.y)
+ / vel_ins_body.y;
+ coeff.y = (coeff.y * num_samples_y + this_coeff) / (num_samples_y + 1);
+ num_samples_y++;
+ }
+ // End calibration when enough samples are averaged
+ if (num_samples_x > 1000 && num_samples_y > 1000 && coeff.x != 0
+ && coeff.y != 0) {
+ dragspeed.coeff = coeff;
+ dragspeed.calibrate_coeff = FALSE;
+ }
+}
+
+/**
+ * Calibrate zero velocity by measuring the accelerations while the drone
+ * hovers in-place.
+ *
+ * The zero-velocity readings can change between flights, e.g. because of small
+ * differences in battery or outer hull positions.
+ */
+static void calibrate_zero(struct Int32Vect3 *accel)
+{
+ // Reset when new calibration is started
+ static bool calibrate_prev = FALSE;
+ static struct FloatVect2 zero;
+ static int num_samples = 0;
+ if (dragspeed.calibrate_zero && !calibrate_prev) {
+ zero.x = 0.0f;
+ zero.y = 0.0f;
+ num_samples = 0;
+ }
+ calibrate_prev = dragspeed.calibrate_zero;
+ // Return when calibration is not active
+ if (!dragspeed.calibrate_zero) {
+ return;
+ }
+
+ // Average accelerometer readings when velocity is sufficiently low
+ struct EnuCoor_f *vel_ins = stateGetSpeedEnu_f();
+ float ins_speed = sqrtf(vel_ins->x * vel_ins->x + vel_ins->y * vel_ins->y);
+ if (ins_speed < 0.1) {
+ zero.x = (zero.x * num_samples + ACCEL_FLOAT_OF_BFP(accel->x))
+ / (num_samples + 1);
+ zero.y = (zero.y * num_samples + ACCEL_FLOAT_OF_BFP(accel->y))
+ / (num_samples + 1);
+ num_samples++;
+ // End calibration when enough samples are averaged
+ if (num_samples > 1000) {
+ dragspeed.zero = zero;
+ dragspeed.calibrate_zero = FALSE;
+ dragspeed.zero_calibrated = TRUE;
+ }
+ }
+}
+
+static void send_dragspeed(struct transport_tx *trans, struct link_device *dev)
+{
+ // Calculate INS velocity in body frame
+ struct FloatEulers *att = stateGetNedToBodyEulers_f();
+ struct EnuCoor_f *vel_ins = stateGetSpeedEnu_f();
+ struct FloatVect2 vel_ins_body = { cosf(att->psi) * vel_ins->y
+ + sinf(att->psi) * vel_ins->x, -sinf(att->psi) * vel_ins->y
+ + cosf(att->psi) * vel_ins->x };
+ // Send telemetry message
+ pprz_msg_send_DRAGSPEED(trans, dev, AC_ID, &dragspeed.vel.x, &dragspeed.vel.y,
+ &vel_ins_body.x, &vel_ins_body.y);
+}
+
diff --git a/sw/airborne/modules/dragspeed/dragspeed.h b/sw/airborne/modules/dragspeed/dragspeed.h
new file mode 100644
index 0000000000..49baa85714
--- /dev/null
+++ b/sw/airborne/modules/dragspeed/dragspeed.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) Tom van Dijk
+ *
+ * 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/dragspeed/dragspeed.h"
+ * @author Tom van Dijk
+ * This module estimates the velocity of rotorcraft by measuring the drag force using the accelerometer.
+ */
+
+#ifndef DRAGSPEED_H
+#define DRAGSPEED_H
+
+#include "math/pprz_algebra_float.h"
+
+struct dragspeed_t
+{
+ // Estimated velocity
+ struct FloatVect2 vel;
+ // Low-pass filter
+ float filter;
+ // Drag coefficient calibration
+ struct FloatVect2 coeff;
+ bool calibrate_coeff;
+ // Zero calibration
+ struct FloatVect2 zero;
+ bool calibrate_zero;
+ bool zero_calibrated;
+};
+extern struct dragspeed_t dragspeed;
+
+extern void dragspeed_init(void);
+
+// Calibration functions for use in flight plans
+extern bool dragspeed_calibrate_coeff(void);
+extern bool dragspeed_calibrate_zero(void);
+extern bool dragspeed_is_calibrating(void);
+
+#endif
+
diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h
index 3332aac2f6..d13cae3258 100644
--- a/sw/airborne/subsystems/abi_sender_ids.h
+++ b/sw/airborne/subsystems/abi_sender_ids.h
@@ -284,6 +284,13 @@
#define IMU_VECTORNAV_ID 19
#endif
+/*
+ * IDs of VELOCITY estimates (message 12)
+ */
+#ifndef VEL_DRAGSPEED_ID
+#define VEL_DRAGSPEED_ID 1
+#endif
+
/*
* IDs of RSSI measurements (message 13)
*/
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index 625ed366b4..9525de7663 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit 625ed366b45c1dbc3de1de3702f1cba661aff06e
+Subproject commit 9525de76635aac9b6dc058cc3940fa8fff17cd53