Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-05 10:41:00 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

addition to system identification modules (#2810)

Browse Source
This commit is contained in:
Rollervi97
2022-01-20 22:56:16 +01:00
committed by GitHub
parent 5eef680c03
commit 02a9757b79
13 changed files with 991 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
conf/modules/sys_id_chirp.xml
View File

@@ -2,7 +2,7 @@
<module name="sys_id_chirp" dir="system_identification">
<doc>
<description>Chirp maneuver for system itentification.
<description>Chirp maneuver for system identification.
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.
@@ -14,6 +14,8 @@
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.
Remeber to deactivate the control input when the system identification input is running using (can be checked using the sys_id_wave_running() function)
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
@@ -22,11 +24,34 @@
- 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.
Start the chirp by checking the Active box in the "Activate chirp". Stop the chirp by checking the Inactive box in the same input setting.
Add the message "CHIRP" to your telemetry to see chirp progress, and to your logger to automatically filter chirps in post-processing.
Add the following message structure to the messages.xml file on a free ID
<!--
<message name="CHIRP" id="00">
<field name="active" type="uint8" unit="bool"> Chirp is active</field>
<field name="percentage_done" type="float">Percentage done</field>
<field name="current_frequency" type="float">Current frequency</field>
<field name="axis" type="uint8">Current chirp axis</field>
<field name="amplitude" type="int16">Amplitude</field>
<field name="fstart" type="float">Start frequency</field>
<field name="fstop" type="float">Stop frequency</field>
<field name="noise_onaxis_ratio" type="float">Noise ratio onaxis</field>
<field name="noise_offaxis" type="float">Noise offaxis</field>
<field name="current_value" type="int16">Current chirp value</field>
<field name="fade_in" type="uint8" unit="bool">Fade in feature active</field>
<field name="exponential" type="uint8" unit="bool">Exponential chirp active</field>
</message>
-->
</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_AXES" value="{COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}" description="Which axes the chirp is applied to (specify as array with {})"/>
<!-- <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"/>
@@ -35,11 +60,10 @@
<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_settings name="Chirp input">
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Chirp" var="chirp_active" type="uint8_t" module="system_identification/sys_id_chirp" handler="activate_handler"/>
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Fade in" var="chirp_fade_in" type="uint8_t" module="system_identification/sys_id_chirp" handler="fade_in_activate_handler"/>
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Exponential" var="chirp_exponential" type="uint8_t" module="system_identification/sys_id_chirp" handler="exponential_activate_handler"/>
<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"/>

71
conf/modules/sys_id_doublet.xml Normal file
View File

@@ -0,0 +1,71 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="sys_id_doublet" dir="system_identification">
<doc>
<description>Doublet input for system identification.
The doublet is a multi step maneuver for system identification. Two modes are implemented, the 0,1,-1,0 maneuver (which correspond to
one period of a squared sine wave oscillation, and the "so-called" 3-2-1-1 maneuver.
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_doublet_add_values(autopilot_get_motors_on(),FALSE,values)"/&gt;
You can pick the axes to apply doublets to by setting the DOUBLET_AXES variable with the COMMAND_XXX variables where XXX are the actuators defined in
the &lt;commands&gt; section of your airframe.
Remeber to deactivate the control input when the system identification input is running using (can be checked using the sys_id_wave_running() function)
Then, the GCS exposes the settings for the doublet.
- 3-2-1-1 setting is to execute the 3-2-1-1 doublet maneuver when active. Otherwise, the normal 1-1 doublet maneuver will be executed.
- The Doublet axis settings is the index (0-based) of the axis to choose within the DOUBLET_AXES variable specified. In the default, this means i.e. 0 means roll doublet.
- Amplitude is the amplitude of the doublet
- Length_s is the length in seconds of the doublet. The lenght of each doublet step depends on the active mode and on the doublet lenght.
- Extra_waiting_time_s is the length in second of the waiting time with zero input after the doublet maneuver is over.
Start the doublet by checking the Active box in the "Activate doublet". Stop the doublet by checking the Inactive box in the same input setting.
Add the message "DOUBLET" to your telemetry to see doublet progress, and to your logger to obtain information about the provided input.
Add the following message structure to the messages.xml file on a free ID
<!--
<message name="DOUBLET" id="181">
<field name="active" type="uint8" unit="bool">Doublet is active</field>
<field name="axis" type="uint8">Current doublet axis</field>
<field name="amplitude" type="int16">Amplitude</field>
<field name="current_value" type="int16">Current doublet value</field>
<field name="mode_3211_active" type="uint8">Doublet 3211 mode active</field>
</message>
-->
</description>
<define name="DOUBLET_AXES" value="{COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW}" description="Which axes the doublet is applied to (specify as array with {})"/>
<define name="DOUBLET_ENABLED" value="TRUE|FALSE" description="If false, the doublet does not run and values are not added"/>
<define name="DOUBLET_MOD3211" value="TRUE|FALSE" description="If true, the 3-2-1-1 doublet will be executed instead of the normal doublet"/>
</doc>
<settings>
<dl_settings name="System identification">
<dl_settings name="Doublet input">
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Doublet" var="doublet_active" type="uint8_t" module="system_identification/sys_id_doublet" handler="activate_handler"/>
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="3-2-1-1 mode" var="doublet_mode_3211" type="uint8_t" module="system_identification/sys_id_doublet" handler="mod3211_handler"/>
<dl_setting min="0" max="9600" step="100" shortname="Amplitude" var="doublet_amplitude" type="int32_t" module="system_identification/sys_id_doublet"/>
<dl_setting min="0" max="5" step="1" shortname="Doublet axis" var="doublet_axis" type="uint8_t" module="system_identification/sys_id_doublet" handler="axis_handler"/>
<dl_setting min="0" max="100" step="0.5" shortname="Length_s" var="doublet_length_s" type="float" module="system_identification/sys_id_doublet"/>
<dl_setting min="0" max="100" step="0.5" shortname="Extra_waiting_s" var="doublet_extra_waiting_time_s" type="float" module="system_identification/sys_id_doublet"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="sys_id_doublet.h"/>
</header>
<init fun="sys_id_doublet_init()"/>
<periodic fun="sys_id_doublet_run()" freq="60" autorun="TRUE"/>
<makefile>
<file name="pprz_doublet.c"/>
<file name="sys_id_doublet.c"/>
</makefile>
</module>

69
conf/modules/sys_id_wave.xml Normal file
View File

@@ -0,0 +1,69 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="sys_id_wave" dir="system_identification">
<doc>
<description>Wave maneuver for system identification.
The wave provide a sine input to the specified command axis.
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_wave_add_values(autopilot_get_motors_on(),FALSE,values)"/&gt;
You can pick the axes to apply waves to by setting the WAVE_AXES variable with the COMMAND_XXX variables where XXX are the actuators defined in
the &lt;commands&gt; section of your airframe.
Remeber to deactivate the control input when the system identification input is running using (can be checked using the sys_id_wave_running() function)
Then, the GCS exposes the settings for the wave.
- The wave axis settings is the index (0-based) of the axis to choose within the WAVE_AXES variable specified. In the default, this means i.e. 0 means roll wave.
- Amplitude is the amplitude of the wave
- Freqyency_hz is the frequency in Hertz of the wave
- Lag_rad is the lag in radiants for the wave signal
Start the wave by checking the Active box in the "Activate wave". Stop the wave by checking the Inactive box in the same input setting.
Add the message "WAVE" to your telemetry to see wave progress, and to your logger to obtain information about the provided input.
Add the following message structure to the messages.xml file on a free ID
<!--
<message name="WAVE" id="00">
<field name="active" type="uint8" unit="bool">Wave is active</field>
<field name="axis" type="uint8">Current wave axis</field>
<field name="amplitude" type="int16">Wave amplitude</field>
<field name="lag_rad" type="float">Lag in radiants</field>
<field name="frequency_hz" type="float">Wave frequency</field>
<field name="current_value" type="int16">Current wave value</field>
</message>
-->
</description>
<define name="WAVE_AXES" value="{COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW}" description="Which axes the wave is applied to (specify as array with {})"/>
<define name="WAVE_ENABLED" value="TRUE|FALSE" description="If false, the wave does not run and values are not added"/>
</doc>
<settings>
<dl_settings name="System identification">
<dl_settings name="Wave input">
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Activate wave" var="wave_active" type="uint8_t" module="system_identification/sys_id_wave" handler="activate_handler">
</dl_setting>
<dl_setting min="0" max="9600" step="100" shortname="Amplitude" var="wave_amplitude" type="int32_t" module="system_identification/sys_id_wave"/>
<dl_setting min="0" max="5" step="1" shortname="Wave axis" var="wave_axis" type="uint8_t" module="system_identification/sys_id_wave" handler="axis_handler"/>
<dl_setting min="0" max="20" step="0.5" shortname="frenquency_hz" var="frequency_hz_" type="float" module="system_identification/sys_id_wave" handler="frequency_hz_set"/>
<dl_setting min="0" max="6.28" step="0.0174" shortname="lag_rad_" var="lag_rad_" type="float" module="system_identification/sys_id_wave" handler="lag_rad_set"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="sys_id_wave.h"/>
</header>
<init fun="sys_id_wave_init()"/>
<periodic fun="sys_id_wave_run()" freq="60" autorun="TRUE"/>
<makefile>
<file name="pprz_wave.c"/>
<file name="sys_id_wave.c"/>
</makefile>
</module>

99
sw/airborne/modules/system_identification/pprz_doublet.c Normal file
View File

@@ -0,0 +1,99 @@
/*
* Copyright (C) Alessandro Collicelli
*
* 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 Alessandro Colllicelli
* Mathematical implementation of doublet for system identification
*/
#include "pprz_doublet.h"
#include "std.h"
void doublet_init(struct doublet_t *doublet, float length_s, float extra_waiting_time_s, float current_time_s, bool mod3211)
{
doublet->t0 = current_time_s;
doublet->tf = length_s;
doublet->total_length_s = doublet->tf + extra_waiting_time_s;
doublet->mod3211 = mod3211;
doublet->current_value = 0;
doublet->current_time_s = current_time_s;
if (mod3211) {
doublet->t1 = length_s / 9;
doublet->t2 = doublet->t1 * 4;
doublet->t3 = doublet->t1 * 6;
doublet->t4 = doublet->t1 * 7;
doublet->t5 = doublet->t1 * 8;
} else{
doublet->t1 = length_s / 4;
doublet->t2 = doublet->t1 * 2;
doublet->t3 = doublet->t1 * 3;
doublet->t4 = doublet->t1 * 4;
doublet->t5 = doublet->tf;
}
}
void doublet_reset(struct doublet_t *doublet, float current_time_s){
doublet->t0 = current_time_s;
doublet->current_value = 0.0f;
doublet->current_time_s = current_time_s;
}
bool doublet_is_running(struct doublet_t *doublet, float current_time_s){
float t = current_time_s - doublet->t0;
return (t >= 0) && (t <= doublet->total_length_s);
}
float doublet_update(struct doublet_t *doublet, float current_time_s){
if(!doublet_is_running(doublet, current_time_s)){
doublet->current_value = 0.0f;
return 0;
}
float t = current_time_s - doublet->t0; // since the start of the doublet
if ((t>=0) & (t<=doublet->tf)){
if (doublet->mod3211){
if (((t >= doublet->t1) & (t <= doublet->t2)) | ((t >= doublet->t3) & (t <= doublet->t4))){
doublet->current_value = 1.0f;
}else if(((t >= doublet->t2) && (t <= doublet->t3)) | ((t >= doublet->t4) && (t <= doublet->t5))){
doublet->current_value = -1.0f;
}else{
doublet->current_value = 0.0f;
}
}
else{
if((t >= doublet->t1) & (t <= doublet->t2)){
doublet->current_value = 1.0f;
}else if((t >= doublet->t2) & (t <= doublet->t3)){
doublet->current_value = -1.0f;
}else{
doublet->current_value = 0.0f;
}
}
}else{
doublet->current_value = 0.0f;
}
return doublet->current_value;
}

80
sw/airborne/modules/system_identification/pprz_doublet.h Normal file
View File

@@ -0,0 +1,80 @@
/*
* Copyright (C) Alessandro Collicelli
*
* 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_doublet.c"
* @author Alessandro Colllicelli
* Mathematical implementation of doublet for system identification
*
* A doublet is a maneuver used during system identification procedures. The standard doublet input (1) is constituted by two consecutive step
* input. The two step inputs are equal in step amplitude and time lenght but opposite in sign. The standard 3-2-1-1 doublet input (2) is
* constituded by four step input, with decresing length in time, equal amplitude and alternating in sign.
*
* ______
* | |
* (1) _________| | ___________ Standard doublet input shape
* | |
* |______|
*
*
* __________________ ______
* | | | |
* (2) _________| | | | _________ 3-2-1-1 doublet input shape
* | | | |
* |____________| |______|
*
*/
#ifndef PPRZ_DOUBLET_H
#define PPRZ_DOUBLET_H
#include "std.h"
/**
* Initialize with doublet_init
* */
struct doublet_t{
float t0;
float t1;
float t2;
float t3;
float t4;
float t5;
float tf;
float total_length_s;
float current_value;
float current_time_s;
bool mod3211;
};
extern void doublet_init(struct doublet_t *doublet, float length_s,
float extra_waiting_time_s, float current_time_s, bool mod3211);
extern void doublet_reset(struct doublet_t *doublet, float current_time_s);
extern bool doublet_is_running(struct doublet_t *doublet, float current_time_s);
extern float doublet_update(struct doublet_t *doublet, float current_time_s);
#endif //PPRZ_DOUBLET_H

62
sw/airborne/modules/system_identification/pprz_wave.c Normal file
View File

@@ -0,0 +1,62 @@
/*
* Copyright (C) Alessandro Collicelli
*
* 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_wave.c"
* @author Alessandro Collicelli
* System excitation pure sine wave
*
*/
#include "std.h"
#include "math.h"
#include "pprz_wave.h"
void wave_init(struct wave_t *wave, float start_time_s, float current_time_s, float frequency_hz, float lag_rad)
{
wave->start_time_s = start_time_s;
wave->current_time_s = current_time_s;
wave->lag_rad = lag_rad;
wave->frequency_hz = frequency_hz;
wave->is_running = false;
wave->current_value = 0.0f;
}
void wave_reset(struct wave_t *wave, float current_time_s)
{
wave->start_time_s = current_time_s;
wave->current_time_s = current_time_s;
wave->current_value = 0.0f;
}
bool wave_is_running(struct wave_t *wave){
return wave->is_running;
}
float wave_update(struct wave_t *wave, float current_time_s){
float frequency_rad = wave->frequency_hz * 2 * M_PI;
float t = current_time_s - wave->start_time_s;
wave->current_value = sinf(t*frequency_rad + wave->lag_rad);
return wave->current_value;
}

50
sw/airborne/modules/system_identification/pprz_wave.h Normal file
View File

@@ -0,0 +1,50 @@
/*
* Copyright (C) Alessandro Collicelli
*
* 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_wave.c"
* @author Alessandro Collicelli
* System excitation pure sine wave
*
*/
#ifndef SYS_ID_WAVE_H
#define SYS_ID_WAVE_H
#include "std.h"
#include "math.h"
struct wave_t {
float start_time_s;
float current_time_s;
float lag_rad;
float frequency_hz;
float current_value;
bool is_running;
};
void wave_init(struct wave_t *wave, float start_time_s, float current_time_s, float frequency_hz, float lag_rad);
void wave_reset(struct wave_t *wave, float current_time_s);
bool wave_is_running(struct wave_t *wave);
float wave_update(struct wave_t *wave, float current_time_s);
#endif //THESIS_PPRZ_WAVE_H

83
sw/airborne/modules/system_identification/sys_id_chirp.c
View File

@@ -36,7 +36,7 @@
#ifndef CHIRP_AXES
#define CHIRP_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW}
#define CHIRP_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}
#endif
#ifndef CHIRP_ENABLED
@@ -47,17 +47,17 @@
#define CHIRP_USE_NOISE TRUE
#endif
#ifndef CHIRP_EXPONENTIAL
#define CHIRP_EXPONENTIAL TRUE
#endif
// #ifndef CHIRP_EXPONENTIAL
// #define CHIRP_EXPONENTIAL TRUE
// #endif
#ifndef CHIRP_FADEIN
#define CHIRP_FADEIN TRUE
#endif
// #ifndef CHIRP_FADEIN
// #define CHIRP_FADEIN TRUE
// #endif
static struct chirp_t chirp;
uint8_t chirp_active = false;
uint8_t chirp_active = FALSE;
uint8_t chirp_axis = 0;
pprz_t chirp_amplitude = 0;
float chirp_noise_stdv_onaxis_ratio = 0.1;
@@ -66,6 +66,9 @@ float chirp_fstart_hz = 1.0f;
float chirp_fstop_hz = 5.0f;
float chirp_length_s = 20;
uint8_t chirp_fade_in = false;
uint8_t chirp_exponential = false;
// 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
@@ -78,23 +81,29 @@ 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 {
// initializing at zero the chirp input for every axis
for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
current_chirp_values[i] = 0;
}
}
// adding values if the chirp is active
if (chirp_active) {
// adding extra on the chirp signal (both on-axis and off axis)
#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
// adding nominal chirp value
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;
}
}
}
@@ -102,20 +111,21 @@ 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);
&chirp_noise_stdv_offaxis, &current_chirp_values[chirp_axis], &chirp_fade_in, &chirp_exponential);
}
static void start_chirp(void)
{
chirp_reset(&chirp, get_sys_time_float());
chirp_active = true;
chirp_active = TRUE;
set_current_chirp_values();
}
static void stop_chirp(void)
{
chirp_reset(&chirp, get_sys_time_float());
chirp_active = false;
chirp_active = FALSE;
set_current_chirp_values();
}
@@ -123,14 +133,19 @@ 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);
chirp_init(&chirp, chirp_fstart_hz, chirp_fstop_hz, chirp_length_s, get_sys_time_float(), chirp_exponential,
chirp_fade_in);
start_chirp();
} else {
stop_chirp();
}
}
uint8_t sys_id_chirp_running(void)
{
return chirp_active;
}
extern void sys_id_chirp_axis_handler(uint8_t axis)
{
if (axis < CHIRP_NB_AXES) {
@@ -152,7 +167,15 @@ extern void sys_id_chirp_fstop_handler(float fstop)
}
}
extern void sys_id_chirp_fade_in_activate_handler(uint8_t fade_in)
{
chirp_fade_in = fade_in;
}
extern void sys_id_chirp_exponential_activate_handler(uint8_t exponential)
{
chirp_exponential = exponential;
}
void sys_id_chirp_init(void)
{
@@ -162,8 +185,8 @@ void sys_id_chirp_init(void)
#endif
chirp_init(&chirp, chirp_fstart_hz, chirp_fstop_hz, chirp_length_s, get_sys_time_float(), CHIRP_EXPONENTIAL,
CHIRP_FADEIN);
chirp_init(&chirp, chirp_fstart_hz, chirp_fstop_hz, chirp_length_s, get_sys_time_float(), chirp_exponential,
chirp_fade_in);
set_current_chirp_values();
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CHIRP, send_chirp);

7
sw/airborne/modules/system_identification/sys_id_chirp.h
View File

@@ -53,6 +53,10 @@ extern float chirp_length_s;
// Index of chirp axis in ACTIVE_CHIRP_AXES
extern uint8_t chirp_axis;
extern uint8_t chirp_fade_in;
extern uint8_t chirp_exponential;
extern void sys_id_chirp_init(void);
// If chirp is running, update its values
@@ -63,6 +67,9 @@ extern void sys_id_chirp_activate_handler(uint8_t activate); // Activate the chi
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
extern void sys_id_chirp_fade_in_activate_handler(uint8_t fade_in); // Fade in feature switch
extern void sys_id_chirp_exponential_activate_handler(uint8_t exponential); // Exponential chirp feature switch
extern uint8_t sys_id_chirp_running(void); // Check if the chirp is running or not
// 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[]);

169
sw/airborne/modules/system_identification/sys_id_doublet.c Normal file
View File

@@ -0,0 +1,169 @@
/*
* Copyright (C) Alessandro Collicelli
*
* 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_doublet.c"
* @author Alessandro Collicelli
* System identification doublet
*
* This is the module implementation for the doublet maneuver. 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_doublet_add_values(autopilot_get_motors_on(),FALSE,values)"/>
*
* In the GCS you can then start and stop the doublet, change type of doublet maneuvre.
* Documentation of the specific options can be found in the module xml file.
*
*/
#include "std.h"
#include "sys_id_doublet.h"
#include "pprz_doublet.h"
#include "modules/datalink/telemetry.h"
#include "generated/airframe.h"
#include "mcu_periph/sys_time.h"
#ifndef DOUBLET_AXES
#define DOUBLET_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}
#endif
#ifndef DOUBLET_ENABLED
#define DOUBLET_ENABLED TRUE
#endif
static struct doublet_t doublet;
uint8_t doublet_active = false;
uint8_t doublet_mode_3211 = false;
uint8_t doublet_axis = 0;
pprz_t doublet_amplitude = 4500;
float doublet_length_s = 20.0f;
float doublet_extra_waiting_time_s = 0.0f;
static const int8_t ACTIVE_DOUBLET_AXES[] = DOUBLET_AXES;
#define DOUBLET_NB_AXES sizeof ACTIVE_DOUBLET_AXES / sizeof ACTIVE_DOUBLET_AXES[0] // Number of items in ACTIVE_DOUBLET_AXES
static pprz_t current_doublet_values[DOUBLET_NB_AXES];
static void set_current_doublet_values(void)
{
if (doublet_active) {
current_doublet_values[doublet_axis] = (int32_t)(doublet_amplitude * doublet.current_value);
} else {
for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) {
current_doublet_values[i] = 0;
}
}
}
static void send_doublet(struct transport_tx *trans, struct link_device *dev){
pprz_msg_send_DOUBLET(trans, dev, AC_ID, &doublet_active,
&doublet_axis, &doublet_amplitude,
&current_doublet_values[doublet_axis], &doublet_mode_3211);
}
static void start_doublet(void)
{
doublet_reset(&doublet, get_sys_time_float());
doublet_active = true;
set_current_doublet_values();
}
static void stop_doublet(void)
{
doublet_reset(&doublet, get_sys_time_float());
doublet_active = false;
set_current_doublet_values();
}
uint8_t sys_id_doublet_running(void){
return doublet_active;
}
void sys_id_doublet_activate_handler(uint8_t activate)
{
doublet_active = activate;
if (doublet_active) {
doublet_init(&doublet, doublet_length_s, doublet_extra_waiting_time_s, get_sys_time_float(), doublet_mode_3211);
start_doublet();
} else {
stop_doublet();
}
}
void sys_id_doublet_axis_handler(uint8_t axis)
{
if (axis < DOUBLET_NB_AXES) {
doublet_axis = axis;
}
}
void sys_id_doublet_mod3211_handler(uint8_t mode){
doublet_mode_3211 = mode;
}
void sys_id_doublet_init(void)
{
doublet_init(&doublet, doublet_length_s, doublet_extra_waiting_time_s, get_sys_time_float(), doublet_mode_3211);
set_current_doublet_values();
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DOUBLET, send_doublet);
for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) {
current_doublet_values[i] = 0;
}
}
void sys_id_doublet_run(void)
{
if (doublet_active) {
if (!doublet_is_running(&doublet, get_sys_time_float())) {
stop_doublet();
} else {
doublet_update(&doublet, get_sys_time_float());
set_current_doublet_values();
}
}
}
void sys_id_doublet_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
{
(void)(override_on); // Suppress unused parameter warnings
#if DOUBLET_ENABLED
if (motors_on) {
for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) {
in_cmd[ACTIVE_DOUBLET_AXES[i]] += current_doublet_values[i];
BoundAbs(in_cmd[ACTIVE_DOUBLET_AXES[i]], MAX_PPRZ);
}
}
#endif
}

64
sw/airborne/modules/system_identification/sys_id_doublet.h Normal file
View File

@@ -0,0 +1,64 @@
/*
* Copyright (C) Alessandro Collicelli
*
* 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_doublet.c"
* @author Alessandro Collicelli
* System identification doublet
*
* This is the module implementation for the doublet maneuver. 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_doublet_add_values(autopilot_get_motors_on(),FALSE,values)"/>
*
* In the GCS you can then start and stop the doublet, change type of doublet maneuvre.
* Documentation of the specific options can be found in the module xml file.
*/
#ifndef SYS_ID_DOUBLET_H
#define SYS_ID_DOUBLET_H
#include "paparazzi.h"
extern uint8_t doublet_active;
extern pprz_t doublet_amplitude;
extern float doublet_length_s;
extern float doublet_extra_waiting_time_s;
// Index of doublet axis in ACTIVE_DOUBLET_AXES
extern uint8_t doublet_axis;
extern uint8_t doublet_mode_3211;
extern void sys_id_doublet_init(void);
// If doublet is running, update its values
extern void sys_id_doublet_run(void);
// Handlers for changing gcs variables
extern void sys_id_doublet_activate_handler(uint8_t activate); // Activate the doublet
extern void sys_id_doublet_axis_handler(uint8_t axis); // Check if new axis
extern void sys_id_doublet_mod3211_handler(uint8_t mode);
extern uint8_t sys_id_doublet_running(void);
// Add the current doublet values to the in_cmd values if motors_on is true
extern void sys_id_doublet_add_values(bool motors_on, bool override_on, pprz_t in_cmd[]);
#endif // SYS_ID_DOUBLET_H

173
sw/airborne/modules/system_identification/sys_id_wave.c Normal file
View File

@@ -0,0 +1,173 @@
/*
* Copyright (C) Alessandro Collicelli
*
* 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_wave.h"
* @author Alessandro Collicelli
* System excitation pure sine wave
*
* This is the module implementation for a sine wave input. Use sys_id_wave 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_wave_add_values(autopilot_get_motors_on(),FALSE,values)"/>
*
* In the GCS you can then start and stop the wave, change wave frequency.
* Documentation of the specific options can be found in the module xml file.
*/
#include "std.h"
#include "math.h"
#include "pprz_wave.h"
#include "sys_id_wave.h"
#include "modules/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 WAVE_AXES
#define WAVE_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}
#endif
#ifndef WAVE_ENABLED
#define WAVE_ENABLED TRUE
#endif
static struct wave_t wave_s;
uint8_t wave_active = false;
uint8_t wave_axis = 0;
pprz_t wave_amplitude = 0;
float frequency_hz_ = 0.0f;
float lag_rad_ = 0.0f;
static const int8_t ACTIVE_WAVE_AXES[] = WAVE_AXES;
#define WAVE_NB_AXES sizeof ACTIVE_WAVE_AXES / sizeof ACTIVE_WAVE_AXES[0] // Number of items in ACTIVE_WAVE_AXES
// Chirp and noise values for all axes (indices correspond to the axes given in WAVE_AXES)
static pprz_t current_wave_values[WAVE_NB_AXES];
static void set_current_wave_values(void){
if (wave_active){
current_wave_values[wave_axis] = (int16_t)(wave_amplitude * wave_s.current_value);
wave_s.is_running = true;
}
else{
for (uint8_t i = 0; i < WAVE_NB_AXES; i++) {
current_wave_values[i] = 0;
wave_s.is_running = false;
}
}
}
static void send_wave(struct transport_tx *trans, struct link_device *dev){
pprz_msg_send_WAVE(trans, dev, AC_ID, &wave_active, &wave_axis, &wave_amplitude, &wave_s.lag_rad, &wave_s.frequency_hz,
&current_wave_values[wave_axis]);
}
static void start_wave(void){
wave_reset(&wave_s, get_sys_time_float());
wave_active = true;
set_current_wave_values();
}
static void stop_wave(void){
wave_reset(&wave_s, get_sys_time_float());
wave_active = false;
set_current_wave_values();
}
void sys_id_wave_activate_handler(uint8_t activate){
wave_active = activate;
if (wave_active) {
wave_init(&wave_s, get_sys_time_float(), get_sys_time_float(), frequency_hz_, lag_rad_);
start_wave();
} else {
stop_wave();
}
}
void sys_id_wave_axis_handler(uint8_t axis)
{
if (axis < WAVE_NB_AXES) {
wave_axis = axis;
}
}
uint8_t sys_id_wave_running(void){
return wave_active;
}
void sys_id_wave_frequency_hz_set(float frequency_hz_set)
{
frequency_hz_ = frequency_hz_set;
wave_init(&wave_s, get_sys_time_float(), get_sys_time_float(), frequency_hz_, lag_rad_);
}
void sys_id_wave_lag_rad_set(float lag_rad_set)
{
lag_rad_ = lag_rad_set;
wave_init(&wave_s, get_sys_time_float(), get_sys_time_float(), frequency_hz_, lag_rad_);
}
void sys_id_wave_init(void)
{
wave_init(&wave_s, get_sys_time_float(), get_sys_time_float(), frequency_hz_, lag_rad_);
set_current_wave_values();
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WAVE, send_wave);
}
void sys_id_wave_run(void)
{
// #if WAVE_ENABLED
if (wave_active) {
if (!wave_is_running(&wave_s)) {
stop_wave();
} else {
wave_update(&wave_s, get_sys_time_float());
set_current_wave_values();
}
}
// #endif
}
void sys_id_wave_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
{
(void)(override_on); // Suppress unused parameter warnings
#if WAVE_ENABLED
if (motors_on) {
for (uint8_t i = 0; i < WAVE_NB_AXES; i++) {
in_cmd[ACTIVE_WAVE_AXES[i]] += current_wave_values[i];
BoundAbs(in_cmd[ACTIVE_WAVE_AXES[i]], MAX_PPRZ);
}
}
#endif
}

62
sw/airborne/modules/system_identification/sys_id_wave.h Normal file
View File

@@ -0,0 +1,62 @@
/*
* Copyright (C) Alessandro Collicelli
*
* 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_wave.c"
* @author Alessandro Collicelli
* System excitation pure sine wave
*
* This is the module implementation for the a sine wave input. Use sys_id_wave 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_wave_add_values(autopilot_get_motors_on(),FALSE,values)"/>
*
* In the GCS you can then start and stop the wave, change wave frequency.
* Documentation of the specific options can be found in the module xml file.
*/
#ifndef SYS_ID_WAVE_H
#define SYS_ID_WAVE_H
#include "paparazzi.h"
extern uint8_t wave_active;
extern uint8_t wave_axis;
extern pprz_t wave_amplitude;
extern float frequency_hz_;
extern float lag_rad_;
extern uint8_t wave_axis;
extern void sys_id_wave_init(void);
extern void sys_id_wave_run(void);
extern void sys_id_wave_axis_handler(uint8_t axis);
extern uint8_t sys_id_wave_running(void);
extern void sys_id_wave_frequency_hz_set(float frequency_hz_set);
extern void sys_id_wave_lag_rad_set(float lag_rad_set);
// handlers for changing in GCS variables
extern void sys_id_wave_activate_handler(uint8_t activate);
extern void sys_id_wave_add_values(bool motors_on, bool override_on, pprz_t in_cmd[]);
#endif // SYS_ID_WAVE_H