Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
sys_id_chirp.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Joost Meulenbeld
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
26 #include "std.h"
27 
28 #include "sys_id_chirp.h"
29 #include "pprz_chirp.h"
30 
32 #include "generated/airframe.h"
33 #include "mcu_periph/sys_time.h"
35 #include "math/pprz_random.h"
36 
37 
38 #ifndef CHIRP_AXES
39 #define CHIRP_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW}
40 #endif
41 
42 #ifndef CHIRP_ENABLED
43 #define CHIRP_ENABLED TRUE
44 #endif
45 
46 #ifndef CHIRP_USE_NOISE
47 #define CHIRP_USE_NOISE TRUE
48 #endif
49 
50 #ifndef CHIRP_EXPONENTIAL
51 #define CHIRP_EXPONENTIAL TRUE
52 #endif
53 
54 #ifndef CHIRP_FADEIN
55 #define CHIRP_FADEIN TRUE
56 #endif
57 
58 
59 static struct chirp_t chirp;
65 float chirp_fstart_hz = 1.0f;
66 float chirp_fstop_hz = 5.0f;
67 float chirp_length_s = 20;
68 
69 // The axes on which noise and chirp values can be applied
71 #define CHIRP_NB_AXES sizeof ACTIVE_CHIRP_AXES / sizeof ACTIVE_CHIRP_AXES[0] // Number of items in ACTIVE_CHIRP_AXES
72 
73 // Filters used to cut-off the gaussian noise fed into the actuator channels
75 
76 // Chirp and noise values for all axes (indices correspond to the axes given in CHIRP_AXES)
78 
79 static void set_current_chirp_values(void)
80 {
81  if (chirp_active) {
82 #if CHIRP_USE_NOISE
83 
84  float amplitude, noise;
85  for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
88  current_chirp_values[i] = (int32_t)(noise * amplitude);
89  }
90 
91 #endif
92 
94  } else {
95  for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
96  current_chirp_values[i] = 0;
97  }
98  }
99 }
100 
101 static void send_chirp(struct transport_tx *trans, struct link_device *dev)
102 {
103  pprz_msg_send_CHIRP(trans, dev, AC_ID, &chirp_active, &chirp.percentage_done, &chirp.current_frequency_hz,
106 }
107 
108 static void start_chirp(void)
109 {
111  chirp_active = true;
113 }
114 
115 static void stop_chirp(void)
116 {
118  chirp_active = false;
120 }
121 
123 {
124  chirp_active = activate;
125  if (chirp_active) {
127  CHIRP_FADEIN);
128  start_chirp();
129  } else {
130  stop_chirp();
131  }
132 }
133 
135 {
136  if (axis < CHIRP_NB_AXES) {
137  chirp_axis = axis;
138  }
139 }
140 
141 extern void sys_id_chirp_fstart_handler(float fstart)
142 {
143  if (fstart < chirp_fstop_hz) {
144  chirp_fstart_hz = fstart;
145  }
146 }
147 
148 extern void sys_id_chirp_fstop_handler(float fstop)
149 {
150  if (fstop > chirp_fstart_hz) {
151  chirp_fstop_hz = fstop;
152  }
153 }
154 
155 
156 
158 {
159 #if CHIRP_USE_NOISE
160 
161  init_random();
162 
163 #endif
164 
166  CHIRP_FADEIN);
169 
170  // Filter cutoff frequency is the chirp maximum frequency
171  float tau = 1 / (chirp_fstop_hz * 2 * M_PI);
172  for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
173  init_first_order_low_pass(&filters[i], tau, SYS_ID_CHIRP_RUN_PERIOD, 0);
174  current_chirp_values[i] = 0;
175  }
176 }
177 
179 {
180 #if CHIRP_ENABLED
181 
182  if (chirp_active) {
184  stop_chirp();
185  } else {
188  }
189  }
190 
191 #endif
192 }
193 
194 void sys_id_chirp_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
195 {
196  (void)(override_on); // Suppress unused parameter warnings
197 
198 #if CHIRP_ENABLED
199 
200  if (motors_on) {
201  for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
202  in_cmd[ACTIVE_CHIRP_AXES[i]] += current_chirp_values[i];
203  BoundAbs(in_cmd[ACTIVE_CHIRP_AXES[i]], MAX_PPRZ);
204  }
205  }
206 
207 #endif
208 }
static struct chirp_t chirp
Definition: sys_id_chirp.c:59
Periodic telemetry system header (includes downlink utility and generated code).
static void send_chirp(struct transport_tx *trans, struct link_device *dev)
Definition: sys_id_chirp.c:101
Simple first order low pass filter with bilinear transform.
void chirp_reset(struct chirp_t *chirp, float current_time_s)
Reset the time of the chirp.
Definition: pprz_chirp.c:56
Initialize with chirp_init.
Definition: pprz_chirp.h:59
int16_t pprz_t
Definition: paparazzi.h:6
static void stop_chirp(void)
Definition: sys_id_chirp.c:115
uint8_t chirp_active
Definition: sys_id_chirp.c:60
float current_frequency_hz
Definition: pprz_chirp.h:66
float chirp_noise_stdv_offaxis
Definition: sys_id_chirp.c:64
float chirp_noise_stdv_onaxis_ratio
Definition: sys_id_chirp.c:63
void init_random(void)
Definition: pprz_random.c:35
void sys_id_chirp_fstart_handler(float fstart)
Definition: sys_id_chirp.c:141
float chirp_update(struct chirp_t *chirp, float current_time_s)
Calculate the value at current_time_s and update the struct with current frequency and value...
Definition: pprz_chirp.c:71
First order low pass filter structure.
void chirp_init(struct chirp_t *chirp, float f0_hz, float f1_hz, float length_s, float current_time_s, bool exponential_chirp, bool fade_in)
Allocate and initialize a new chirp struct.
Definition: pprz_chirp.c:34
float current_value
Definition: pprz_chirp.h:67
#define CHIRP_NB_AXES
Definition: sys_id_chirp.c:71
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
#define CHIRP_AXES
Definition: sys_id_chirp.c:39
#define CHIRP_EXPONENTIAL
Definition: sys_id_chirp.c:51
static pprz_t current_chirp_values[CHIRP_NB_AXES]
Definition: sys_id_chirp.c:77
void sys_id_chirp_fstop_handler(float fstop)
Definition: sys_id_chirp.c:148
double rand_gaussian(void)
Definition: pprz_random.c:53
Architecture independent timing functions.
static void set_current_chirp_values(void)
Definition: sys_id_chirp.c:79
void sys_id_chirp_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
Definition: sys_id_chirp.c:194
uint8_t chirp_axis
Definition: sys_id_chirp.c:61
bool chirp_is_running(struct chirp_t *chirp, float current_time_s)
Return if the current_time is within the chirp manoeuvre.
Definition: pprz_chirp.c:65
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
static void start_chirp(void)
Definition: sys_id_chirp.c:108
pprz_t chirp_amplitude
Definition: sys_id_chirp.c:62
signed long int32_t
Definition: types.h:19
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
float percentage_done
Definition: pprz_chirp.h:69
unsigned char uint8_t
Definition: types.h:14
void sys_id_chirp_activate_handler(uint8_t activate)
Definition: sys_id_chirp.c:122
void sys_id_chirp_run(void)
Definition: sys_id_chirp.c:178
static struct FirstOrderLowPass filters[CHIRP_NB_AXES]
Definition: sys_id_chirp.c:74
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
float chirp_fstop_hz
Definition: sys_id_chirp.c:66
float chirp_fstart_hz
Definition: sys_id_chirp.c:65
#define CHIRP_FADEIN
Definition: sys_id_chirp.c:55
void sys_id_chirp_init(void)
Definition: sys_id_chirp.c:157
#define MAX_PPRZ
Definition: paparazzi.h:8
signed char int8_t
Definition: types.h:15
static const int8_t ACTIVE_CHIRP_AXES[]
Definition: sys_id_chirp.c:70
float chirp_length_s
Definition: sys_id_chirp.c:67
void sys_id_chirp_axis_handler(uint8_t axis)
Definition: sys_id_chirp.c:134
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.