Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 // Use uart interface directly
33 #define InterMcuBuffer() uart_char_available(&(INTERMCU_LINK))
34 #define InterMcuUartSend1(c) uart_put_byte(&(INTERMCU_LINK), 0, c)
35 #define InterMcuUartSetBaudrate(_a) uart_periph_set_baudrate(&(INTERMCU_LINK), _a)
36 #define InterMcuUartSendMessage() {}
37 #define InterMcuUartGetch() uart_getch(&(INTERMCU_LINK))
38 
40 // PROTOCOL
41 
42 #define INTERMCU_SYNC1 0xB5
43 #define INTERMCU_SYNC2 0x62
44 
45 #define InterMcuInitCheksum() { intermcu_data.send_ck_a = intermcu_data.send_ck_b = 0; }
46 #define InterMcuUpdateChecksum(c) { intermcu_data.send_ck_a += c; intermcu_data.send_ck_b += intermcu_data.send_ck_a; }
47 #define InterMcuTrailer() { InterMcuUartSend1(intermcu_data.send_ck_a); InterMcuUartSend1(intermcu_data.send_ck_b); InterMcuUartSendMessage(); }
48 
49 #define InterMcuSend1(c) { uint8_t i8=c; InterMcuUartSend1(i8); InterMcuUpdateChecksum(i8); }
50 #define InterMcuSend2(c) { uint16_t i16=c; InterMcuSend1(i16&0xff); InterMcuSend1(i16 >> 8); }
51 #define InterMcuSend1ByAddr(x) { InterMcuSend1(*x); }
52 #define InterMcuSend2ByAddr(x) { InterMcuSend1(*x); InterMcuSend1(*(x+1)); }
53 #define InterMcuSend4ByAddr(x) { InterMcuSend1(*x); InterMcuSend1(*(x+1)); InterMcuSend1(*(x+2)); InterMcuSend1(*(x+3)); }
54 
55 #define InterMcuHeader(nav_id, msg_id, len) { \
56  InterMcuUartSend1(INTERMCU_SYNC1); \
57  InterMcuUartSend1(INTERMCU_SYNC2); \
58  InterMcuInitCheksum(); \
59  InterMcuSend1(nav_id); \
60  InterMcuSend1(msg_id); \
61  InterMcuSend2(len); \
62  }
63 
65 // MESSAGES
66 
67 // class
68 #define MSG_INTERMCU_ID 100
69 
70 
71 #define MSG_INTERMCU_COMMAND_ID 0x05
72 #define MSG_INTERMCU_COMMAND_LENGTH (2*(COMMANDS_NB))
73 #define MSG_INTERMCU_COMMAND(_intermcu_payload, nr) (uint16_t)(*((uint8_t*)_intermcu_payload+0+(2*(nr)))|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8)
74 
75 #define InterMcuSend_INTERMCU_COMMAND(cmd) { \
76  InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_COMMAND_ID, MSG_INTERMCU_COMMAND_LENGTH);\
77  for (int i=0;i<COMMANDS_NB;i++) { \
78  uint16_t _cmd = cmd[i]; \
79  InterMcuSend2ByAddr((uint8_t*)&_cmd);\
80  } \
81  InterMcuTrailer();\
82  }
83 
84 #define MSG_INTERMCU_RADIO_ID 0x08
85 #define MSG_INTERMCU_RADIO_LENGTH (2*(RADIO_CONTROL_NB_CHANNEL))
86 #define MSG_INTERMCU_RADIO(_intermcu_payload, nr) (uint16_t)(*((uint8_t*)_intermcu_payload+0+(2*(nr)))|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8)
87 
88 #define InterMcuSend_INTERMCU_RADIO(cmd) { \
89  InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_RADIO_ID, MSG_INTERMCU_RADIO_LENGTH);\
90  for (int i=0;i<RADIO_CONTROL_NB_CHANNEL;i++) { \
91  uint16_t _cmd = cmd[i]; \
92  InterMcuSend2ByAddr((uint8_t*)&_cmd);\
93  } \
94  InterMcuTrailer();\
95  }
96 
97 #define MSG_INTERMCU_FBW_ID 0x06
98 #define MSG_INTERMCU_FBW_MOD(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+0))
99 #define MSG_INTERMCU_FBW_STAT(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+1))
100 #define MSG_INTERMCU_FBW_ERR(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+2))
101 #define MSG_INTERMCU_FBW_VOLT(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+3)|*((uint8_t*)_intermcu_payload+1+3)<<8)
102 //FIXME: Current is now 4BYTES
103 #define MSG_INTERMCU_FBW_CURRENT(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+5)|*((uint8_t*)_intermcu_payload+1+5)<<8)
104 
105 #define InterMcuSend_INTERMCU_FBW(mod,stat,err,volt,current) { \
106  InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_FBW_ID, 7);\
107  uint8_t _mod = mod; InterMcuSend1ByAddr((uint8_t*)&_mod);\
108  uint8_t _stat = stat; InterMcuSend1ByAddr((uint8_t*)&_stat);\
109  uint8_t _err = err; InterMcuSend1ByAddr((uint8_t*)&_err);\
110  uint16_t _volt = volt; InterMcuSend2ByAddr((uint8_t*)&_volt);\
111  uint16_t _current = current; InterMcuSend2ByAddr((uint8_t*)&_current);\
112  InterMcuTrailer();\
113  }
114 
115 #define MSG_INTERMCU_TRIM_ID 0x07
116 #define MSG_INTERMCU_TRIM_ROLL(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+0)|*((uint8_t*)_intermcu_payload+1)<<8)
117 #define MSG_INTERMCU_TRIM_PITCH(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+2)|*((uint8_t*)_intermcu_payload+3)<<8)
118 
119 #define InterMcuSend_INTERMCU_TRIM(roll,pitch) { \
120  InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_TRIM_ID, 4);\
121  uint16_t _roll = roll; InterMcuSend2ByAddr((uint8_t*)&_roll);\
122  uint16_t _pitch = pitch; InterMcuSend2ByAddr((uint8_t*)&_pitch);\
123  InterMcuTrailer();\
124  }
125 
127 // PARSER
128 
129 /* parser status */
130 #define LINK_MCU_UNINIT 0
131 #define LINK_MCU_GOT_SYNC1 1
132 #define LINK_MCU_GOT_SYNC2 2
133 #define LINK_MCU_GOT_CLASS 3
134 #define LINK_MCU_GOT_ID 4
135 #define LINK_MCU_GOT_LEN1 5
136 #define LINK_MCU_GOT_LEN2 6
137 #define LINK_MCU_GOT_PAYLOAD 7
138 #define LINK_MCU_GOT_CHECKSUM1 8
139 
140 
141 #define INTERMCU_MAX_PAYLOAD 255
142 struct InterMcuData {
144  uint8_t msg_buf[INTERMCU_MAX_PAYLOAD] __attribute__((aligned));
147 
154 };
155 
157 
158 /* INTERMCU parsing */
159 void intermcu_parse(uint8_t c);
161 {
163  intermcu_data.ck_a += c;
165  }
166  switch (intermcu_data.status) {
167  case LINK_MCU_UNINIT:
168  if (c == INTERMCU_SYNC1) {
170  }
171  break;
172  case LINK_MCU_GOT_SYNC1:
173  if (c != INTERMCU_SYNC2) {
174  goto error;
175  }
176  intermcu_data.ck_a = 0;
177  intermcu_data.ck_b = 0;
179  break;
180  case LINK_MCU_GOT_SYNC2:
182  /* Previous message has not yet been parsed: discard this one */
183  goto error;
184  }
187  break;
188  case LINK_MCU_GOT_CLASS:
189  intermcu_data.msg_id = c;
191  break;
192  case LINK_MCU_GOT_ID:
193  intermcu_data.len = c;
195  break;
196  case LINK_MCU_GOT_LEN1:
197  intermcu_data.len |= (c << 8);
199  goto error;
200  }
203  break;
204  case LINK_MCU_GOT_LEN2:
209  }
210  break;
212  if (c != intermcu_data.ck_a) {
213  goto error;
214  }
216  break;
218  if (c != intermcu_data.ck_b) {
219  goto error;
220  }
222  goto restart;
223  break;
224  default:
225  goto error;
226  }
227  return;
228 error:
230 restart:
232  return;
233 }
234 
235 
236 
238 // USER
239 
240 
243 
244 inline void parse_mavpilot_msg(void);
245 
246 
247 #ifdef AP
249 
250 #define RC_OK 0
251 #define RC_LOST 1
252 #define RC_REALLY_LOST 2
253 
254 
255 static void send_commands(struct transport_tx *trans, struct link_device *dev)
256 {
257  pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands);
258 }
259 
260 
261 static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
262 {
263  uint8_t rc_status = 0;
264  uint8_t fbw_status = 0;
265  if (bit_is_set(fbw_state->status, STATUS_MODE_AUTO)) {
266  fbw_status = FBW_MODE_AUTO;
267  }
268  if (bit_is_set(fbw_state->status, STATUS_MODE_FAILSAFE)) {
269  fbw_status = FBW_MODE_FAILSAFE;
270  }
271  if (bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)) {
272  rc_status = RC_REALLY_LOST;
273  } else if (bit_is_set(fbw_state->status, RC_OK)) {
274  rc_status = RC_OK;
275  } else {
276  rc_status = RC_LOST;
277  }
278  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
279  &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->electrical.vsupply), &(fbw_state->electrical.current));
280 }
281 #endif
282 
283 void link_mcu_init(void)
284 {
288 #ifdef AP
289 #if PERIODIC_TELEMETRY
290  // If FBW has not telemetry, then AP can send some of the info
293 #endif
294 #endif
295 }
296 
298 {
301 #if COMMANDS_NB > 8
302 #error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED"
303 #endif
304 
305  for (int i = 0; i < COMMANDS_NB; i++) {
307  }
308 
309 #ifdef LINK_MCU_LED
310  LED_TOGGLE(LINK_MCU_LED);
311 #endif
312  inter_mcu_received_ap = true;
314 #if RADIO_CONTROL_NB_CHANNEL > 10
315 #error "INTERMCU UART CAN ONLY SEND 10 RADIO CHANNELS OR THE UART WILL BE OVERFILLED"
316 #endif
317 
318  for (int i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) {
320  }
321  } else if (intermcu_data.msg_id == MSG_INTERMCU_TRIM_ID) {
322  ap_state->command_roll_trim = ((pprz_t) MSG_INTERMCU_TRIM_ROLL(intermcu_data.msg_buf));
323  ap_state->command_pitch_trim = ((pprz_t) MSG_INTERMCU_TRIM_PITCH(intermcu_data.msg_buf));
324  } else if (intermcu_data.msg_id == MSG_INTERMCU_FBW_ID) {
328  fbw_state->electrical.vsupply = (float)(MSG_INTERMCU_FBW_VOLT(intermcu_data.msg_buf)) / 10.f;
329  fbw_state->electrical.current = (float)(MSG_INTERMCU_FBW_CURRENT(intermcu_data.msg_buf)) / 10.f;
330 
331 #ifdef LINK_MCU_LED
332  LED_TOGGLE(LINK_MCU_LED);
333 #endif
334  inter_mcu_received_fbw = true;
335  }
336  }
337 }
338 
339 
340 #ifdef AP
341 void link_mcu_send(void)
342 {
344  InterMcuSend_INTERMCU_TRIM(ap_state->command_roll_trim, ap_state->command_pitch_trim);
345 }
346 #endif
347 
348 #ifdef FBW
349 // 60 Hz
350 static uint8_t SixtyHzCounter = 0;
351 
352 void link_mcu_periodic_task(void)
353 {
354  SixtyHzCounter++;
355  if (SixtyHzCounter >= 3) {
356  // 20 Hz
357  SixtyHzCounter = 0;
358  inter_mcu_fill_fbw_state();
361  fbw_state->ppm_cpt,
362  fbw_state->status,
363  fbw_state->nb_err,
364  fbw_state->electrical.vsupply * 10.f,
365  fbw_state->electrical.current * 10.f);
366 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
368 #endif
369 
370  }
371 }
372 #endif
373 
375 {
376  /* A message has been received */
377  if (InterMcuBuffer()) {
378  while (InterMcuBuffer()) {
383  }
384  }
385  }
386 }
#define RC_LOST
Definition: radio_control.h:57
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
static struct fbw_status_t fbw_status
Definition: intermcu_ap.c:47
Periodic telemetry system header (includes downlink utility and generated code).
int16_t pprz_t
Definition: paparazzi.h:6
volatile bool inter_mcu_received_ap
Definition: inter_mcu.c:41
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:49
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
Hardware independent code for commands handling.
#define LED_TOGGLE(i)
Definition: led_hw.h:53
#define RC_REALLY_LOST
Definition: radio_control.h:58
#define RC_OK
Definition: radio_control.h:56
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
volatile bool inter_mcu_received_fbw
Definition: inter_mcu.c:40
unsigned char uint8_t
Definition: types.h:14
static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
Definition: main_fbw.c:325
static void send_commands(struct transport_tx *trans, struct link_device *dev)
arch independent LED (Light Emitting Diodes) API
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46