mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-22 04:13:39 +08:00
System identification module (#2544)
Co-authored-by: Joost Meulenbeld <joost.meulenbeld@gmail.com>
This commit is contained in:
Christophe De Wagter
committed by
GitHub
GitHub
parent
c118b0801c
commit
94b1c3d24f
@@ -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"/>
|
||||
|
||||
@@ -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 <command_laws> section:
|
||||
|
||||
<call fun="sys_id_chirp_add_values(autopilot_get_motors_on(),FALSE,values)"/>
|
||||
|
||||
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 <commands> 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>
|
||||
@@ -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"
|
||||
/>
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
/*
|
||||
* 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
|
||||
* A "chirp" or frequency sweep is a sine wave with in time increasing frequency, and can be
|
||||
* used for system identification purposes. This registers a broad frequency spectrum.
|
||||
*
|
||||
* Chirps can be made with frequency increasing linearly or exponentially with time. This is set using an argument in
|
||||
* the chirp_init method. The latter one is better for system identification, according to [2].
|
||||
*
|
||||
* The time length of the chirp is best put at a minimum of 4 / f_minimum such that low-frequency effects
|
||||
* can be correctly discovered.
|
||||
*
|
||||
* The fade_in argument determines if the chirp fades in with increasing amplitude at
|
||||
* constant frequency (the lowest frequency). This makes it much easier to maintain the UAV around the trim position.
|
||||
*
|
||||
* Usage example:
|
||||
* // Initialize a chirp between 1 and 5 Hz during 2 seconds (for sys. id. use minimum of 4 * 1/f0)
|
||||
* struct chirp_t chirp;
|
||||
* chirp_init(&chirp, 1, 5, 2, get_current_time());
|
||||
*
|
||||
* // During flight loop, add chirp value to stick input
|
||||
* while (chirp_is_running(chirp, get_current_time())) {
|
||||
* control[YAW] = stick_control + chirp_update(chirp, get_current_time());
|
||||
* }
|
||||
*
|
||||
* [1] https://en.wikipedia.org/wiki/Chirp for a derivation of the linear chirp
|
||||
* [2] Aircraft and Rotorcraft System Identification, 2nd edition by M. Tischler for a derivation of exponential chirp
|
||||
*/
|
||||
|
||||
#ifndef PPRZ_CHIRP_H
|
||||
#define PPRZ_CHIRP_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
/**
|
||||
* Initialize with chirp_init
|
||||
*/
|
||||
struct chirp_t {
|
||||
float f0_hz;
|
||||
float f1_hz;
|
||||
float start_time_s;
|
||||
float length_s; // Amount of seconds of the chirp, excluding fade-in if applicable
|
||||
float total_length_s; // Amount of seconds of the chirp, including fade-in if applicable
|
||||
|
||||
float current_frequency_hz;
|
||||
float current_value; // Value is [-1, 1]
|
||||
float current_time_s;
|
||||
float percentage_done; // t / total_length: [0, 1]
|
||||
|
||||
bool exponential_chirp;
|
||||
bool fade_in;
|
||||
};
|
||||
|
||||
/**
|
||||
* Allocate and initialize a new chirp struct. set start_time to the current time
|
||||
* @param f0_hz: Minimum frequency of the chirp in Hz
|
||||
* @param f1_hz: Maximum frequency of the chirp in Hz
|
||||
* @param length_s: Time interval in s (starting from start_time_s) in which the chirp should be carried out, excluding fade-in time
|
||||
* @param current_time_s: Current time in s, starting point of the chirp
|
||||
* @param exponential_chirp: If true, use exponential-time chirp, otherwise use linear-time chirp (see wikipedia)
|
||||
* @param fade_in: If true, begin the chirp with 2 wavelengths of the lowest frequency, with increasing amplitude
|
||||
*/
|
||||
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);
|
||||
|
||||
/**
|
||||
* Reset the time of the chirp
|
||||
* @param chirp: The chirp struct pointer to reset
|
||||
* @param current_time_s: The time to set the chirp start at
|
||||
**/
|
||||
void chirp_reset(struct chirp_t *chirp, float current_time_s);
|
||||
|
||||
/**
|
||||
* Return if the current_time is within the chirp manoeuvre
|
||||
*/
|
||||
bool chirp_is_running(struct chirp_t *chirp, float current_time_s);
|
||||
|
||||
/**
|
||||
* Calculate the value at current_time_s and update the struct with current frequency and value
|
||||
* @return Current value chirp->current_value
|
||||
*/
|
||||
float chirp_update(struct chirp_t *chirp, float current_time_s);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,208 @@
|
||||
/*
|
||||
* 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/sys_id_chirp.c"
|
||||
* @author Joost Meulenbeld
|
||||
* System identification chirp
|
||||
*/
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#include "sys_id_chirp.h"
|
||||
#include "pprz_chirp.h"
|
||||
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "mcu_periph/sys_time.h"
|
||||
#include "filters/low_pass_filter.h"
|
||||
#include "math/pprz_random.h"
|
||||
|
||||
|
||||
#ifndef CHIRP_AXES
|
||||
#define CHIRP_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW}
|
||||
#endif
|
||||
|
||||
#ifndef CHIRP_ENABLED
|
||||
#define CHIRP_ENABLED TRUE
|
||||
#endif
|
||||
|
||||
#ifndef CHIRP_USE_NOISE
|
||||
#define CHIRP_USE_NOISE TRUE
|
||||
#endif
|
||||
|
||||
#ifndef CHIRP_EXPONENTIAL
|
||||
#define CHIRP_EXPONENTIAL TRUE
|
||||
#endif
|
||||
|
||||
#ifndef CHIRP_FADEIN
|
||||
#define CHIRP_FADEIN TRUE
|
||||
#endif
|
||||
|
||||
|
||||
static struct chirp_t chirp;
|
||||
uint8_t chirp_active = false;
|
||||
uint8_t chirp_axis = 0;
|
||||
pprz_t chirp_amplitude = 0;
|
||||
float chirp_noise_stdv_onaxis_ratio = 0.1;
|
||||
float chirp_noise_stdv_offaxis = 200;
|
||||
float chirp_fstart_hz = 1.0f;
|
||||
float chirp_fstop_hz = 5.0f;
|
||||
float chirp_length_s = 20;
|
||||
|
||||
// The axes on which noise and chirp values can be applied
|
||||
static const int8_t ACTIVE_CHIRP_AXES[] = CHIRP_AXES;
|
||||
#define CHIRP_NB_AXES sizeof ACTIVE_CHIRP_AXES / sizeof ACTIVE_CHIRP_AXES[0] // Number of items in ACTIVE_CHIRP_AXES
|
||||
|
||||
// Filters used to cut-off the gaussian noise fed into the actuator channels
|
||||
static struct FirstOrderLowPass filters[CHIRP_NB_AXES];
|
||||
|
||||
// Chirp and noise values for all axes (indices correspond to the axes given in CHIRP_AXES)
|
||||
static pprz_t current_chirp_values[CHIRP_NB_AXES];
|
||||
|
||||
static void set_current_chirp_values(void)
|
||||
{
|
||||
if (chirp_active) {
|
||||
#if CHIRP_USE_NOISE
|
||||
|
||||
float amplitude, noise;
|
||||
for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
|
||||
noise = update_first_order_low_pass(&filters[i], rand_gaussian());
|
||||
amplitude = chirp_axis == i ? chirp_noise_stdv_onaxis_ratio * chirp_amplitude : chirp_noise_stdv_offaxis;
|
||||
current_chirp_values[i] = (int32_t)(noise * amplitude);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
current_chirp_values[chirp_axis] += (int32_t)(chirp_amplitude * chirp.current_value);
|
||||
} else {
|
||||
for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
|
||||
current_chirp_values[i] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void send_chirp(struct transport_tx *trans, struct link_device *dev)
|
||||
{
|
||||
pprz_msg_send_CHIRP(trans, dev, AC_ID, &chirp_active, &chirp.percentage_done, &chirp.current_frequency_hz,
|
||||
&chirp_axis, &chirp_amplitude, &chirp_fstart_hz, &chirp_fstop_hz, &chirp_noise_stdv_onaxis_ratio,
|
||||
&chirp_noise_stdv_offaxis);
|
||||
}
|
||||
|
||||
static void start_chirp(void)
|
||||
{
|
||||
chirp_reset(&chirp, get_sys_time_float());
|
||||
chirp_active = true;
|
||||
set_current_chirp_values();
|
||||
}
|
||||
|
||||
static void stop_chirp(void)
|
||||
{
|
||||
chirp_reset(&chirp, get_sys_time_float());
|
||||
chirp_active = false;
|
||||
set_current_chirp_values();
|
||||
}
|
||||
|
||||
void sys_id_chirp_activate_handler(uint8_t activate)
|
||||
{
|
||||
chirp_active = activate;
|
||||
if (chirp_active) {
|
||||
chirp_init(&chirp, chirp_fstart_hz, chirp_fstop_hz, chirp_length_s, get_sys_time_float(), CHIRP_EXPONENTIAL,
|
||||
CHIRP_FADEIN);
|
||||
start_chirp();
|
||||
} else {
|
||||
stop_chirp();
|
||||
}
|
||||
}
|
||||
|
||||
extern void sys_id_chirp_axis_handler(uint8_t axis)
|
||||
{
|
||||
if (axis < CHIRP_NB_AXES) {
|
||||
chirp_axis = axis;
|
||||
}
|
||||
}
|
||||
|
||||
extern void sys_id_chirp_fstart_handler(float fstart)
|
||||
{
|
||||
if (fstart < chirp_fstop_hz) {
|
||||
chirp_fstart_hz = fstart;
|
||||
}
|
||||
}
|
||||
|
||||
extern void sys_id_chirp_fstop_handler(float fstop)
|
||||
{
|
||||
if (fstop > chirp_fstart_hz) {
|
||||
chirp_fstop_hz = fstop;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void sys_id_chirp_init(void)
|
||||
{
|
||||
#if CHIRP_USE_NOISE
|
||||
|
||||
init_random();
|
||||
|
||||
#endif
|
||||
|
||||
chirp_init(&chirp, chirp_fstart_hz, chirp_fstop_hz, chirp_length_s, get_sys_time_float(), CHIRP_EXPONENTIAL,
|
||||
CHIRP_FADEIN);
|
||||
set_current_chirp_values();
|
||||
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CHIRP, send_chirp);
|
||||
|
||||
// Filter cutoff frequency is the chirp maximum frequency
|
||||
float tau = 1 / (chirp_fstop_hz * 2 * M_PI);
|
||||
for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
|
||||
init_first_order_low_pass(&filters[i], tau, SYS_ID_CHIRP_RUN_PERIOD, 0);
|
||||
current_chirp_values[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void sys_id_chirp_run(void)
|
||||
{
|
||||
#if CHIRP_ENABLED
|
||||
|
||||
if (chirp_active) {
|
||||
if (!chirp_is_running(&chirp, get_sys_time_float())) {
|
||||
stop_chirp();
|
||||
} else {
|
||||
chirp_update(&chirp, get_sys_time_float());
|
||||
set_current_chirp_values();
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void sys_id_chirp_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
|
||||
{
|
||||
(void)(override_on); // Suppress unused parameter warnings
|
||||
|
||||
#if CHIRP_ENABLED
|
||||
|
||||
if (motors_on) {
|
||||
for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
|
||||
in_cmd[ACTIVE_CHIRP_AXES[i]] += current_chirp_values[i];
|
||||
BoundAbs(in_cmd[ACTIVE_CHIRP_AXES[i]], MAX_PPRZ);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
/*
|
||||
* 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/helicopter/sys_id_chirp.h"
|
||||
* @author Joost Meulenbeld
|
||||
* System identification chirp
|
||||
*
|
||||
* This is the module implementation for the chirp maneuver. The mathematical definition of the chirp
|
||||
* can be found in pprz_chirp.h. Use sys_id_chirp by adding the module to your airframe file and
|
||||
* adding the following line to the top of the <command_laws> section of your airframe file:
|
||||
*
|
||||
* <call fun="sys_id_chirp_add_values(autopilot_get_motors_on(),FALSE,values)"/>
|
||||
*
|
||||
* In the GCS you can then start and stop the chirp, change frequencies and choose which axis it should
|
||||
* be applied to. Documentation of the specific options can be found in the module xml file.
|
||||
*
|
||||
* The axes to which noise is applied is set in the xml file with the variable CHIRP_AXES. The axis that
|
||||
* is selected in the GCS to apply the chirp on is the index in the CHIRP_AXES array
|
||||
*/
|
||||
|
||||
#ifndef SYS_ID_CHIRP_H
|
||||
#define SYS_ID_CHIRP_H
|
||||
|
||||
#include "paparazzi.h"
|
||||
|
||||
|
||||
extern uint8_t chirp_active;
|
||||
extern pprz_t chirp_amplitude;
|
||||
extern float chirp_noise_stdv_onaxis_ratio; // On-axis noise is amplitude times this value
|
||||
extern float chirp_noise_stdv_offaxis; // Off-axis noise (the axes that the chirp is not applied to)
|
||||
|
||||
extern float chirp_fstart_hz;
|
||||
extern float chirp_fstop_hz;
|
||||
extern float chirp_length_s;
|
||||
|
||||
// Index of chirp axis in ACTIVE_CHIRP_AXES
|
||||
extern uint8_t chirp_axis;
|
||||
|
||||
extern void sys_id_chirp_init(void);
|
||||
|
||||
// If chirp is running, update its values
|
||||
extern void sys_id_chirp_run(void);
|
||||
|
||||
// Handlers for changing gcs variables
|
||||
extern void sys_id_chirp_activate_handler(uint8_t activate); // Activate the chirp
|
||||
extern void sys_id_chirp_axis_handler(uint8_t axis); // Check if new axis is valid
|
||||
extern void sys_id_chirp_fstart_handler(float fstart); // Check if fstart is lower than current fend
|
||||
extern void sys_id_chirp_fstop_handler(float fstop); // Check if fend is higher than current fstart
|
||||
|
||||
// Add the current chirp values to the in_cmd values if motors_on is true
|
||||
extern void sys_id_chirp_add_values(bool motors_on, bool override_on, pprz_t in_cmd[]);
|
||||
|
||||
#endif // SYS_ID_CHIRP_H
|
||||
Reference in New Issue
Block a user