Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
link_mcu_usart.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-2012 The Paparazzi Team
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
23 #include "link_mcu_usart.h"
24 #include "mcu_periph/uart.h"
25 #include "led.h"
26 
27 #include "subsystems/commands.h"
28 
30 // LINK
31 
32 #define __InterMcuLink(dev, _x) dev##_x
33 #define _InterMcuLink(dev, _x) __InterMcuLink(dev, _x)
34 #define InterMcuLink(_x) _InterMcuLink(INTERMCU_LINK, _x)
35 
36 #define InterMcuBuffer() InterMcuLink(ChAvailable())
37 
38 #define InterMcuUartSend1(c) InterMcuLink(Transmit(c))
39 #define InterMcuUartSetBaudrate(_a) InterMcuLink(SetBaudrate(_a))
40 #define InterMcuUartRunning InterMcuLink(TxRunning)
41 #define InterMcuUartSendMessage InterMcuLink(SendMessage)
42 
44 // PROTOCOL
45 
46 #define INTERMCU_SYNC1 0xB5
47 #define INTERMCU_SYNC2 0x62
48 
49 #define InterMcuInitCheksum() { intermcu_data.send_ck_a = intermcu_data.send_ck_b = 0; }
50 #define UpdateChecksum(c) { intermcu_data.send_ck_a += c; intermcu_data.send_ck_b += intermcu_data.send_ck_a; }
51 #define InterMcuTrailer() { InterMcuUartSend1(intermcu_data.send_ck_a); InterMcuUartSend1(intermcu_data.send_ck_b); InterMcuUartSendMessage(); }
52 
53 #define InterMcuSend1(c) { uint8_t i8=c; InterMcuUartSend1(i8); UpdateChecksum(i8); }
54 #define InterMcuSend2(c) { uint16_t i16=c; InterMcuSend1(i16&0xff); InterMcuSend1(i16 >> 8); }
55 #define InterMcuSend1ByAddr(x) { InterMcuSend1(*x); }
56 #define InterMcuSend2ByAddr(x) { InterMcuSend1(*x); InterMcuSend1(*(x+1)); }
57 #define InterMcuSend4ByAddr(x) { InterMcuSend1(*x); InterMcuSend1(*(x+1)); InterMcuSend1(*(x+2)); InterMcuSend1(*(x+3)); }
58 
59 #define InterMcuHeader(nav_id, msg_id, len) { \
60  InterMcuUartSend1(INTERMCU_SYNC1); \
61  InterMcuUartSend1(INTERMCU_SYNC2); \
62  InterMcuInitCheksum(); \
63  InterMcuSend1(nav_id); \
64  InterMcuSend1(msg_id); \
65  InterMcuSend2(len); \
66  }
67 
69 // MESSAGES
70 
71 // class
72 #define MSG_INTERMCU_ID 100
73 
74 
75 #define MSG_INTERMCU_COMMAND_ID 0x05
76 #define MSG_INTERMCU_COMMAND_LENGTH (2*(COMMANDS_NB))
77 #define MSG_INTERMCU_COMMAND(_intermcu_payload, nr) (uint16_t)(*((uint8_t*)_intermcu_payload+0)|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8)
78 
79 #define InterMcuSend_INTERMCU_COMMAND(cmd) { \
80  InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_COMMAND_ID, MSG_INTERMCU_COMMAND_LENGTH);\
81  for (int i=0;i<COMMANDS_NB;i++) { \
82  uint16_t _cmd = cmd[i]; \
83  InterMcuSend2ByAddr((uint8_t*)&_cmd);\
84  } \
85  InterMcuTrailer();\
86 }
87 
88 #define MSG_INTERMCU_RADIO_ID 0x08
89 #define MSG_INTERMCU_RADIO_LENGTH (2*(RADIO_CONTROL_NB_CHANNEL))
90 #define MSG_INTERMCU_RADIO(_intermcu_payload, nr) (uint16_t)(*((uint8_t*)_intermcu_payload+0)|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8)
91 
92 #define InterMcuSend_INTERMCU_RADIO(cmd) { \
93  InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_RADIO_ID, MSG_INTERMCU_RADIO_LENGTH);\
94  for (int i=0;i<RADIO_CONTROL_NB_CHANNEL;i++) { \
95  uint16_t _cmd = cmd[i]; \
96  InterMcuSend2ByAddr((uint8_t*)&_cmd);\
97  } \
98  InterMcuTrailer();\
99 }
100 
101 #define MSG_INTERMCU_FBW_ID 0x06
102 #define MSG_INTERMCU_FBW_MOD(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+0))
103 #define MSG_INTERMCU_FBW_STAT(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+1))
104 #define MSG_INTERMCU_FBW_ERR(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+2))
105 #define MSG_INTERMCU_FBW_VOLT(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+3)|*((uint8_t*)_intermcu_payload+1+3)<<8)
106 #define MSG_INTERMCU_FBW_CURRENT(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+5)|*((uint8_t*)_intermcu_payload+1+5)<<8)
107 
108 #define InterMcuSend_INTERMCU_FBW(mod,stat,err,volt,current) { \
109  InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_FBW_ID, 7);\
110  uint8_t _mod = mod; InterMcuSend1ByAddr((uint8_t*)&_mod);\
111  uint8_t _stat = stat; InterMcuSend1ByAddr((uint8_t*)&_stat);\
112  uint8_t _err = err; InterMcuSend1ByAddr((uint8_t*)&_err);\
113  uint16_t _volt = volt; InterMcuSend2ByAddr((uint8_t*)&_volt);\
114  uint16_t _current = current; InterMcuSend2ByAddr((uint8_t*)&_current);\
115  InterMcuTrailer();\
116 }
117 
118 #define MSG_INTERMCU_TRIM_ID 0x07
119 #define MSG_INTERMCU_TRIM_ROLL(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+0)|*((uint8_t*)_intermcu_payload+1)<<8)
120 #define MSG_INTERMCU_TRIM_PITCH(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+2)|*((uint8_t*)_intermcu_payload+3)<<8)
121 
122 #define InterMcuSend_INTERMCU_TRIM(roll,pitch) { \
123  InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_TRIM_ID, 4);\
124  uint16_t _roll = roll; InterMcuSend2ByAddr((uint8_t*)&_roll);\
125  uint16_t _pitch = pitch; InterMcuSend2ByAddr((uint8_t*)&_pitch);\
126  InterMcuTrailer();\
127 }
128 
130 // PARSER
131 
132 /* parser status */
133 #define LINK_MCU_UNINIT 0
134 #define LINK_MCU_GOT_SYNC1 1
135 #define LINK_MCU_GOT_SYNC2 2
136 #define LINK_MCU_GOT_CLASS 3
137 #define LINK_MCU_GOT_ID 4
138 #define LINK_MCU_GOT_LEN1 5
139 #define LINK_MCU_GOT_LEN2 6
140 #define LINK_MCU_GOT_PAYLOAD 7
141 #define LINK_MCU_GOT_CHECKSUM1 8
142 
143 
144 #define INTERMCU_MAX_PAYLOAD 255
145 struct InterMcuData {
147  uint8_t msg_buf[INTERMCU_MAX_PAYLOAD] __attribute__ ((aligned));
150 
157 };
158 
160 
161 /* INTERMCU parsing */
162 void intermcu_parse( uint8_t c );
165  intermcu_data.ck_a += c;
167  }
168  switch (intermcu_data.status) {
169  case LINK_MCU_UNINIT:
170  if (c == INTERMCU_SYNC1)
172  break;
173  case LINK_MCU_GOT_SYNC1:
174  if (c != INTERMCU_SYNC2) {
175  goto error;
176  }
177  intermcu_data.ck_a = 0;
178  intermcu_data.ck_b = 0;
180  break;
181  case LINK_MCU_GOT_SYNC2:
183  /* Previous message has not yet been parsed: discard this one */
184  goto error;
185  }
188  break;
189  case LINK_MCU_GOT_CLASS:
190  intermcu_data.msg_id = c;
192  break;
193  case LINK_MCU_GOT_ID:
194  intermcu_data.len = c;
196  break;
197  case LINK_MCU_GOT_LEN1:
198  intermcu_data.len |= (c<<8);
200  goto error;
201  }
204  break;
205  case LINK_MCU_GOT_LEN2:
210  }
211  break;
213  if (c != intermcu_data.ck_a) {
214  goto error;
215  }
217  break;
219  if (c != intermcu_data.ck_b) {
220  goto error;
221  }
223  goto restart;
224  break;
225  default:
226  goto error;
227  }
228  return;
229  error:
231  restart:
233  return;
234 }
235 
236 
237 
239 // USER
240 
241 
244 
245 inline void parse_mavpilot_msg( void );
246 
247 
248 #ifdef AP
250 
251 #define RC_OK 0
252 #define RC_LOST 1
253 #define RC_REALLY_LOST 2
254 
255 
256 static void send_commands(void) {
257  DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, ap_state->commands);
258 }
259 
260 
261 static void send_fbw_status(void) {
262  uint8_t rc_status = 0;
263  uint8_t fbw_status = 0;
264  if (bit_is_set(fbw_state->status, STATUS_MODE_AUTO))
265  fbw_status = FBW_MODE_AUTO;
266  if (bit_is_set(fbw_state->status, STATUS_MODE_FAILSAFE))
267  fbw_status = FBW_MODE_FAILSAFE;
268  if (bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST))
269  rc_status = RC_REALLY_LOST;
270  else if (bit_is_set(fbw_state->status, RC_OK))
271  rc_status = RC_OK;
272  else
273  rc_status = RC_LOST;
274  DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice,
275  &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current));
276 }
277 #endif
278 
279 void link_mcu_init( void )
280 {
284 #ifdef AP
285  #if PERIODIC_TELEMETRY
286  // If FBW has not telemetry, then AP can send some of the info
287  register_periodic_telemetry(DefaultPeriodic, "COMMANDS", send_commands);
288  register_periodic_telemetry(DefaultPeriodic, "FBW_STATUS", send_fbw_status);
289 #endif
290 #endif
291 }
292 
293 void parse_mavpilot_msg( void )
294 {
296  {
298  {
299 #if COMMANDS_NB > 8
300 #error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED"
301 #endif
302 
303  for (int i=0; i< COMMANDS_NB; i++)
304  {
306  }
307 
308 #ifdef LINK_MCU_LED
309  LED_TOGGLE(LINK_MCU_LED);
310 #endif
312  }
314  {
315 #if RADIO_CONTROL_NB_CHANNEL > 10
316 #error "INTERMCU UART CAN ONLY SEND 10 RADIO CHANNELS OR THE UART WILL BE OVERFILLED"
317 #endif
318 
319  for (int i=0; i< RADIO_CONTROL_NB_CHANNEL; i++)
320  {
322  }
323  }
325  {
326  ap_state->command_roll_trim = ((pprz_t) MSG_INTERMCU_TRIM_ROLL(intermcu_data.msg_buf));
327  ap_state->command_pitch_trim = ((pprz_t) MSG_INTERMCU_TRIM_PITCH(intermcu_data.msg_buf));
328  }
330  {
336 
337 #ifdef LINK_MCU_LED
338  LED_TOGGLE(LINK_MCU_LED);
339 #endif
341  }
342  }
343 }
344 
345 
346 #ifdef AP
347 void link_mcu_send( void )
348 {
350  InterMcuSend_INTERMCU_TRIM( ap_state->command_roll_trim, ap_state->command_pitch_trim );
351 }
352 #endif
353 
354 #ifdef FBW
355 // 60 Hz
356 static uint8_t SixtyHzCounter = 0;
357 
358 void link_mcu_periodic_task( void )
359 {
360  SixtyHzCounter++;
361  if (SixtyHzCounter >= 3)
362  {
363  // 20 Hz
364  SixtyHzCounter = 0;
365  inter_mcu_fill_fbw_state();
368  fbw_state->ppm_cpt,
369  fbw_state->status,
370  fbw_state->nb_err,
371  fbw_state->vsupply,
372  fbw_state->current);
373 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
375 #endif
376 
377  }
378 }
379 #endif
380 
381 void link_mcu_event_task( void ) {
382  /* A message has been received */
383  if (InterMcuBuffer()) {
384  while (InterMcuLink(ChAvailable()))
385  {
386  intermcu_parse(InterMcuLink(Getch()));
390  }
391  }
392  }
393 }
#define RC_LOST
Definition: radio_control.h:46
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
Periodic telemetry system header (includes downlink utility and generated code).
uint8_t send_ck_b
volatile bool_t inter_mcu_received_ap
Definition: inter_mcu.c:37
bool_t msg_available
int16_t pprz_t
Definition: paparazzi.h:6
volatile bool_t inter_mcu_received_fbw
Definition: inter_mcu.c:36
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
struct fbw_state * fbw_state
Definition: inter_mcu.c:32
#define FALSE
Definition: imu_chimu.h:141
struct ap_state * ap_state
Definition: inter_mcu.c:33
#define FBW_MODE_AUTO
Definition: main_fbw.h:36
Hardware independent code for commands handling.
#define LED_TOGGLE(i)
Definition: led_hw.h:30
#define RC_REALLY_LOST
Definition: radio_control.h:47
#define TRUE
Definition: imu_chimu.h:144
#define RC_OK
Definition: radio_control.h:45
#define RADIO_CONTROL_NB_CHANNEL
Definition: spektrum_arch.h:34
unsigned char uint8_t
Definition: types.h:14
#define FBW_MODE_FAILSAFE
Definition: main_fbw.h:37
arch independent LED (Light Emitting Diodes) API
uint8_t send_ck_a
uint8_t msg_class
uint8_t msg_buf[INTERMCU_MAX_PAYLOAD]
uint8_t error_cnt