Paparazzi UAS  v6.1.0_stable
Paparazzi is a free software Unmanned Aircraft System.
inter_mcu.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin
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  */
20 
32 #ifndef INTER_MCU_H
33 #define INTER_MCU_H
34 
35 
36 #ifdef INTER_MCU
37 
38 #include <inttypes.h>
39 
40 #include "std.h"
41 
42 #include "paparazzi.h"
43 #include "pprz_mutex.h"
44 #include "generated/airframe.h"
48 
50 static inline void intermcu_init(void) {}
51 static inline void intermcu_periodic(void) {}
52 
54 struct fbw_state {
55 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
57  uint8_t ppm_cpt;
58 #endif
60  uint8_t nb_err;
61  struct Electrical electrical;
62 };
63 
64 struct ap_state {
65  pprz_t commands[COMMANDS_NB];
69 };
70 
71 // Status bits from FBW to AUTOPILOT
72 #define STATUS_RADIO_OK 0
73 #define STATUS_RADIO_REALLY_LOST 1
74 #define STATUS_MODE_AUTO 2
75 #define STATUS_MODE_FAILSAFE 3
76 #define AVERAGED_CHANNELS_SENT 4
77 #define MASK_FBW_CHANGED 0xf
78 
79 
80 extern struct fbw_state *fbw_state;
81 extern struct ap_state *ap_state;
82 
83 extern volatile bool inter_mcu_received_fbw;
84 extern volatile bool inter_mcu_received_ap;
85 
90 PPRZ_MUTEX_DECL(ap_state_mtx);
91 PPRZ_MUTEX_DECL(fbw_state_mtx);
92 
97 static inline pprz_t imcu_get_command(uint8_t cmd_idx)
98 {
99  PPRZ_MUTEX_LOCK(ap_state_mtx);
100  pprz_t val = ap_state->commands[cmd_idx];
101  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
102  return val;
103 }
104 
109 static inline void imcu_set_command(uint8_t cmd_idx, pprz_t cmd)
110 {
111  PPRZ_MUTEX_LOCK(ap_state_mtx);
112  ap_state->commands[cmd_idx] = cmd;
113  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
114 }
115 
119 static inline pprz_t imcu_get_roll_trim(void)
120 {
121  PPRZ_MUTEX_LOCK(ap_state_mtx);
122  pprz_t val = ap_state->command_roll_trim;
123  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
124  return val;
125 }
126 
130 static inline void imcu_set_roll_trim(pprz_t roll_trim)
131 {
132  PPRZ_MUTEX_LOCK(ap_state_mtx);
133  ap_state->command_roll_trim = roll_trim;
134  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
135 }
136 
140 static inline pprz_t imcu_get_pitch_trim(void)
141 {
142  PPRZ_MUTEX_LOCK(ap_state_mtx);
143  pprz_t val = ap_state->command_pitch_trim;
144  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
145  return val;
146 }
147 
151 static inline void imcu_set_pitch_trim(pprz_t pitch_trim)
152 {
153  PPRZ_MUTEX_LOCK(ap_state_mtx);
154  ap_state->command_pitch_trim = pitch_trim;
155  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
156 }
157 
161 static inline pprz_t imcu_get_yaw_trim(void)
162 {
163  PPRZ_MUTEX_LOCK(ap_state_mtx);
164  pprz_t val = ap_state->command_yaw_trim;
165  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
166  return val;
167 }
168 
172 static inline void imcu_set_yaw_trim(pprz_t yaw_trim)
173 {
174  PPRZ_MUTEX_LOCK(ap_state_mtx);
175  ap_state->command_yaw_trim = yaw_trim;
176  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
177 }
178 
179 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
180 
184 static inline pprz_t imcu_get_radio(uint8_t radio_idx)
185 {
186  PPRZ_MUTEX_LOCK(fbw_state_mtx);
187  pprz_t val = fbw_state->channels[radio_idx];
188  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
189  return val;
190 }
191 
196 static inline void imcu_set_radio(uint8_t radio_idx, pprz_t radio)
197 {
198  PPRZ_MUTEX_LOCK(fbw_state_mtx);
199  fbw_state->channels[radio_idx] = radio;
200  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
201 }
202 
206 static inline uint8_t imcu_get_ppm_cpt(void)
207 {
208  PPRZ_MUTEX_LOCK(fbw_state_mtx);
209  uint8_t val = fbw_state->ppm_cpt;
210  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
211  return val;
212 }
213 
217 static inline void imcu_set_ppm_cpt(uint8_t ppm_cpt)
218 {
219  PPRZ_MUTEX_LOCK(fbw_state_mtx);
220  fbw_state->ppm_cpt = ppm_cpt;
221  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
222 }
223 #endif
224 
228 static inline uint8_t imcu_get_status(void)
229 {
230  PPRZ_MUTEX_LOCK(fbw_state_mtx);
231  uint8_t val = fbw_state->status;
232  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
233  return val;
234 }
235 
239 static inline void imcu_set_status(uint8_t status)
240 {
241  PPRZ_MUTEX_LOCK(fbw_state_mtx);
242  fbw_state->status = status;
243  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
244 }
245 
249 static inline void imcu_get_electrical(struct Electrical *_electrical)
250 {
251  PPRZ_MUTEX_LOCK(fbw_state_mtx);
252  *_electrical = fbw_state->electrical;
253  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
254 }
255 
259 static inline void imcu_set_electrical(struct Electrical *_electrical)
260 {
261  PPRZ_MUTEX_LOCK(fbw_state_mtx);
262  fbw_state->electrical = *_electrical;
263  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
264 }
265 
266 #ifdef FBW
267 
268 extern uint8_t time_since_last_ap;
269 extern bool ap_ok;
270 
271 #define AP_STALLED_TIME 30 // 500ms with a 60Hz timer
272 
273 
274 static inline void inter_mcu_init(void)
275 {
276  fbw_state->status = 0;
277  fbw_state->nb_err = 0;
278 
279  PPRZ_MUTEX_INIT(ap_state_mtx);
280  PPRZ_MUTEX_INIT(fbw_state_mtx);
281 
282  ap_ok = false;
283 }
284 
285 
286 /* Prepare data to be sent to mcu0 */
287 static inline void inter_mcu_fill_fbw_state(void)
288 {
289  PPRZ_MUTEX_LOCK(fbw_state_mtx);
290  uint8_t status = 0;
291 
292 #ifdef RADIO_CONTROL
293  uint8_t i;
294  for (i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) {
295  fbw_state->channels[i] = radio_control.values[i];
296  }
297 
299 
300  status = (radio_control.status == RC_OK ? _BV(STATUS_RADIO_OK) : 0);
301  status |= (radio_control.status == RC_REALLY_LOST ? _BV(STATUS_RADIO_REALLY_LOST) : 0);
302  status |= (radio_control.status == RC_OK ? _BV(AVERAGED_CHANNELS_SENT) :
303  0); // Any valid frame contains averaged channels
304 #endif // RADIO_CONTROL
305 
306  status |= (fbw_mode == FBW_MODE_AUTO ? _BV(STATUS_MODE_AUTO) : 0);
307  status |= (fbw_mode == FBW_MODE_FAILSAFE ? _BV(STATUS_MODE_FAILSAFE) : 0);
308  fbw_state->status = status;
309 
310  fbw_state->electrical = electrical;
311 #if defined SINGLE_MCU
312 
313  inter_mcu_received_fbw = true;
314 #endif
315  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
316 }
317 
319 static inline void inter_mcu_event_task(void)
320 {
321  time_since_last_ap = 0;
322  ap_ok = true;
323 }
324 
326 static inline void inter_mcu_periodic_task(void)
327 {
328  if (time_since_last_ap >= AP_STALLED_TIME) {
329  ap_ok = false;
330 #ifdef SINGLE_MCU
331  // Keep filling the buffer even if no AP commands are received
332  inter_mcu_fill_fbw_state();
333 #endif
334 
335  } else {
336  time_since_last_ap++;
337  }
338 }
339 
340 #endif /* FBW */
341 
342 #endif /* INTER_MCU */
343 
344 #endif /* INTER_MCU_H */
radio_control.h
electrical.h
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
PPRZ_MUTEX_INIT
#define PPRZ_MUTEX_INIT(_mtx)
Definition: pprz_mutex.h:45
command_roll_trim
pprz_t command_roll_trim
Trim commands for roll, pitch and yaw.
Definition: main_fbw.c:68
val
uint16_t val[TCOUPLE_NB]
Definition: temp_tcouple_adc.c:49
FBW_MODE_FAILSAFE
@ FBW_MODE_FAILSAFE
Definition: main_fbw.h:64
status
uint8_t status
Definition: nps_radio_control_spektrum.c:101
fbw_mode
uint8_t fbw_mode
Definition: main_fbw.c:63
inter_mcu_received_ap
volatile bool inter_mcu_received_ap
Definition: inter_mcu.c:41
fbw_state
struct fbw_state * fbw_state
Definition: inter_mcu.c:36
PPRZ_MUTEX_DECL
#define PPRZ_MUTEX_DECL(_mtx)
Definition: pprz_mutex.h:44
Electrical
Definition: electrical.h:44
PPRZ_MUTEX_UNLOCK
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
paparazzi.h
RadioControl::frame_rate
uint8_t frame_rate
Definition: radio_control.h:67
command_yaw_trim
pprz_t command_yaw_trim
Definition: main_fbw.c:70
std.h
intermcu_periodic
void intermcu_periodic(void)
Definition: intermcu_ap.c:90
pprz_t
int16_t pprz_t
Definition: paparazzi.h:6
inter_mcu_received_fbw
volatile bool inter_mcu_received_fbw
Definition: inter_mcu.c:40
RADIO_CONTROL_NB_CHANNEL
#define RADIO_CONTROL_NB_CHANNEL
Definition: intermcu_ap.h:49
RadioControl::status
uint8_t status
Definition: radio_control.h:64
command_pitch_trim
pprz_t command_pitch_trim
Definition: main_fbw.c:69
PPRZ_MUTEX_LOCK
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
commands
static const ShellCommand commands[]
Definition: shell_arch.c:71
cmd_idx
uint8_t cmd_idx
Definition: usb_serial_stm32_example1.c:37
RC_OK
#define RC_OK
Definition: radio_control.h:56
pprz_mutex.h
FBW_MODE_AUTO
@ FBW_MODE_AUTO
Definition: main_fbw.h:64
ap_state
struct ap_state * ap_state
Definition: inter_mcu.c:37
electrical
struct Electrical electrical
Definition: electrical.c:66
intermcu_init
void intermcu_init(void)
Definition: intermcu_ap.c:71
main_fbw.h
RC_REALLY_LOST
#define RC_REALLY_LOST
Definition: radio_control.h:58
radio_control
struct RadioControl radio_control
Definition: radio_control.c:30
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69