Paparazzi UAS  v6.2.0_stable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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;
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 }
bool doublet_is_running(struct doublet_t *doublet, float current_time_s)
Definition: pprz_doublet.c:63
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
void doublet_reset(struct doublet_t *doublet, float current_time_s)
Definition: pprz_doublet.c:57
float doublet_update(struct doublet_t *doublet, float current_time_s)
Definition: pprz_doublet.c:68
float t2
Definition: pprz_doublet.h:59
float total_length_s
Definition: pprz_doublet.h:64
float tf
Definition: pprz_doublet.h:63
float t5
Definition: pprz_doublet.h:62
float t4
Definition: pprz_doublet.h:61
float t3
Definition: pprz_doublet.h:60
bool mod3211
Definition: pprz_doublet.h:68
float current_value
Definition: pprz_doublet.h:65
float t1
Definition: pprz_doublet.h:58
float t0
Definition: pprz_doublet.h:57
float current_time_s
Definition: pprz_doublet.h:66
Initialize with doublet_init.
Definition: pprz_doublet.h:56
static struct doublet_t doublet