Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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;
60  float energy;
61 };
62 
63 struct ap_state {
64  pprz_t commands[COMMANDS_NB];
68 };
69 
70 // Status bits from FBW to AUTOPILOT
71 #define STATUS_RADIO_OK 0
72 #define STATUS_RADIO_REALLY_LOST 1
73 #define STATUS_MODE_AUTO 2
74 #define STATUS_MODE_FAILSAFE 3
75 #define AVERAGED_CHANNELS_SENT 4
76 #define MASK_FBW_CHANGED 0xf
77 
78 
79 extern struct fbw_state *fbw_state;
80 extern struct ap_state *ap_state;
81 
82 extern volatile bool inter_mcu_received_fbw;
83 extern volatile bool inter_mcu_received_ap;
84 
89 PPRZ_MUTEX_DECL(ap_state_mtx);
90 PPRZ_MUTEX_DECL(fbw_state_mtx);
91 
96 static inline pprz_t imcu_get_command(uint8_t cmd_idx)
97 {
98  PPRZ_MUTEX_LOCK(ap_state_mtx);
99  pprz_t val = ap_state->commands[cmd_idx];
100  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
101  return val;
102 }
103 
108 static inline void imcu_set_command(uint8_t cmd_idx, pprz_t cmd)
109 {
110  PPRZ_MUTEX_LOCK(ap_state_mtx);
111  ap_state->commands[cmd_idx] = cmd;
112  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
113 }
114 
118 static inline pprz_t imcu_get_roll_trim(void)
119 {
120  PPRZ_MUTEX_LOCK(ap_state_mtx);
121  pprz_t val = ap_state->command_roll_trim;
122  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
123  return val;
124 }
125 
129 static inline void imcu_set_roll_trim(pprz_t roll_trim)
130 {
131  PPRZ_MUTEX_LOCK(ap_state_mtx);
132  ap_state->command_roll_trim = roll_trim;
133  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
134 }
135 
139 static inline pprz_t imcu_get_pitch_trim(void)
140 {
141  PPRZ_MUTEX_LOCK(ap_state_mtx);
142  pprz_t val = ap_state->command_pitch_trim;
143  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
144  return val;
145 }
146 
150 static inline void imcu_set_pitch_trim(pprz_t pitch_trim)
151 {
152  PPRZ_MUTEX_LOCK(ap_state_mtx);
153  ap_state->command_pitch_trim = pitch_trim;
154  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
155 }
156 
160 static inline pprz_t imcu_get_yaw_trim(void)
161 {
162  PPRZ_MUTEX_LOCK(ap_state_mtx);
163  pprz_t val = ap_state->command_yaw_trim;
164  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
165  return val;
166 }
167 
171 static inline void imcu_set_yaw_trim(pprz_t yaw_trim)
172 {
173  PPRZ_MUTEX_LOCK(ap_state_mtx);
174  ap_state->command_yaw_trim = yaw_trim;
175  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
176 }
177 
178 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
179 
183 static inline pprz_t imcu_get_radio(uint8_t radio_idx)
184 {
185  PPRZ_MUTEX_LOCK(fbw_state_mtx);
186  pprz_t val = fbw_state->channels[radio_idx];
187  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
188  return val;
189 }
190 
195 static inline void imcu_set_radio(uint8_t radio_idx, pprz_t radio)
196 {
197  PPRZ_MUTEX_LOCK(fbw_state_mtx);
198  fbw_state->channels[radio_idx] = radio;
199  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
200 }
201 
205 static inline uint8_t imcu_get_ppm_cpt(void)
206 {
207  PPRZ_MUTEX_LOCK(fbw_state_mtx);
208  uint8_t val = fbw_state->ppm_cpt;
209  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
210  return val;
211 }
212 
216 static inline void imcu_set_ppm_cpt(uint8_t ppm_cpt)
217 {
218  PPRZ_MUTEX_LOCK(fbw_state_mtx);
219  fbw_state->ppm_cpt = ppm_cpt;
220  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
221 }
222 #endif
223 
227 static inline uint8_t imcu_get_status(void)
228 {
229  PPRZ_MUTEX_LOCK(fbw_state_mtx);
230  uint8_t val = fbw_state->status;
231  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
232  return val;
233 }
234 
238 static inline void imcu_set_status(uint8_t status)
239 {
240  PPRZ_MUTEX_LOCK(fbw_state_mtx);
241  fbw_state->status = status;
242  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
243 }
244 
250 static inline void imcu_get_electrical(uint16_t *_vsupply, int32_t *_current, float *_energy)
251 {
252  PPRZ_MUTEX_LOCK(fbw_state_mtx);
253  *_vsupply = fbw_state->vsupply;
254  *_current = fbw_state->current;
255  *_energy = fbw_state->energy;
256  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
257 }
258 
264 static inline void imcu_set_electrical(uint16_t _vsupply, int32_t _current, float _energy)
265 {
266  PPRZ_MUTEX_LOCK(fbw_state_mtx);
267  fbw_state->vsupply = _vsupply;
268  fbw_state->current = _current;
269  fbw_state->energy = _energy;
270  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
271 }
272 
273 #ifdef FBW
274 
275 extern uint8_t time_since_last_ap;
276 extern bool ap_ok;
277 
278 #define AP_STALLED_TIME 30 // 500ms with a 60Hz timer
279 
280 
281 static inline void inter_mcu_init(void)
282 {
283  fbw_state->status = 0;
284  fbw_state->nb_err = 0;
285 
286  PPRZ_MUTEX_INIT(ap_state_mtx);
287  PPRZ_MUTEX_INIT(fbw_state_mtx);
288 
289  ap_ok = false;
290 }
291 
292 
293 /* Prepare data to be sent to mcu0 */
294 static inline void inter_mcu_fill_fbw_state(void)
295 {
296  PPRZ_MUTEX_LOCK(fbw_state_mtx);
297  uint8_t status = 0;
298 
299 #ifdef RADIO_CONTROL
300  uint8_t i;
301  for (i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) {
302  fbw_state->channels[i] = radio_control.values[i];
303  }
304 
305  fbw_state->ppm_cpt = radio_control.frame_rate;
306 
307  status = (radio_control.status == RC_OK ? _BV(STATUS_RADIO_OK) : 0);
308  status |= (radio_control.status == RC_REALLY_LOST ? _BV(STATUS_RADIO_REALLY_LOST) : 0);
309  status |= (radio_control.status == RC_OK ? _BV(AVERAGED_CHANNELS_SENT) :
310  0); // Any valid frame contains averaged channels
311 #endif // RADIO_CONTROL
312 
313  status |= (fbw_mode == FBW_MODE_AUTO ? _BV(STATUS_MODE_AUTO) : 0);
314  status |= (fbw_mode == FBW_MODE_FAILSAFE ? _BV(STATUS_MODE_FAILSAFE) : 0);
315  fbw_state->status = status;
316 
317  fbw_state->vsupply = electrical.vsupply;
318  fbw_state->current = electrical.current;
319  fbw_state->energy = electrical.energy;
320 #if defined SINGLE_MCU
321 
322  inter_mcu_received_fbw = true;
323 #endif
324  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
325 }
326 
328 static inline void inter_mcu_event_task(void)
329 {
330  time_since_last_ap = 0;
331  ap_ok = true;
332 }
333 
335 static inline void inter_mcu_periodic_task(void)
336 {
337  if (time_since_last_ap >= AP_STALLED_TIME) {
338  ap_ok = false;
339 #ifdef SINGLE_MCU
340  // Keep filling the buffer even if no AP commands are received
341  inter_mcu_fill_fbw_state();
342 #endif
343 
344  } else {
345  time_since_last_ap++;
346  }
347 }
348 
349 #endif /* FBW */
350 
351 #endif /* INTER_MCU */
352 
353 #endif /* INTER_MCU_H */
int32_t current
current in milliamps
Definition: electrical.h:49
unsigned short uint16_t
Definition: types.h:16
pprz_t command_pitch_trim
Definition: main_fbw.c:69
int32_t current
Supply current in milliAmpere.
uint8_t status
Definition: radio_control.h:64
int16_t pprz_t
Definition: paparazzi.h:6
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
volatile bool inter_mcu_received_ap
Definition: inter_mcu.c:41
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
struct fbw_state * fbw_state
Definition: inter_mcu.c:36
struct ap_state * ap_state
Definition: inter_mcu.c:37
#define RADIO_CONTROL_NB_CHANNEL
Definition: intermcu_ap.h:48
float energy
Energy consumption (mAh) This is the ap copy of the measurement from fbw FIXME use electrical module ...
uint16_t vsupply
Supply voltage in deciVolt.
Interface for electrical status: supply voltage, current, battery status, etc.
pprz_t command_roll_trim
Trim commands for roll, pitch and yaw.
Definition: main_fbw.c:68
uint8_t status
uint16_t val[TCOUPLE_NB]
#define PPRZ_MUTEX_DECL(_mtx)
Definition: pprz_mutex.h:44
struct RadioControl radio_control
Definition: radio_control.c:30
pprz_t command_yaw_trim
Definition: main_fbw.c:70
#define PPRZ_MUTEX_INIT(_mtx)
Definition: pprz_mutex.h:45
signed long int32_t
Definition: types.h:19
#define RC_REALLY_LOST
Definition: radio_control.h:58
#define RC_OK
Definition: radio_control.h:56
volatile bool inter_mcu_received_fbw
Definition: inter_mcu.c:40
uint8_t fbw_mode
Definition: main_fbw.c:63
Utility functions and macros to abstract some RTOS functionalities such as mutexes.
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
uint16_t vsupply
supply voltage in decivolts
Definition: electrical.h:48
float energy
consumed energy in mAh
Definition: electrical.h:51
struct Electrical electrical
Definition: electrical.c:65
FBW ( FlyByWire ) process API.
uint8_t cmd_idx
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
uint8_t frame_rate
Definition: radio_control.h:67