Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 "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, 6);\
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 UNINIT 0
134 #define GOT_SYNC1 1
135 #define GOT_SYNC2 2
136 #define GOT_CLASS 3
137 #define GOT_ID 4
138 #define GOT_LEN1 5
139 #define GOT_LEN2 6
140 #define GOT_PAYLOAD 7
141 #define 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 UNINIT:
170  if (c == INTERMCU_SYNC1)
172  break;
173  case 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 GOT_SYNC2:
183  /* Previous message has not yet been parsed: discard this one */
184  goto error;
185  }
188  break;
189  case GOT_CLASS:
192  break;
193  case GOT_ID:
194  intermcu_data.len = c;
196  break;
197  case GOT_LEN1:
198  intermcu_data.len |= (c<<8);
200  goto error;
201  }
204  break;
205  case GOT_LEN2:
210  }
211  break;
212  case GOT_PAYLOAD:
213  if (c != intermcu_data.ck_a) {
214  goto error;
215  }
217  break;
218  case GOT_CHECKSUM1:
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 void link_mcu_init( void )
248 {
252 }
253 
254 void parse_mavpilot_msg( void )
255 {
257  {
259  {
260 #if COMMANDS_NB > 8
261 #error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED"
262 #endif
263 
264  for (int i=0; i< COMMANDS_NB; i++)
265  {
266  ap_state->commands[i] = ((pprz_t)MSG_INTERMCU_COMMAND(intermcu_data.msg_buf, i));
267  }
268 
269 #ifdef LINK_MCU_LED
270  LED_TOGGLE(LINK_MCU_LED);
271 #endif
273  }
275  {
276 #if RADIO_CONTROL_NB_CHANNEL > 10
277 #error "INTERMCU UART CAN ONLY SEND 10 RADIO CHANNELS OR THE UART WILL BE OVERFILLED"
278 #endif
279 
280  for (int i=0; i< RADIO_CONTROL_NB_CHANNEL; i++)
281  {
282  fbw_state->channels[i] = ((pprz_t)MSG_INTERMCU_RADIO(intermcu_data.msg_buf, i));
283  }
284  }
286  {
287  ap_state->command_roll_trim = ((pprz_t) MSG_INTERMCU_TRIM_ROLL(intermcu_data.msg_buf));
288  ap_state->command_pitch_trim = ((pprz_t) MSG_INTERMCU_TRIM_PITCH(intermcu_data.msg_buf));
289  }
291  {
292  fbw_state->ppm_cpt = MSG_INTERMCU_FBW_MOD(intermcu_data.msg_buf);
293  fbw_state->status = MSG_INTERMCU_FBW_STAT(intermcu_data.msg_buf);
294  fbw_state->nb_err = MSG_INTERMCU_FBW_ERR(intermcu_data.msg_buf);
295  fbw_state->vsupply = MSG_INTERMCU_FBW_VOLT(intermcu_data.msg_buf);
297 
299  }
300  }
301 }
302 
303 
304 #ifdef AP
305 void link_mcu_send( void )
306 {
307 #ifdef LINK_MCU_LED
308  LED_TOGGLE(LINK_MCU_LED);
309 #endif
311  InterMcuSend_INTERMCU_TRIM( ap_state->command_roll_trim, ap_state->command_pitch_trim );
312 }
313 #endif
314 
315 #ifdef FBW
316 // 60 Hz
317 static uint8_t SixtyHzCounter = 0;
318 
319 void link_mcu_periodic_task( void )
320 {
321  SixtyHzCounter++;
322  if (SixtyHzCounter >= 3)
323  {
324  // 20 Hz
325  SixtyHzCounter = 0;
326  inter_mcu_fill_fbw_state();
329  fbw_state->ppm_cpt,
330  fbw_state->status,
331  fbw_state->nb_err,
332  fbw_state->vsupply,
333  fbw_state->current);
334 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
336 #endif
337 
338  }
339 }
340 #endif
341 
342 void link_mcu_event_task( void ) {
343  /* A message has been received */
344  if (InterMcuBuffer()) {
345  while (InterMcuLink(ChAvailable())&&!intermcu_data.msg_available)
346  intermcu_parse(InterMcuLink(Getch()));
347  }
348 
352  }
353 }
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
uint8_t msg_buf[INTERMCU_MAX_PAYLOAD] __attribute__((aligned))
uint8_t send_ck_b
volatile bool_t inter_mcu_received_ap
Definition: inter_mcu.c:39
bool_t msg_available
int16_t pprz_t
Definition: paparazzi.h:6
volatile bool_t inter_mcu_received_fbw
Definition: inter_mcu.c:38
struct fbw_state * fbw_state
Definition: inter_mcu.c:34
#define FALSE
Definition: imu_chimu.h:141
struct ap_state * ap_state
Definition: inter_mcu.c:35
#define LED_TOGGLE(i)
Definition: led_hw.h:30
#define TRUE
Definition: imu_chimu.h:144
#define RADIO_CONTROL_NB_CHANNEL
Definition: spektrum_arch.h:36
unsigned char uint8_t
Definition: types.h:14
arch independent LED (Light Emitting Diodes) API
uint8_t send_ck_a
uint8_t msg_class
static struct point c
Definition: discsurvey.c:13
uint8_t error_cnt