Drag-based velocity estimator (#2132)

conf/modules/dragspeed.xml

@@ -0,0 +1,114 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="dragspeed">
+  <doc>
+    <description>
+      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
+      
+    </description>
+    <define name="DRAGSPEED_SEND_ABI_MESSAGE" description="Send VELOCITY_ESTIMATE ABI messages" value="TRUE|FALSE (default: TRUE)"/>
+    <define name="DRAGSPEED_ACCEL_ID" description="Accelerometer ABI ID to subscribe to (default: ABI_BROADCAST)"/>
+    <define name="DRAGSPEED_COEFF_X" description="Drag coefficient (mu/m) of the linear drag model along the body x axis, where m*a = v*mu"/>
+    <define name="DRAGSPEED_COEFF_Y" description="Drag coefficient (mu/m) of the linear drag model along the body y axis, where m*a = v*mu"/>
+    <define name="DRAGSPEED_ZERO_X" description="Accelerometer reading (x axis) when stationary [m/s^2]"/>
+    <define name="DRAGSPEED_ZERO_Y" description="Accelerometer reading (y axis) when stationary [m/s^2]"/>
+    <define name="DRAGSPEED_R" description="Velocity measurement noise variance [(m/s)^2]"/>
+    <define name="DRAGSPEED_FILTER" description="First-order low-pass filter strength [0..1] (default: 0.8)"/>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings name="dragspeed">
+        <dl_setting shortname="Calibrate zero" var="dragspeed.calibrate_zero" min="0" step="1" max="1"/>
+        <dl_setting shortname="Zero_X" var="dragspeed.zero.x" min="-2.0" step="0.01" max="2.0"/>
+        <dl_setting shortname="Zero_Y" var="dragspeed.zero.y" min="-2.0" step="0.01" max="2.0"/>
+        <dl_setting shortname="Calibrate coeff" var="dragspeed.calibrate_coeff" min="0" step="1" max="1"/>
+        <dl_setting shortname="Coeff X" var="dragspeed.coeff.x" min="0" step="0.01" max="2.0"/>
+        <dl_setting shortname="Coeff Y" var="dragspeed.coeff.y" min="0" step="0.01" max="2.0"/>
+        <dl_setting shortname="Filter" var="dragspeed.filter" min="0" step="0.01" max="1.0"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+  <header>
+    <file name="dragspeed.h"/>
+  </header>
+  <init fun="dragspeed_init()"/>
+  <makefile>
+    <file name="dragspeed.c"/>
+  </makefile>
+</module>
+

conf/telemetry/default_rotorcraft.xml

@@ -28,6 +28,7 @@
       <message name="OPTIC_FLOW_EST"         period="0.05"/>
       <message name="VECTORNAV_INFO"         period="0.5"/>
       <message name="DIVERGENCE"             period="0.05"/>
+      <message name="DRAGSPEED"              period="0.02"/>
     </mode>
 
     <mode name="ppm">

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
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @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 <stdio.h>
+
+#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);
+}
+

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
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @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
+

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)
  */