Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
pprz_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  */
25 #include "pprz_chirp.h"
26 #include "std.h"
27 
28 // Values for exponential chirp (See ref [2] in the header file). C2 is based on C1 s.t. the frequency range exactly covers the required range
29 #define CHIRP_C1 4.0f
30 #define CHIRP_C2 1.0f / (expf(CHIRP_C1) - 1)
31 
32 
33 
34 void chirp_init(struct chirp_t *chirp, float f0_hz, float f1_hz, float length_s, float current_time_s,
35  bool exponential_chirp, bool fade_in)
36 {
37  chirp->f0_hz = f0_hz;
38  chirp->f1_hz = f1_hz;
39 
40  chirp->length_s = length_s;
41  if (fade_in) { // The fade-in takes two of the longest wave-lengths, total_length is including that time
42  chirp->total_length_s = length_s + 2 / f0_hz;
43  } else {
44  chirp->total_length_s = length_s;
45  }
46 
47  chirp->start_time_s = current_time_s;
48  chirp->exponential_chirp = exponential_chirp;
49  chirp->fade_in = fade_in;
50 
52  chirp->current_value = 0;
54 }
55 
56 void chirp_reset(struct chirp_t *chirp, float current_time_s)
57 {
58  chirp->current_time_s = current_time_s;
59  chirp->start_time_s = current_time_s;
61  chirp->current_value = 0;
63 }
64 
65 bool chirp_is_running(struct chirp_t *chirp, float current_time_s)
66 {
67  float t = current_time_s - chirp->start_time_s;
68  return (t >= 0) && (t <= chirp->total_length_s);
69 }
70 
71 float chirp_update(struct chirp_t *chirp, float current_time_s)
72 {
73  if (!chirp_is_running(chirp, current_time_s)) { // Outside the chirp interval, return 0
74  chirp->current_value = 0.0f;
75  return 0;
76  }
77 
78  float t = current_time_s - chirp->start_time_s; // Time since the start of the chirp
79  chirp->current_time_s = current_time_s;
80  // Protect against divide by zero
81  if (chirp->total_length_s <= 0) {
82  chirp->total_length_s = 0.01;
83  }
85 
86  if (chirp->fade_in && t < 2 / chirp->f0_hz) { // Fade-in is two times the wavelength of f0
88 
89  // First wavelength increases linearly in amplitude, second wavelength has unity amplitude
90  chirp->current_value = sinf(t * 2 * M_PI * chirp->f0_hz) * Min(1, t * chirp->f0_hz);
91  return chirp->current_value;
92  }
93 
94  // If the fade-in is finished, the current time t is the time since the fade-in stopped
95  if (chirp->fade_in) {
96  t -= 2 / chirp->f0_hz;
97  }
98 
99  if (chirp->exponential_chirp) { // See the book referenced in the header for the equations
100  float exponential = expf(CHIRP_C1 * t / chirp->length_s);
101  float K = CHIRP_C2 * (exponential - 1);
102 
104 
105  float theta = 2 * M_PI * (chirp->f0_hz * t
106  + (chirp->f1_hz - chirp->f0_hz) * (chirp->length_s / CHIRP_C1 * K - CHIRP_C2 * t));
107 
108  chirp->current_value = sinf(theta);
109  } else { // linear-time chirp
110  float k = (chirp->f1_hz - chirp->f0_hz) / chirp->length_s;
111 
112  chirp->current_frequency_hz = k * t;
113  chirp->current_value = sinf(2 * M_PI * t * (chirp->f0_hz + chirp->current_frequency_hz / 2));
114  }
115 
116  return chirp->current_value;
117 }
CHIRP_C1
#define CHIRP_C1
Definition: pprz_chirp.c:29
chirp_t::percentage_done
float percentage_done
Definition: pprz_chirp.h:69
chirp_t::fade_in
bool fade_in
Definition: pprz_chirp.h:72
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
chirp_t::f0_hz
float f0_hz
Definition: pprz_chirp.h:60
chirp_t::exponential_chirp
bool exponential_chirp
Definition: pprz_chirp.h:71
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_t::total_length_s
float total_length_s
Definition: pprz_chirp.h:64
pprz_chirp.h
chirp_t::start_time_s
float start_time_s
Definition: pprz_chirp.h:62
std.h
chirp_t::current_value
float current_value
Definition: pprz_chirp.h:67
CHIRP_C2
#define CHIRP_C2
Definition: pprz_chirp.c:30
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
chirp_t
Initialize with chirp_init.
Definition: pprz_chirp.h:59
K
static float K[9]
Definition: undistort_image.c:41
chirp_t::current_frequency_hz
float current_frequency_hz
Definition: pprz_chirp.h:66
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
chirp_t::length_s
float length_s
Definition: pprz_chirp.h:63
chirp_t::current_time_s
float current_time_s
Definition: pprz_chirp.h:68
Min
#define Min(x, y)
Definition: esc_dshot.c:85
chirp_t::f1_hz
float f1_hz
Definition: pprz_chirp.h:61