Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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,COMMAND_THRUST}
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 
71 
72 // The axes on which noise and chirp values can be applied
74 #define CHIRP_NB_AXES sizeof ACTIVE_CHIRP_AXES / sizeof ACTIVE_CHIRP_AXES[0] // Number of items in ACTIVE_CHIRP_AXES
75 
76 // Filters used to cut-off the gaussian noise fed into the actuator channels
78 
79 // Chirp and noise values for all axes (indices correspond to the axes given in CHIRP_AXES)
81 
82 static void set_current_chirp_values(void)
83 {
84  // initializing at zero the chirp input for every axis
85  for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
86  current_chirp_values[i] = 0;
87  }
88  // adding values if the chirp is active
89  if (chirp_active) {
90  // adding extra on the chirp signal (both on-axis and off axis)
91  #if CHIRP_USE_NOISE
92 
93  float amplitude, noise;
94  for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
97  current_chirp_values[i] += (int32_t)(noise * amplitude);
98  }
99 
100  #endif
101  // adding nominal chirp value
103  } else {
104  for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
105  current_chirp_values[i] = 0;
106  }
107  }
108 }
109 
110 static void send_chirp(struct transport_tx *trans, struct link_device *dev)
111 {
112  pprz_msg_send_CHIRP(trans, dev, AC_ID, &chirp_active, &chirp.percentage_done, &chirp.current_frequency_hz,
115 
116 }
117 
118 static void start_chirp(void)
119 {
121  chirp_active = TRUE;
123 }
124 
125 static void stop_chirp(void)
126 {
130 }
131 
133 {
134  chirp_active = activate;
135  if (chirp_active) {
137  chirp_fade_in);
138  start_chirp();
139  } else {
140  stop_chirp();
141  }
142 }
143 
145 {
146  return chirp_active;
147 }
148 
150 {
151  if (axis < CHIRP_NB_AXES) {
152  chirp_axis = axis;
153  }
154 }
155 
156 extern void sys_id_chirp_fstart_handler(float fstart)
157 {
158  if (fstart < chirp_fstop_hz) {
159  chirp_fstart_hz = fstart;
160  }
161 }
162 
163 extern void sys_id_chirp_fstop_handler(float fstop)
164 {
165  if (fstop > chirp_fstart_hz) {
166  chirp_fstop_hz = fstop;
167  }
168 }
169 
171 {
172  chirp_fade_in = fade_in;
173 }
174 
176 {
177  chirp_exponential = exponential;
178 }
179 
181 {
182 #if CHIRP_USE_NOISE
183 
184  init_random();
185 
186 #endif
187 
189  chirp_fade_in);
192 
193  // Filter cutoff frequency is the chirp maximum frequency
194  float tau = 1 / (chirp_fstop_hz * 2 * M_PI);
195  for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
196  init_first_order_low_pass(&filters[i], tau, SYS_ID_CHIRP_RUN_PERIOD, 0);
197  current_chirp_values[i] = 0;
198  }
199 }
200 
202 {
203 #if CHIRP_ENABLED
204 
205  if (chirp_active) {
207  stop_chirp();
208  } else {
211  }
212  }
213 
214 #endif
215 }
216 
217 void sys_id_chirp_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
218 {
219  (void)(override_on); // Suppress unused parameter warnings
220 
221 #if CHIRP_ENABLED
222 
223  if (motors_on) {
224  for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
225  in_cmd[ACTIVE_CHIRP_AXES[i]] += current_chirp_values[i];
226  BoundAbs(in_cmd[ACTIVE_CHIRP_AXES[i]], MAX_PPRZ);
227  }
228  }
229 
230 #endif
231 }
sys_id_chirp_fade_in_activate_handler
void sys_id_chirp_fade_in_activate_handler(uint8_t fade_in)
Definition: sys_id_chirp.c:170
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
sys_id_chirp_init
void sys_id_chirp_init(void)
Definition: sys_id_chirp.c:180
start_chirp
static void start_chirp(void)
Definition: sys_id_chirp.c:118
int8_t
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
sys_id_chirp_exponential_activate_handler
void sys_id_chirp_exponential_activate_handler(uint8_t exponential)
Definition: sys_id_chirp.c:175
chirp_t::percentage_done
float percentage_done
Definition: pprz_chirp.h:69
sys_id_chirp_running
uint8_t sys_id_chirp_running(void)
Definition: sys_id_chirp.c:144
chirp_is_running
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
sys_id_chirp_axis_handler
void sys_id_chirp_axis_handler(uint8_t axis)
Definition: sys_id_chirp.c:149
get_sys_time_float
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
rand_gaussian
double rand_gaussian(void)
Definition: pprz_random.c:53
init_first_order_low_pass
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
Definition: low_pass_filter.h:57
update_first_order_low_pass
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.
Definition: low_pass_filter.h:71
chirp_fade_in
uint8_t chirp_fade_in
Definition: sys_id_chirp.c:69
chirp_noise_stdv_onaxis_ratio
float chirp_noise_stdv_onaxis_ratio
Definition: sys_id_chirp.c:63
set_current_chirp_values
static void set_current_chirp_values(void)
Definition: sys_id_chirp.c:82
chirp_fstop_hz
float chirp_fstop_hz
Definition: sys_id_chirp.c:66
chirp_amplitude
pprz_t chirp_amplitude
Definition: sys_id_chirp.c:62
chirp_exponential
uint8_t chirp_exponential
Definition: sys_id_chirp.c:70
chirp_reset
void chirp_reset(struct chirp_t *chirp, float current_time_s)
Reset the time of the chirp.
Definition: pprz_chirp.c:56
chirp_fstart_hz
float chirp_fstart_hz
Definition: sys_id_chirp.c:65
stop_chirp
static void stop_chirp(void)
Definition: sys_id_chirp.c:125
pprz_chirp.h
sys_id_chirp_fstart_handler
void sys_id_chirp_fstart_handler(float fstart)
Definition: sys_id_chirp.c:156
telemetry.h
std.h
chirp_noise_stdv_offaxis
float chirp_noise_stdv_offaxis
Definition: sys_id_chirp.c:64
chirp_t::current_value
float current_value
Definition: pprz_chirp.h:67
pprz_t
int16_t pprz_t
Definition: paparazzi.h:6
filters
static struct FirstOrderLowPass filters[CHIRP_NB_AXES]
Definition: sys_id_chirp.c:77
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
sys_time.h
Architecture independent timing functions.
ACTIVE_CHIRP_AXES
static const int8_t ACTIVE_CHIRP_AXES[]
Definition: sys_id_chirp.c:73
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
current_chirp_values
static pprz_t current_chirp_values[CHIRP_NB_AXES]
Definition: sys_id_chirp.c:80
pprz_random.h
CHIRP_AXES
#define CHIRP_AXES
Definition: sys_id_chirp.c:39
sys_id_chirp_activate_handler
void sys_id_chirp_activate_handler(uint8_t activate)
Definition: sys_id_chirp.c:132
chirp_init
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
chirp
static struct chirp_t chirp
Definition: sys_id_chirp.c:59
send_chirp
static void send_chirp(struct transport_tx *trans, struct link_device *dev)
Definition: sys_id_chirp.c:110
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
chirp_t
Initialize with chirp_init.
Definition: pprz_chirp.h:59
sys_id_chirp.h
sys_id_chirp_run
void sys_id_chirp_run(void)
Definition: sys_id_chirp.c:201
init_random
void init_random(void)
Definition: pprz_random.c:35
chirp_length_s
float chirp_length_s
Definition: sys_id_chirp.c:67
chirp_t::current_frequency_hz
float current_frequency_hz
Definition: pprz_chirp.h:66
sys_id_chirp_fstop_handler
void sys_id_chirp_fstop_handler(float fstop)
Definition: sys_id_chirp.c:163
CHIRP_NB_AXES
#define CHIRP_NB_AXES
Definition: sys_id_chirp.c:74
chirp_update
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
FALSE
#define FALSE
Definition: std.h:5
TRUE
#define TRUE
Definition: std.h:4
FirstOrderLowPass
First order low pass filter structure.
Definition: low_pass_filter.h:39
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
chirp_axis
uint8_t chirp_axis
Definition: sys_id_chirp.c:61
sys_id_chirp_add_values
void sys_id_chirp_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
Definition: sys_id_chirp.c:217
chirp_active
uint8_t chirp_active
Definition: sys_id_chirp.c:60
low_pass_filter.h
Simple first order low pass filter with bilinear transform.