Paparazzi UAS  v5.18.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"
46 #include "subsystems/electrical.h"
48 
49 
51 struct fbw_state {
52 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
54  uint8_t ppm_cpt;
55 #endif
57  uint8_t nb_err;
58  struct Electrical electrical;
59 };
60 
61 struct ap_state {
62  pprz_t commands[COMMANDS_NB];
66 };
67 
68 // Status bits from FBW to AUTOPILOT
69 #define STATUS_RADIO_OK 0
70 #define STATUS_RADIO_REALLY_LOST 1
71 #define STATUS_MODE_AUTO 2
72 #define STATUS_MODE_FAILSAFE 3
73 #define AVERAGED_CHANNELS_SENT 4
74 #define MASK_FBW_CHANGED 0xf
75 
76 
77 extern struct fbw_state *fbw_state;
78 extern struct ap_state *ap_state;
79 
80 extern volatile bool inter_mcu_received_fbw;
81 extern volatile bool inter_mcu_received_ap;
82 
87 PPRZ_MUTEX_DECL(ap_state_mtx);
88 PPRZ_MUTEX_DECL(fbw_state_mtx);
89 
94 static inline pprz_t imcu_get_command(uint8_t cmd_idx)
95 {
96  PPRZ_MUTEX_LOCK(ap_state_mtx);
97  pprz_t val = ap_state->commands[cmd_idx];
98  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
99  return val;
100 }
101 
106 static inline void imcu_set_command(uint8_t cmd_idx, pprz_t cmd)
107 {
108  PPRZ_MUTEX_LOCK(ap_state_mtx);
109  ap_state->commands[cmd_idx] = cmd;
110  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
111 }
112 
116 static inline pprz_t imcu_get_roll_trim(void)
117 {
118  PPRZ_MUTEX_LOCK(ap_state_mtx);
119  pprz_t val = ap_state->command_roll_trim;
120  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
121  return val;
122 }
123 
127 static inline void imcu_set_roll_trim(pprz_t roll_trim)
128 {
129  PPRZ_MUTEX_LOCK(ap_state_mtx);
130  ap_state->command_roll_trim = roll_trim;
131  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
132 }
133 
137 static inline pprz_t imcu_get_pitch_trim(void)
138 {
139  PPRZ_MUTEX_LOCK(ap_state_mtx);
140  pprz_t val = ap_state->command_pitch_trim;
141  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
142  return val;
143 }
144 
148 static inline void imcu_set_pitch_trim(pprz_t pitch_trim)
149 {
150  PPRZ_MUTEX_LOCK(ap_state_mtx);
151  ap_state->command_pitch_trim = pitch_trim;
152  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
153 }
154 
158 static inline pprz_t imcu_get_yaw_trim(void)
159 {
160  PPRZ_MUTEX_LOCK(ap_state_mtx);
161  pprz_t val = ap_state->command_yaw_trim;
162  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
163  return val;
164 }
165 
169 static inline void imcu_set_yaw_trim(pprz_t yaw_trim)
170 {
171  PPRZ_MUTEX_LOCK(ap_state_mtx);
172  ap_state->command_yaw_trim = yaw_trim;
173  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
174 }
175 
176 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
177 
181 static inline pprz_t imcu_get_radio(uint8_t radio_idx)
182 {
183  PPRZ_MUTEX_LOCK(fbw_state_mtx);
184  pprz_t val = fbw_state->channels[radio_idx];
185  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
186  return val;
187 }
188 
193 static inline void imcu_set_radio(uint8_t radio_idx, pprz_t radio)
194 {
195  PPRZ_MUTEX_LOCK(fbw_state_mtx);
196  fbw_state->channels[radio_idx] = radio;
197  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
198 }
199 
203 static inline uint8_t imcu_get_ppm_cpt(void)
204 {
205  PPRZ_MUTEX_LOCK(fbw_state_mtx);
206  uint8_t val = fbw_state->ppm_cpt;
207  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
208  return val;
209 }
210 
214 static inline void imcu_set_ppm_cpt(uint8_t ppm_cpt)
215 {
216  PPRZ_MUTEX_LOCK(fbw_state_mtx);
217  fbw_state->ppm_cpt = ppm_cpt;
218  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
219 }
220 #endif
221 
225 static inline uint8_t imcu_get_status(void)
226 {
227  PPRZ_MUTEX_LOCK(fbw_state_mtx);
228  uint8_t val = fbw_state->status;
229  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
230  return val;
231 }
232 
236 static inline void imcu_set_status(uint8_t status)
237 {
238  PPRZ_MUTEX_LOCK(fbw_state_mtx);
239  fbw_state->status = status;
240  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
241 }
242 
246 static inline void imcu_get_electrical(struct Electrical *_electrical)
247 {
248  PPRZ_MUTEX_LOCK(fbw_state_mtx);
249  *_electrical = fbw_state->electrical;
250  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
251 }
252 
256 static inline void imcu_set_electrical(struct Electrical *_electrical)
257 {
258  PPRZ_MUTEX_LOCK(fbw_state_mtx);
259  fbw_state->electrical = *_electrical;
260  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
261 }
262 
263 #ifdef FBW
264 
265 extern uint8_t time_since_last_ap;
266 extern bool ap_ok;
267 
268 #define AP_STALLED_TIME 30 // 500ms with a 60Hz timer
269 
270 
271 static inline void inter_mcu_init(void)
272 {
273  fbw_state->status = 0;
274  fbw_state->nb_err = 0;
275 
276  PPRZ_MUTEX_INIT(ap_state_mtx);
277  PPRZ_MUTEX_INIT(fbw_state_mtx);
278 
279  ap_ok = false;
280 }
281 
282 
283 /* Prepare data to be sent to mcu0 */
284 static inline void inter_mcu_fill_fbw_state(void)
285 {
286  PPRZ_MUTEX_LOCK(fbw_state_mtx);
287  uint8_t status = 0;
288 
289 #ifdef RADIO_CONTROL
290  uint8_t i;
291  for (i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) {
292  fbw_state->channels[i] = radio_control.values[i];
293  }
294 
296 
297  status = (radio_control.status == RC_OK ? _BV(STATUS_RADIO_OK) : 0);
298  status |= (radio_control.status == RC_REALLY_LOST ? _BV(STATUS_RADIO_REALLY_LOST) : 0);
299  status |= (radio_control.status == RC_OK ? _BV(AVERAGED_CHANNELS_SENT) :
300  0); // Any valid frame contains averaged channels
301 #endif // RADIO_CONTROL
302 
303  status |= (fbw_mode == FBW_MODE_AUTO ? _BV(STATUS_MODE_AUTO) : 0);
304  status |= (fbw_mode == FBW_MODE_FAILSAFE ? _BV(STATUS_MODE_FAILSAFE) : 0);
305  fbw_state->status = status;
306 
307  fbw_state->electrical = electrical;
308 #if defined SINGLE_MCU
309 
310  inter_mcu_received_fbw = true;
311 #endif
312  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
313 }
314 
316 static inline void inter_mcu_event_task(void)
317 {
318  time_since_last_ap = 0;
319  ap_ok = true;
320 }
321 
323 static inline void inter_mcu_periodic_task(void)
324 {
325  if (time_since_last_ap >= AP_STALLED_TIME) {
326  ap_ok = false;
327 #ifdef SINGLE_MCU
328  // Keep filling the buffer even if no AP commands are received
329  inter_mcu_fill_fbw_state();
330 #endif
331 
332  } else {
333  time_since_last_ap++;
334  }
335 }
336 
337 #endif /* FBW */
338 
339 #endif /* INTER_MCU */
340 
341 #endif /* INTER_MCU_H */
radio_control.h
electrical.h
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
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
uint8_t
unsigned char uint8_t
Definition: types.h:14
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
main_fbw.h
inttypes.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