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
51
chirp
->
current_frequency_hz
= 0;
52
chirp
->
current_value
= 0;
53
chirp
->
percentage_done
= 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;
60
chirp
->
current_frequency_hz
= 0;
61
chirp
->
current_value
= 0;
62
chirp
->
percentage_done
= 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
}
84
chirp
->
percentage_done
= t /
chirp
->
total_length_s
;
85
86
if
(
chirp
->
fade_in
&& t < 2 / chirp->f0_hz) {
// Fade-in is two times the wavelength of f0
87
chirp
->
current_frequency_hz
=
chirp
->
f0_hz
;
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
103
chirp
->
current_frequency_hz
=
chirp
->
f0_hz
+
K
* (
chirp
->
f1_hz
-
chirp
->
f0_hz
);
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
sw
airborne
modules
system_identification
pprz_chirp.c
Generated on Tue Feb 1 2022 13:51:16 for Paparazzi UAS by
1.8.17