From f65b4ae84d009933ab47df1bfe37ea4b3bb352ca Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 21 Sep 2023 15:33:25 +0200 Subject: [PATCH] [modules] system identification update (#3075) * [modules] system identification updates: auto-doublet-sequence, new doublets, RC-triggered chirps, testing extra sysid rename generic test compile * defines in a section instead of gcc, cleanup docs --- conf/airframes/tudelft/nederdrone4.xml | 29 ++-- conf/modules/sys_id_auto_doublets.xml | 31 ++++ conf/modules/sys_id_chirp.xml | 19 ++- conf/modules/sys_id_doublet.xml | 37 ++--- conf/userconf/tudelft/conf.xml | 2 +- .../system_identification/pprz_doublet.c | 55 ++++--- .../system_identification/pprz_doublet.h | 4 +- .../sys_id_auto_doublets.c | 155 ++++++++++++++++++ .../sys_id_auto_doublets.h | 37 +++++ .../system_identification/sys_id_chirp.c | 89 ++++++---- .../system_identification/sys_id_doublet.c | 81 ++++++--- .../system_identification/sys_id_doublet.h | 4 +- sw/ext/pprzlink | 2 +- 13 files changed, 420 insertions(+), 125 deletions(-) create mode 100644 conf/modules/sys_id_auto_doublets.xml create mode 100644 sw/airborne/modules/system_identification/sys_id_auto_doublets.c create mode 100644 sw/airborne/modules/system_identification/sys_id_auto_doublets.h diff --git a/conf/airframes/tudelft/nederdrone4.xml b/conf/airframes/tudelft/nederdrone4.xml index a81f6d61c4..9f92cd994c 100644 --- a/conf/airframes/tudelft/nederdrone4.xml +++ b/conf/airframes/tudelft/nederdrone4.xml @@ -173,14 +173,10 @@ - - + + + + @@ -241,8 +237,8 @@ - - + + @@ -284,6 +280,19 @@ +
+ + + + + + + + + + +
+
diff --git a/conf/modules/sys_id_auto_doublets.xml b/conf/modules/sys_id_auto_doublets.xml new file mode 100644 index 0000000000..bdebf5dab7 --- /dev/null +++ b/conf/modules/sys_id_auto_doublets.xml @@ -0,0 +1,31 @@ + + + + Module that automatically runs a doublet sequence on a list of actuators +
+ + + +
+ + + + + + + + +
+ +
+ + + + + + + + + + + diff --git a/conf/modules/sys_id_chirp.xml b/conf/modules/sys_id_chirp.xml index 3974a4916f..91b5fa0ea0 100644 --- a/conf/modules/sys_id_chirp.xml +++ b/conf/modules/sys_id_chirp.xml @@ -29,21 +29,22 @@ Add the message "CHIRP" to your telemetry to see chirp progress, and to your logger to automatically filter chirps in post-processing. - - - - - - +
+ + + + + +
- - - + + + diff --git a/conf/modules/sys_id_doublet.xml b/conf/modules/sys_id_doublet.xml index d4b7b08726..5376563279 100644 --- a/conf/modules/sys_id_doublet.xml +++ b/conf/modules/sys_id_doublet.xml @@ -10,47 +10,34 @@ <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,values)"/> - 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 + You can pick the axes to apply doublets to by setting the SYS_ID_DOUBLET_AXES variable with the COMMAND_XXX variables where XXX are the actuators defined in the <commands> 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. + - The Doublet axis settings is the index (0-based) of the axis to choose within the SYS_ID_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 - - - + 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. - - - - +
+ + + +
+ - - - + + + diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 993c616082..dc93047def 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -282,7 +282,7 @@ telemetry="telemetry/highspeed_rotorcraft.xml" flight_plan="flight_plans/tudelft/nederdrone_cyberzoo.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml" + settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_chirp.xml modules/sys_id_doublet.xml modules/target_pos.xml" gui_color="blue" /> t0 = current_time_s; doublet->tf = length_s; doublet->total_length_s = doublet->tf + extra_waiting_time_s; - doublet->mod3211 = mod3211; + doublet->mod = mod; 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{ + if (mod==0) { // Normal doublet doublet->t1 = length_s / 4; doublet->t2 = doublet->t1 * 2; doublet->t3 = doublet->t1 * 3; doublet->t4 = doublet->t1 * 4; doublet->t5 = doublet->tf; - + } else if (mod==1) { // Half doublet + doublet->t1 = length_s / 4; + doublet->t2 = doublet->t1 * 2; + doublet->t3 = doublet->t1 * 3; + doublet->t4 = doublet->t1 * 4; + doublet->t5 = doublet->tf; + } else if (mod==2) { // 3211 doublet + 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; } - } void doublet_reset(struct doublet_t *doublet, float current_time_s){ @@ -74,7 +78,23 @@ float doublet_update(struct doublet_t *doublet, float current_time_s){ float t = current_time_s - doublet->t0; // since the start of the doublet if ((t>=0) & (t<=doublet->tf)){ - if (doublet->mod3211){ + if (doublet->mod == 0) { + 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 if (doublet->mod == 1) { + 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 if (doublet->mod == 2) { 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))){ @@ -83,16 +103,7 @@ float doublet_update(struct doublet_t *doublet, float current_time_s){ 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{ + } else { doublet->current_value = 0.0f; } return doublet->current_value; diff --git a/sw/airborne/modules/system_identification/pprz_doublet.h b/sw/airborne/modules/system_identification/pprz_doublet.h index 2f750740a2..945f9c6ed4 100644 --- a/sw/airborne/modules/system_identification/pprz_doublet.h +++ b/sw/airborne/modules/system_identification/pprz_doublet.h @@ -65,11 +65,11 @@ struct doublet_t{ float current_value; float current_time_s; - bool mod3211; + uint8_t mod; }; extern void doublet_init(struct doublet_t *doublet, float length_s, - float extra_waiting_time_s, float current_time_s, bool mod3211); + float extra_waiting_time_s, float current_time_s, uint8_t mod3211); extern void doublet_reset(struct doublet_t *doublet, float current_time_s); diff --git a/sw/airborne/modules/system_identification/sys_id_auto_doublets.c b/sw/airborne/modules/system_identification/sys_id_auto_doublets.c new file mode 100644 index 0000000000..5266cb0ff6 --- /dev/null +++ b/sw/airborne/modules/system_identification/sys_id_auto_doublets.c @@ -0,0 +1,155 @@ +/* + * Copyright (C) 2023 Dennis van Wijngaarden + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** @file "modules/system_identification/sys_id_auto_doublets.c" + * @author Dennis van Wijngaarden + * Module that automatically runs a doublet program for the rotating wing drone + */ +#include "std.h" + +#include "modules/system_identification/sys_id_auto_doublets.h" +#include "modules/system_identification/sys_id_doublet.h" +#include "modules/system_identification/pprz_doublet.h" + +#include "mcu_periph/sys_time.h" +#include "generated/airframe.h" + +// Perform checks if mandatory macros are defined + +#ifndef SYS_ID_AUTO_DOUBLETS_N_ACTUATORS +#error No doublet actuators SYS_ID_AUTO_DOUBLETS_N_ACTUATORS defined +#endif + +#ifndef SYS_ID_AUTO_DOUBLETS_ACTUATORS +#error No doublet actuators SYS_ID_AUTO_DOUBLETS_ACTUATORS defined +#endif + +#ifndef SYS_ID_AUTO_DOUBLETS_AMPLITUDE +#error No doublet actuators SYS_ID_AUTO_DOUBLETS_AMPLITUDE defined +#endif + +uint8_t sys_id_auto_doublets_actuators[SYS_ID_AUTO_DOUBLETS_N_ACTUATORS] = SYS_ID_AUTO_DOUBLETS_ACTUATORS; +int16_t sys_id_auto_doublets_amplitude[SYS_ID_AUTO_DOUBLETS_N_ACTUATORS] = SYS_ID_AUTO_DOUBLETS_AMPLITUDE; + +float sys_id_auto_doublets_time = 0.5; // time of one doublet +float sys_id_auto_doublets_interval_time = 5.; // time interval for doublets +int8_t sys_id_auto_doublets_n_repeat = 5; // The number of times a doublet has to be repeated on a single actuator + +bool sys_id_auto_doublets_activated = false; + +// Other variables needed for the periodic loop +uint8_t sys_id_auto_doublets_n_doublets; + +float sys_id_auto_doublets_start_time_s; +float sys_id_auto_doublets_timer; +uint8_t sys_id_auto_doublets_counter; + +inline void perform_sys_id_auto_doublets(uint8_t actuator_index); +inline void sys_id_auto_doublets_on_deactivation(void); + +void init_sys_id_auto_doublets(void) +{ + sys_id_auto_doublets_n_doublets = SYS_ID_AUTO_DOUBLETS_N_ACTUATORS * sys_id_auto_doublets_n_repeat; + sys_id_auto_doublets_start_time_s = 0.; + sys_id_auto_doublets_timer = 0.; + sys_id_auto_doublets_counter = 0; +} + +void periodic_sys_id_auto_doublets(void) +{ + // your periodic code here. + // freq = 20.0 Hz + + // Run if doublet activated + if (sys_id_auto_doublets_activated) + { + float current_time_s = get_sys_time_float(); + sys_id_auto_doublets_timer = current_time_s - sys_id_auto_doublets_start_time_s; + + // Check when to increase the counters and perform doublet + uint8_t current_index = sys_id_auto_doublets_timer / sys_id_auto_doublets_interval_time; + + // stop doublets when all doublets done + if (current_index >= sys_id_auto_doublets_n_doublets) + { + sys_id_auto_doublets_on_deactivation(); + } + else + { // continue doublets when not finished yet + // Check if doublet should be performed and index inxreased + if (current_index > sys_id_auto_doublets_counter) + { + uint8_t actuator_index = current_index / sys_id_auto_doublets_n_repeat; + perform_sys_id_auto_doublets(actuator_index); + sys_id_auto_doublets_counter++; + } + } + } +} + +void perform_sys_id_auto_doublets(uint8_t actuator_index) +{ + // Do nothing id doublet index is invalid + if (actuator_index >= SYS_ID_AUTO_DOUBLETS_N_ACTUATORS) + { + return; + } + // Set correct settings + sys_id_doublet_axis_handler(sys_id_auto_doublets_actuators[actuator_index]); + doublet_length_s = sys_id_auto_doublets_time; + doublet_amplitude = sys_id_auto_doublets_amplitude[actuator_index]; + + // call doublet activation handler + sys_id_doublet_activate_handler(1); +} + +void sys_id_auto_doublets_on_activation(uint8_t active) +{ + sys_id_auto_doublets_activated = active; + // If the activation boolean is set to true + if (sys_id_auto_doublets_activated) + { + if (sys_id_auto_doublets_timer > 0) + { + // Do nothing if already activated + } + else + { + // Activate auto doublet if not activated yet + sys_id_auto_doublets_start_time_s = get_sys_time_float(); + sys_id_auto_doublets_timer = 0; + perform_sys_id_auto_doublets(0); + } + } + else + { // If the activation boolean is set to false + sys_id_auto_doublets_on_deactivation(); + } +} + +void sys_id_auto_doublets_on_deactivation(void) +{ + // call doublet deactivation handler + sys_id_doublet_activate_handler(0); + sys_id_auto_doublets_activated = false; + sys_id_auto_doublets_timer = 0; + sys_id_auto_doublets_counter = 0; + doublet_amplitude = 0; +} \ No newline at end of file diff --git a/sw/airborne/modules/system_identification/sys_id_auto_doublets.h b/sw/airborne/modules/system_identification/sys_id_auto_doublets.h new file mode 100644 index 0000000000..cb672ead68 --- /dev/null +++ b/sw/airborne/modules/system_identification/sys_id_auto_doublets.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2023 Dennis van Wijngaarden + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** @file "modules/system_identification/sys_id_auto_doublets.h" + * @author Dennis van Wijngaarden + * Module that automatically runs a doublet program for the rotating wing drone + */ + +#ifndef SYS_ID_AUTO_DOUBLETS_H +#define SYS_ID_AUTO_DOUBLETS_H + +#include "std.h" + +extern bool sys_id_auto_doublets_activated; + +extern void init_sys_id_auto_doublets(void); +extern void periodic_sys_id_auto_doublets(void); +extern void sys_id_auto_doublets_on_activation(uint8_t active); + +#endif // SYS_ID_AUTO_DOUBLETS_H diff --git a/sw/airborne/modules/system_identification/sys_id_chirp.c b/sw/airborne/modules/system_identification/sys_id_chirp.c index 153f17bbe1..816585b99b 100644 --- a/sw/airborne/modules/system_identification/sys_id_chirp.c +++ b/sw/airborne/modules/system_identification/sys_id_chirp.c @@ -35,24 +35,29 @@ #include "math/pprz_random.h" -#ifndef CHIRP_AXES -#define CHIRP_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST} +#ifndef SYS_ID_CHIRP_AXES +#define SYS_ID_CHIRP_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST} #endif -#ifndef CHIRP_ENABLED -#define CHIRP_ENABLED TRUE +#ifndef SYS_ID_CHIRP_ENABLED +#define SYS_ID_CHIRP_ENABLED TRUE #endif -#ifndef CHIRP_USE_NOISE -#define CHIRP_USE_NOISE TRUE +#ifndef SYS_ID_CHIRP_USE_NOISE +#define SYS_ID_CHIRP_USE_NOISE TRUE #endif -// #ifndef CHIRP_EXPONENTIAL -// #define CHIRP_EXPONENTIAL TRUE +#ifdef SYS_ID_CHIRP_RADIO_CHANNEL +#include "modules/radio_control/radio_control.h" +pprz_t previous_radio_value_chirp = 0; +#endif + +// #ifndef SYS_ID_CHIRP_EXPONENTIAL +// #define SYS_ID_CHIRP_EXPONENTIAL TRUE // #endif -// #ifndef CHIRP_FADEIN -// #define CHIRP_FADEIN TRUE +// #ifndef SYS_ID_CHIRP_FADEIN +// #define SYS_ID_CHIRP_FADEIN TRUE // #endif @@ -70,28 +75,28 @@ 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 +static const int8_t SYS_ID_ACTIVE_CHIRP_AXES[] = SYS_ID_CHIRP_AXES; +#define SYS_ID_CHIRP_NB_AXES sizeof SYS_ID_ACTIVE_CHIRP_AXES / sizeof SYS_ID_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]; +static struct FirstOrderLowPass filters[SYS_ID_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 pprz_t current_chirp_values[SYS_ID_CHIRP_NB_AXES]; static void set_current_chirp_values(void) { // initializing at zero the chirp input for every axis - for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) { + for (uint8_t i = 0; i < SYS_ID_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 + #if SYS_ID_CHIRP_USE_NOISE float amplitude, noise; - for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) { + for (uint8_t i = 0; i < SYS_ID_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); @@ -101,7 +106,7 @@ static void set_current_chirp_values(void) // 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++) { + for (uint8_t i = 0; i < SYS_ID_CHIRP_NB_AXES; i++) { current_chirp_values[i] = 0; } } @@ -132,6 +137,13 @@ static void stop_chirp(void) void sys_id_chirp_activate_handler(uint8_t activate) { chirp_active = activate; + #ifdef SYS_ID_CHIRP_RADIO_CHANNEL + // Don't activate chirp when radio signal is low + if (radio_control.values[SYS_ID_CHIRP_RADIO_CHANNEL] < 1750) + { + chirp_active = 0; + } + #endif if (chirp_active) { chirp_init(&chirp, chirp_fstart_hz, chirp_fstop_hz, chirp_length_s, get_sys_time_float(), chirp_exponential, chirp_fade_in); @@ -148,7 +160,7 @@ uint8_t sys_id_chirp_running(void) extern void sys_id_chirp_axis_handler(uint8_t axis) { - if (axis < CHIRP_NB_AXES) { + if (axis < SYS_ID_CHIRP_NB_AXES) { chirp_axis = axis; } } @@ -179,7 +191,7 @@ extern void sys_id_chirp_exponential_activate_handler(uint8_t exponential) void sys_id_chirp_init(void) { -#if CHIRP_USE_NOISE +#if SYS_ID_CHIRP_USE_NOISE init_random(); @@ -192,7 +204,7 @@ void sys_id_chirp_init(void) // 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++) { + for (uint8_t i = 0; i < SYS_ID_CHIRP_NB_AXES; i++) { init_first_order_low_pass(&filters[i], tau, SYS_ID_CHIRP_RUN_PERIOD, 0); current_chirp_values[i] = 0; } @@ -200,7 +212,29 @@ void sys_id_chirp_init(void) void sys_id_chirp_run(void) { -#if CHIRP_ENABLED +#if SYS_ID_CHIRP_ENABLED + + #ifdef SYS_ID_CHIRP_RADIO_CHANNEL + // Check if chirp switched on when off before + if (previous_radio_value_chirp < 1750) + { + if (radio_control.values[SYS_ID_CHIRP_RADIO_CHANNEL] > 1750) + { + // Activate chirp + sys_id_chirp_activate_handler(1); + } + } + // Check if chirp switched off when on before + if (previous_radio_value_chirp > 1750) + { + if (radio_control.values[SYS_ID_CHIRP_RADIO_CHANNEL] < 1750) + { + // Deactivate chirp + sys_id_chirp_activate_handler(0); + } + } + previous_radio_value_chirp = radio_control.values[SYS_ID_CHIRP_RADIO_CHANNEL]; + #endif if (chirp_active) { if (!chirp_is_running(&chirp, get_sys_time_float())) { @@ -214,16 +248,15 @@ void sys_id_chirp_run(void) #endif } -void sys_id_chirp_add_values(bool motors_on, bool override_on, pprz_t in_cmd[]) +void sys_id_chirp_add_values(bool UNUSED motors_on, bool UNUSED override_on, pprz_t UNUSED in_cmd[]) { - (void)(override_on); // Suppress unused parameter warnings -#if CHIRP_ENABLED +#if SYS_ID_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); + for (uint8_t i = 0; i < SYS_ID_CHIRP_NB_AXES; i++) { + in_cmd[SYS_ID_ACTIVE_CHIRP_AXES[i]] += current_chirp_values[i]; + BoundAbs(in_cmd[SYS_ID_ACTIVE_CHIRP_AXES[i]], MAX_PPRZ); } } diff --git a/sw/airborne/modules/system_identification/sys_id_doublet.c b/sw/airborne/modules/system_identification/sys_id_doublet.c index 174dd78726..246fa33a25 100644 --- a/sw/airborne/modules/system_identification/sys_id_doublet.c +++ b/sw/airborne/modules/system_identification/sys_id_doublet.c @@ -40,33 +40,37 @@ #include "generated/airframe.h" #include "mcu_periph/sys_time.h" -#ifndef DOUBLET_AXES -#define DOUBLET_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST} +#ifndef SYS_ID_DOUBLET_AXES +#define SYS_ID_DOUBLET_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST} #endif -#ifndef DOUBLET_ENABLED -#define DOUBLET_ENABLED TRUE +#ifndef SYS_ID_DOUBLET_ENABLED +#define SYS_ID_DOUBLET_ENABLED TRUE #endif +#ifdef SYS_ID_DOUBLET_RADIO_CHANNEL +#include "modules/radio_control/radio_control.h" +pprz_t previous_radio_value_doublet = 0; +#endif static struct doublet_t doublet; uint8_t doublet_active = false; -uint8_t doublet_mode_3211 = false; +uint8_t doublet_mode = 0; uint8_t doublet_axis = 0; -pprz_t doublet_amplitude = 4500; -float doublet_length_s = 20.0f; +pprz_t doublet_amplitude = 0; +float doublet_length_s = 0.5f; 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 const int8_t SYS_ID_ACTIVE_DOUBLET_AXES[] = SYS_ID_DOUBLET_AXES; +#define SYS_ID_DOUBLET_NB_AXES sizeof SYS_ID_ACTIVE_DOUBLET_AXES / sizeof SYS_ID_ACTIVE_DOUBLET_AXES[0] // Number of items in ACTIVE_DOUBLET_AXES -static pprz_t current_doublet_values[DOUBLET_NB_AXES]; +static pprz_t current_doublet_values[SYS_ID_DOUBLET_NB_AXES]; static void set_current_doublet_values(void) { @@ -74,7 +78,7 @@ static void set_current_doublet_values(void) current_doublet_values[doublet_axis] = (int32_t)(doublet_amplitude * doublet.current_value); } else { - for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) { + for (uint8_t i = 0; i < SYS_ID_DOUBLET_NB_AXES; i++) { current_doublet_values[i] = 0; } } @@ -83,7 +87,7 @@ static void set_current_doublet_values(void) 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, - ¤t_doublet_values[doublet_axis], &doublet_mode_3211); + ¤t_doublet_values[doublet_axis], &doublet_mode); } static void start_doublet(void) @@ -107,8 +111,15 @@ uint8_t sys_id_doublet_running(void){ void sys_id_doublet_activate_handler(uint8_t activate) { doublet_active = activate; + #ifdef DOUBLET_RADIO_CHANNEL + // Don't activate doublet when radio signal is low + if (radio_control.values[SYS_ID_DOUBLET_RADIO_CHANNEL] < 1750) + { + doublet_active = 0; + } + #endif if (doublet_active) { - doublet_init(&doublet, doublet_length_s, doublet_extra_waiting_time_s, get_sys_time_float(), doublet_mode_3211); + doublet_init(&doublet, doublet_length_s, doublet_extra_waiting_time_s, get_sys_time_float(), doublet_mode); start_doublet(); } else { stop_doublet(); @@ -117,29 +128,50 @@ void sys_id_doublet_activate_handler(uint8_t activate) void sys_id_doublet_axis_handler(uint8_t axis) { - if (axis < DOUBLET_NB_AXES) { + if (axis < SYS_ID_DOUBLET_NB_AXES) { doublet_axis = axis; } } -void sys_id_doublet_mod3211_handler(uint8_t mode){ - doublet_mode_3211 = mode; +void sys_id_doublet_mod_handler(uint8_t mode){ + doublet_mode = 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); + doublet_init(&doublet, doublet_length_s, doublet_extra_waiting_time_s, get_sys_time_float(), doublet_mode); set_current_doublet_values(); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DOUBLET, send_doublet); - for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) { + for (uint8_t i = 0; i < SYS_ID_DOUBLET_NB_AXES; i++) { current_doublet_values[i] = 0; } } void sys_id_doublet_run(void) -{ +{ + #ifdef SYS_ID_DOUBLET_RADIO_CHANNEL + // Check if doublet switched on when off before + if (previous_radio_value_doublet < 1750) + { + if (radio_control.values[SYS_ID_DOUBLET_RADIO_CHANNEL] > 1750) + { + // Activate doublet + sys_id_doublet_activate_handler(1); + } + } + // Check if doublet switched off when on before + if (previous_radio_value_doublet > 1750) + { + if (radio_control.values[SYS_ID_DOUBLET_RADIO_CHANNEL] < 1750) + { + // Deactivate doublet + sys_id_doublet_activate_handler(0); + } + } + previous_radio_value_doublet = radio_control.values[SYS_ID_DOUBLET_RADIO_CHANNEL]; + #endif if (doublet_active) { if (!doublet_is_running(&doublet, get_sys_time_float())) { stop_doublet(); @@ -152,16 +184,15 @@ void sys_id_doublet_run(void) } -void sys_id_doublet_add_values(bool motors_on, bool override_on, pprz_t in_cmd[]) +void sys_id_doublet_add_values(bool UNUSED motors_on, bool UNUSED override_on, pprz_t UNUSED in_cmd[]) { - (void)(override_on); // Suppress unused parameter warnings -#if DOUBLET_ENABLED +#if SYS_ID_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); + for (uint8_t i = 0; i < SYS_ID_DOUBLET_NB_AXES; i++) { + in_cmd[SYS_ID_ACTIVE_DOUBLET_AXES[i]] += current_doublet_values[i]; + BoundAbs(in_cmd[SYS_ID_ACTIVE_DOUBLET_AXES[i]], MAX_PPRZ); } } diff --git a/sw/airborne/modules/system_identification/sys_id_doublet.h b/sw/airborne/modules/system_identification/sys_id_doublet.h index a097b5856b..1ae2e9cdfe 100644 --- a/sw/airborne/modules/system_identification/sys_id_doublet.h +++ b/sw/airborne/modules/system_identification/sys_id_doublet.h @@ -46,7 +46,7 @@ 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 uint8_t doublet_mode; extern void sys_id_doublet_init(void); @@ -56,7 +56,7 @@ 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 void sys_id_doublet_mod_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[]); diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 9208dc8417..5b3970d34e 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 9208dc8417a21f150edb9e6c10d5cf5d363c8b85 +Subproject commit 5b3970d34e39cff2befb46838e7ba8fe0d3ac43f