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