System identification module (paparazzi#2544)

Co-authored-by: Joost Meulenbeld <joost.meulenbeld@gmail.com>

conf/airframes/tudelft/aa_quadplane.xml

@@ -44,6 +44,8 @@
     <module name="geo_mag"/>
     <module name="air_data"/>
 
+    <module name="sys_id_chirp"/>
+
   </firmware>
 
 <!--   COMMANDS    -->
@@ -76,6 +78,7 @@
 <!--   COMMANDS LAWS   -->
 
   <command_laws>
+    <call fun="sys_id_chirp_add_values(autopilot_get_motors_on(),FALSE,values)"/>
     <!-- Switch to command transition to forward flight -->
     <let var="forward_on"     value="@FORWARD_MODE > -4800? 1 : 0"/>
     <let var="forward_only"     value="@FORWARD_MODE > 4800? 1 : 0"/>

conf/modules/sys_id_chirp.xml

@@ -0,0 +1,66 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="sys_id_chirp" dir="system_identification">
+  <doc>
+    <description>Chirp maneuver for system itentification.
+    The chirp is a sine wave with frequency increasing constantly in time. It's a good candidate as input for system identification
+    since it covers a broad frequency spectrum. This module automates performance of the chirp and exposes an easy interface
+    for tailoring the maneuver to your specific aircraft.
+
+    The module is used by including the module in the airframe file and adding the following line to the &lt;command_laws&gt; section:
+
+    &lt;call fun="sys_id_chirp_add_values(autopilot_get_motors_on(),FALSE,values)"/&gt;
+
+    You can pick the axes to apply chirps to by setting the CHIRP_AXES variable with the COMMAND_XXX variables where XXX are the actuators defined in 
+    the &lt;commands&gt; section of your airframe.
+
+    Then, the GCS exposes the settings for the chirp.
+      - The Chirp axis settings is the index (0-based) of the axis to choose within the CHIRP_AXES variable specified. In the default, this means i.e. 0 means roll chirp.
+      - Amplitude is the amplitude of the chirp
+      - On-axis noise is the fraction of the chirp amplitude to add as noise to the chirp axis (see pprz_chirp.h for more details)
+      - On-axis noise is the absolute value for off-axis noise (see pprz_chirp.h for more details)
+      - Fstart_hz and Fend_hz are the frequencies in hertz at the start and end of the chirp. Make sure to cover all relevant dynamics frequencies
+      - Length_s is the length in seconds of the chirp
+
+    Start the chirp by pressing the "Chirp start" button in the strip. Pressing "Chirp stop" will instantly stop both chirp and noise.
+
+    Add the message "CHIRP" to your telemetry to see chirp progress, and to your logger to automatically filter chirps in post-processing.
+    </description>
+    <define name="CHIRP_AXES"        value="{COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW}" description="Which axes the chirp is applied to (specify as array with {})"/>
+    <define name="CHIRP_ENABLED"     value="TRUE|FALSE"  description="If false, the chirp does not run and values are not added"/>
+    <define name="CHIRP_USE_NOISE"   value="TRUE|FALSE"  description="If true, add noise to all axes (also the axes where no chirp is active)"/>
+    <define name="CHIRP_EXPONENTIAL" value="TRUE|FALSE"  description="If true, exponential-time chirp. Else, linear-time chirp"/>
+    <define name="CHIRP_FADEIN"      value="TRUE|FALSE"  description="If true, start the chirp with two wavelengths of the lowest frequency"/>
+  </doc>
+
+  <settings>
+    <dl_settings name="System identification">
+      <dl_settings name="System chirp">
+        <dl_setting min="0" max="1"    step="1" values="Inactive|Active" shortname="Activate chirp" var="chirp_active" type="uint8_t" module="system_identification/sys_id_chirp" handler="activate_handler">
+          <strip_button name="Chirp start" value="1" group="System identification"/>
+          <strip_button name="Chirp stop"  value="0" group="System identification"/>
+        </dl_setting>
+        <dl_setting min="0"    max="5"    step="1"    shortname="Chirp axis"     var="chirp_axis"                    type="uint8_t" module="system_identification/sys_id_chirp" handler="axis_handler"/>
+        <dl_setting min="0"    max="9600" step="100"  shortname="Amplitude"      var="chirp_amplitude"               type="int32_t" module="system_identification/sys_id_chirp"/>
+        <dl_setting min="0"    max="0.5"    step="0.01" shortname="on-axis noise"  var="chirp_noise_stdv_onaxis_ratio" type="float"   module="system_identification/sys_id_chirp"/>
+        <dl_setting min="0"    max="9600" step="50"   shortname="off-axis noise" var="chirp_noise_stdv_offaxis"      type="float"   module="system_identification/sys_id_chirp"/>
+        <dl_setting min="0.05" max="20"    step="0.05" shortname="Fstart_hz"      var="chirp_fstart_hz"                   type="float"   module="system_identification/sys_id_chirp" handler="fstart_handler"/>
+        <dl_setting min="0.1"  max="20"   step="0.1"  shortname="Fend_hz"        var="chirp_fstop_hz"                   type="float"   module="system_identification/sys_id_chirp" handler="fstop_handler"/>
+        <dl_setting min="0"    max="100"  step="0.5"  shortname="Length_s"       var="chirp_length_s"                type="float"   module="system_identification/sys_id_chirp"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
+  <header>
+    <file name="sys_id_chirp.h"/>
+  </header>
+
+  <init fun="sys_id_chirp_init()"/>
+  <periodic fun="sys_id_chirp_run()" freq="60" autorun="TRUE"/>
+
+  <makefile>
+    <file name="pprz_random.c" dir="math"/>
+    <file name="pprz_chirp.c"/>
+    <file name="sys_id_chirp.c"/>
+  </makefile>
+</module>

conf/userconf/tudelft/conf.xml

@@ -328,7 +328,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/control/stabilization_att_int.xml settings/nps.xml settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml"
-   settings_modules="modules/air_data.xml modules/geo_mag.xml modules/gps_ubx_ucenter.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
+   settings_modules="modules/sys_id_chirp.xml modules/air_data.xml modules/geo_mag.xml modules/gps_ubx_ucenter.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
    gui_color="blue"
    release="f739a81cfa2c633e33f31ab5f095d8f617276974"
   />

sw/airborne/math/pprz_random.c

@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) Joost Meulenbeld
+ *
+ * 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 "math/pprz_random.c"
+ * @author Joost Meulenbeld
+ * Random number functions
+ */
+
+#include "pprz_random.h"
+
+#ifdef BOARD_CONFIG
+#include "mcu_periph/sys_time.h"
+#else
+#include <time.h>
+#endif
+
+
+void init_random(void)
+{
+#ifdef BOARD_CONFIG
+  srand(get_sys_time_msec());
+#else
+  srand(time(NULL));
+#endif
+
+}
+
+double rand_uniform(void)
+{
+  return (double) rand() / RAND_MAX;
+}
+
+/*
+ * http://www.taygeta.com/random/gaussian.html
+ */
+double rand_gaussian(void)
+{
+  static int nb_call = 0;
+  static double x2;
+  static double w;
+  double x1;
+
+  nb_call++;
+  if (nb_call % 2) {
+    do {
+      x1 = 2.0 * rand_uniform() - 1.0;
+      x2 = 2.0 * rand_uniform() - 1.0;
+      w = x1 * x1 + x2 * x2;
+    } while (w >= 1.0 || w == 0.0);
+
+    w = sqrt((-2.0 * log(w)) / w);
+    return x1 * w;
+  } else {
+    return x2 * w;
+  }
+}

sw/airborne/math/pprz_random.h

@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2017-2018 Joost Meulenbeld
+ *
+ * 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 "math/pprz_random.c"
+ * @author Joost Meulenbeld
+ *
+ * Functions for getting random numbers. The rand_gaussian() internally uses the rand_uniform().
+ * rand_uniform() uses rand() internally which is initialized with the current time on rand_init().
+ * This means that the board doesn't need an internal rng but comes at the cost of more computations.
+ *
+ * Usage:
+ * rand_init(); // initialize once
+ *
+ * float random_number = rand_uniform();
+ */
+
+#ifndef RANDOM_H
+#define RANDOM_H
+
+#include <std.h>
+#include <stdlib.h>
+#include <math.h>
+#include <stdbool.h>
+
+// Initialize the random number generator (call this before using the other functions)
+void init_random(void);
+
+// Random number from uniform[0,1] distribution
+double rand_uniform(void);
+
+// Random number from gaussian(0, 1) distribution
+double rand_gaussian(void);
+
+#endif // RANDOM_H

sw/airborne/modules/system_identification/pprz_chirp.c

@@ -0,0 +1,117 @@
+/*
+ * Copyright (C) Joost Meulenbeld
+ *
+ * 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/system_identification/pprz_chirp.c"
+ * @author Joost Meulenbeld
+ * Mathematical implementation of the chirp
+ */
+#include "pprz_chirp.h"
+#include "std.h"
+
+// Values for exponential chirp (See ref [2] in the header file). C2 is based on C1 s.t. the frequency range exactly covers the required range
+#define CHIRP_C1 4.0f
+#define CHIRP_C2 1.0f / (expf(CHIRP_C1) - 1)
+
+
+
+void chirp_init(struct chirp_t *chirp, float f0_hz, float f1_hz, float length_s, float current_time_s,
+                bool exponential_chirp, bool fade_in)
+{
+  chirp->f0_hz = f0_hz;
+  chirp->f1_hz = f1_hz;
+
+  chirp->length_s = length_s;
+  if (fade_in) { // The fade-in takes two of the longest wave-lengths, total_length is including that time
+    chirp->total_length_s = length_s + 2 / f0_hz;
+  } else {
+    chirp->total_length_s = length_s;
+  }
+
+  chirp->start_time_s = current_time_s;
+  chirp->exponential_chirp = exponential_chirp;
+  chirp->fade_in = fade_in;
+
+  chirp->current_frequency_hz = 0;
+  chirp->current_value = 0;
+  chirp->percentage_done = 0;
+}
+
+void chirp_reset(struct chirp_t *chirp, float current_time_s)
+{
+  chirp->current_time_s = current_time_s;
+  chirp->start_time_s = current_time_s;
+  chirp->current_frequency_hz = 0;
+  chirp->current_value = 0;
+  chirp->percentage_done = 0;
+}
+
+bool chirp_is_running(struct chirp_t *chirp, float current_time_s)
+{
+  float t = current_time_s - chirp->start_time_s;
+  return (t >= 0) && (t <= chirp->total_length_s);
+}
+
+float chirp_update(struct chirp_t *chirp, float current_time_s)
+{
+  if (!chirp_is_running(chirp, current_time_s)) { // Outside the chirp interval, return 0
+    chirp->current_value = 0.0f;
+    return 0;
+  }
+
+  float t = current_time_s - chirp->start_time_s; // Time since the start of the chirp
+  chirp->current_time_s = current_time_s;
+  // Protect against divide by zero
+  if (chirp->total_length_s <= 0) {
+    chirp->total_length_s = 0.01;
+  }
+  chirp->percentage_done = t / chirp->total_length_s;
+
+  if (chirp->fade_in && t < 2 / chirp->f0_hz) { // Fade-in is two times the wavelength of f0
+    chirp->current_frequency_hz = chirp->f0_hz;
+
+    // First wavelength increases linearly in amplitude, second wavelength has unity amplitude
+    chirp->current_value = sinf(t * 2 * M_PI * chirp->f0_hz) * Min(1, t * chirp->f0_hz);
+    return chirp->current_value;
+  }
+
+  // If the fade-in is finished, the current time t is the time since the fade-in stopped
+  if (chirp->fade_in) {
+    t -= 2 / chirp->f0_hz;
+  }
+
+  if (chirp->exponential_chirp) { // See the book referenced in the header for the equations
+    float exponential = expf(CHIRP_C1 * t / chirp->length_s);
+    float K = CHIRP_C2 * (exponential - 1);
+
+    chirp->current_frequency_hz = chirp->f0_hz + K * (chirp->f1_hz - chirp->f0_hz);
+
+    float theta = 2 * M_PI * (chirp->f0_hz * t
+                              + (chirp->f1_hz - chirp->f0_hz) * (chirp->length_s / CHIRP_C1 * K - CHIRP_C2 * t));
+
+    chirp->current_value = sinf(theta);
+  } else { // linear-time chirp
+    float k = (chirp->f1_hz - chirp->f0_hz) / chirp->length_s;
+
+    chirp->current_frequency_hz = k * t;
+    chirp->current_value = sinf(2 * M_PI * t * (chirp->f0_hz + chirp->current_frequency_hz / 2));
+  }
+
+  return chirp->current_value;
+}