Paparazzi UAS
v6.1.0_stable
Paparazzi is a free software Unmanned Aircraft System.
pprz_doublet.c
Go to the documentation of this file.
1
/*
2
* Copyright (C) Alessandro Collicelli
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 "
pprz_doublet.h
"
27
#include "
std.h
"
28
29
30
31
void
doublet_init
(
struct
doublet_t
*
doublet
,
float
length_s,
float
extra_waiting_time_s,
float
current_time_s,
bool
mod3211)
32
{
33
doublet
->
t0
= current_time_s;
34
doublet
->
tf
= length_s;
35
doublet
->
total_length_s
=
doublet
->
tf
+ extra_waiting_time_s;
36
doublet
->
mod3211
= mod3211;
37
doublet
->
current_value
= 0;
38
doublet
->
current_time_s
= current_time_s;
39
40
if
(mod3211) {
41
doublet
->
t1
= length_s / 9;
42
doublet
->
t2
=
doublet
->
t1
* 4;
43
doublet
->
t3
=
doublet
->
t1
* 6;
44
doublet
->
t4
=
doublet
->
t1
* 7;
45
doublet
->
t5
=
doublet
->
t1
* 8;
46
}
else
{
47
doublet
->
t1
= length_s / 4;
48
doublet
->
t2
=
doublet
->
t1
* 2;
49
doublet
->
t3
=
doublet
->
t1
* 3;
50
doublet
->
t4
=
doublet
->
t1
* 4;
51
doublet
->
t5
=
doublet
->
tf
;
52
53
}
54
55
}
56
57
void
doublet_reset
(
struct
doublet_t
*
doublet
,
float
current_time_s){
58
doublet
->
t0
= current_time_s;
59
doublet
->
current_value
= 0.0f;
60
doublet
->
current_time_s
= current_time_s;
61
}
62
63
bool
doublet_is_running
(
struct
doublet_t
*
doublet
,
float
current_time_s){
64
float
t = current_time_s -
doublet
->
t0
;
65
return
(t >= 0) && (t <=
doublet
->
total_length_s
);
66
}
67
68
float
doublet_update
(
struct
doublet_t
*
doublet
,
float
current_time_s){
69
70
if
(!
doublet_is_running
(
doublet
, current_time_s)){
71
doublet
->
current_value
= 0.0f;
72
return
0;
73
}
74
75
float
t = current_time_s -
doublet
->
t0
;
// since the start of the doublet
76
if
((t>=0) & (t<=doublet->tf)){
77
if
(
doublet
->
mod3211
){
78
if
(((t >=
doublet
->
t1
) & (t <= doublet->t2)) | ((t >=
doublet
->
t3
) & (t <= doublet->t4))){
79
doublet
->
current_value
= 1.0f;
80
}
else
if
(((t >=
doublet
->
t2
) && (t <= doublet->t3)) | ((t >=
doublet
->
t4
) && (t <= doublet->t5))){
81
doublet
->
current_value
= -1.0f;
82
}
else
{
83
doublet
->
current_value
= 0.0f;
84
}
85
}
86
else
{
87
if
((t >=
doublet
->
t1
) & (t <= doublet->t2)){
88
doublet
->
current_value
= 1.0f;
89
}
else
if
((t >=
doublet
->
t2
) & (t <= doublet->t3)){
90
doublet
->
current_value
= -1.0f;
91
}
else
{
92
doublet
->
current_value
= 0.0f;
93
}
94
}
95
}
else
{
96
doublet
->
current_value
= 0.0f;
97
}
98
return
doublet
->
current_value
;
99
}
doublet_t::t5
float t5
Definition:
pprz_doublet.h:62
doublet_t::total_length_s
float total_length_s
Definition:
pprz_doublet.h:64
doublet_update
float doublet_update(struct doublet_t *doublet, float current_time_s)
Definition:
pprz_doublet.c:68
doublet_t::t3
float t3
Definition:
pprz_doublet.h:60
pprz_doublet.h
doublet_reset
void doublet_reset(struct doublet_t *doublet, float current_time_s)
Definition:
pprz_doublet.c:57
doublet_init
void doublet_init(struct doublet_t *doublet, float length_s, float extra_waiting_time_s, float current_time_s, bool mod3211)
Definition:
pprz_doublet.c:31
doublet_t::mod3211
bool mod3211
Definition:
pprz_doublet.h:68
doublet_t::tf
float tf
Definition:
pprz_doublet.h:63
doublet_t::current_time_s
float current_time_s
Definition:
pprz_doublet.h:66
std.h
doublet_t
Initialize with doublet_init.
Definition:
pprz_doublet.h:56
doublet_t::t1
float t1
Definition:
pprz_doublet.h:58
doublet_is_running
bool doublet_is_running(struct doublet_t *doublet, float current_time_s)
Definition:
pprz_doublet.c:63
doublet_t::t2
float t2
Definition:
pprz_doublet.h:59
doublet
static struct doublet_t doublet
Definition:
sys_id_doublet.c:53
doublet_t::current_value
float current_value
Definition:
pprz_doublet.h:65
doublet_t::t0
float t0
Definition:
pprz_doublet.h:57
doublet_t::t4
float t4
Definition:
pprz_doublet.h:61
sw
airborne
modules
system_identification
pprz_doublet.c
Generated on Tue Feb 1 2022 13:08:46 for Paparazzi UAS by
1.8.17