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
sys_id_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  */
34 #include "std.h"
35 
36 #include "sys_id_doublet.h"
37 #include "pprz_doublet.h"
38 
40 #include "generated/airframe.h"
41 #include "mcu_periph/sys_time.h"
42 
43 #ifndef DOUBLET_AXES
44 #define DOUBLET_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}
45 #endif
46 
47 #ifndef DOUBLET_ENABLED
48 #define DOUBLET_ENABLED TRUE
49 #endif
50 
51 
52 
53 static struct doublet_t doublet;
54 
57 
59 
61 float doublet_length_s = 20.0f;
63 
64 
65 
67 #define DOUBLET_NB_AXES sizeof ACTIVE_DOUBLET_AXES / sizeof ACTIVE_DOUBLET_AXES[0] // Number of items in ACTIVE_DOUBLET_AXES
68 
70 
71 static void set_current_doublet_values(void)
72 {
73  if (doublet_active) {
75 
76  } else {
77  for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) {
79  }
80  }
81 }
82 
83 static void send_doublet(struct transport_tx *trans, struct link_device *dev){
84  pprz_msg_send_DOUBLET(trans, dev, AC_ID, &doublet_active,
87 }
88 
89 static void start_doublet(void)
90 {
92  doublet_active = true;
94 }
95 
96 static void stop_doublet(void)
97 {
99  doublet_active = false;
101 }
102 
104  return doublet_active;
105 }
106 
108 {
109  doublet_active = activate;
110  if (doublet_active) {
112  start_doublet();
113  } else {
114  stop_doublet();
115  }
116 }
117 
119 {
120  if (axis < DOUBLET_NB_AXES) {
121  doublet_axis = axis;
122  }
123 }
124 
127 }
128 
130 {
132 
135 
136  for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) {
137  current_doublet_values[i] = 0;
138  }
139 }
140 
142 {
143  if (doublet_active) {
145  stop_doublet();
146  } else {
149  }
150  }
151 
152 
153 }
154 
155 void sys_id_doublet_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
156 {
157  (void)(override_on); // Suppress unused parameter warnings
158 
159 #if DOUBLET_ENABLED
160 
161 if (motors_on) {
162  for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) {
164  BoundAbs(in_cmd[ACTIVE_DOUBLET_AXES[i]], MAX_PPRZ);
165  }
166 }
167 
168 #endif
169 }
int16_t pprz_t
Definition: paparazzi.h:6
#define MAX_PPRZ
Definition: paparazzi.h:8
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 current_value
Definition: pprz_doublet.h:65
Initialize with doublet_init.
Definition: pprz_doublet.h:56
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
Definition: sonar_bebop.c:69
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
static pprz_t current_doublet_values[DOUBLET_NB_AXES]
#define DOUBLET_NB_AXES
uint8_t doublet_active
static struct doublet_t doublet
void sys_id_doublet_activate_handler(uint8_t activate)
#define DOUBLET_AXES
static void send_doublet(struct transport_tx *trans, struct link_device *dev)
float doublet_extra_waiting_time_s
static void set_current_doublet_values(void)
void sys_id_doublet_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
void sys_id_doublet_init(void)
static const int8_t ACTIVE_DOUBLET_AXES[]
float doublet_length_s
void sys_id_doublet_mod3211_handler(uint8_t mode)
void sys_id_doublet_axis_handler(uint8_t axis)
void sys_id_doublet_run(void)
uint8_t doublet_axis
uint8_t sys_id_doublet_running(void)
static void stop_doublet(void)
static void start_doublet(void)
pprz_t doublet_amplitude
uint8_t doublet_mode_3211
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103