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
datalink.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Kirk Scheper <kirkscheper@gmail.com>
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 
28 #define DATALINK_C
29 #define MODULES_DATALINK_C
30 
31 #include "datalink.h"
33 
34 #include "generated/modules.h"
35 #include "generated/settings.h"
36 
37 #include "pprzlink/messages.h"
38 
39 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
41 #endif
42 
43 #if USE_GPS
44 #include "subsystems/gps.h"
45 #endif
46 
47 #ifdef RADIO_CONTROL_DATALINK_LED
48 #include "led.h"
49 #endif
50 
51 #if USE_NPS
52 bool datalink_enabled = true;
53 #endif
54 
55 
56 void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
57 {
58  uint8_t sender_id = SenderIdOfPprzMsg(buf);
59  uint8_t msg_id = IdOfPprzMsg(buf);
60 
61  /* parse telemetry messages coming from other AC */
62  if (sender_id != 0) {
63  switch (msg_id) {
64  default: {
65  break;
66  }
67  }
68  } else {
69 #if PPRZLINK_DEFAULT_VER == 2
70  // Check that the message is really a datalink message
71  if (pprzlink_get_msg_class_id(buf) == DL_datalink_CLASS_ID) {
72 #endif
73  /* parse datalink messages coming from ground station */
74  switch (msg_id) {
75  case DL_PING: {
76 #if PPRZLINK_DEFAULT_VER == 2
77  // Reply to the sender of the message
78  struct pprzlink_msg msg;
79  msg.trans = trans;
80  msg.dev = dev;
81  msg.sender_id = AC_ID;
82  msg.receiver_id = sender_id;
83  msg.component_id = 0;
84  pprzlink_msg_send_PONG(&msg);
85 #else
86  pprz_msg_send_PONG(trans, dev, AC_ID);
87 #endif
88  }
89  break;
90 
91  case DL_SETTING : {
92  if (DL_SETTING_ac_id(buf) != AC_ID) { break; }
93  uint8_t i = DL_SETTING_index(buf);
94  float var = DL_SETTING_value(buf);
95  DlSetting(i, var);
96 #if PPRZLINK_DEFAULT_VER == 2
97  // Reply to the sender of the message
98  struct pprzlink_msg msg;
99  msg.trans = trans;
100  msg.dev = dev;
101  msg.sender_id = AC_ID;
102  msg.receiver_id = sender_id;
103  msg.component_id = 0;
104  pprzlink_msg_send_DL_VALUE(&msg, &i, &var);
105 #else
106  pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &var);
107 #endif
108  }
109  break;
110 
111  case DL_GET_SETTING : {
112  if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
113  uint8_t i = DL_GET_SETTING_index(buf);
114  float val = settings_get_value(i);
115 #if PPRZLINK_DEFAULT_VER == 2
116  // Reply to the sender of the message
117  struct pprzlink_msg msg;
118  msg.trans = trans;
119  msg.dev = dev;
120  msg.sender_id = AC_ID;
121  msg.receiver_id = sender_id;
122  msg.component_id = 0;
123  pprzlink_msg_send_DL_VALUE(&msg, &i, &val);
124 #else
125  pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &val);
126 #endif
127  }
128  break;
129 
130 #ifdef RADIO_CONTROL_TYPE_DATALINK
131  case DL_RC_3CH :
132 #ifdef RADIO_CONTROL_DATALINK_LED
133  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
134 #endif
136  DL_RC_3CH_throttle_mode(buf),
137  DL_RC_3CH_roll(buf),
138  DL_RC_3CH_pitch(buf));
139  break;
140  case DL_RC_4CH :
141  if (DL_RC_4CH_ac_id(buf) == AC_ID) {
142 #ifdef RADIO_CONTROL_DATALINK_LED
143  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
144 #endif
145  parse_rc_4ch_datalink(DL_RC_4CH_mode(buf),
146  DL_RC_4CH_throttle(buf),
147  DL_RC_4CH_roll(buf),
148  DL_RC_4CH_pitch(buf),
149  DL_RC_4CH_yaw(buf));
150  }
151  break;
152 #endif // RADIO_CONTROL_TYPE_DATALINK
153 
154 #if USE_GPS
155  case DL_GPS_INJECT : {
156  // Check if the GPS is for this AC
157  if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; }
158 
159  // GPS parse data
161  DL_GPS_INJECT_packet_id(buf),
162  DL_GPS_INJECT_data_length(buf),
163  DL_GPS_INJECT_data(buf)
164  );
165  }
166  break;
167 #if USE_GPS_UBX_RTCM
168  case DL_RTCM_INJECT : {
169  // GPS parse data
170  gps_inject_data(DL_RTCM_INJECT_packet_id(buf),
171  DL_RTCM_INJECT_data_length(buf),
172  DL_RTCM_INJECT_data(buf));
173  }
174  break;
175 #endif // USE_GPS_UBX_RTCM
176 #endif // USE_GPS
177 
178  default:
179  break;
180  }
181 #if PPRZLINK_DEFAULT_VER == 2
182  }
183 #endif
184  }
185  /* Parse firmware specific datalink */
186  firmware_parse_msg(dev, trans, buf);
187 
188  /* Parse modules datalink */
189  modules_parse_datalink(msg_id, dev, trans, buf);
190 }
191 
192 /* default empty WEAK implementation for firmwares without an extra firmware_parse_msg */
193 WEAK void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf __attribute__((unused)))
194 {
195 }
Device independent GPS code (interface)
uint16_t val[TCOUPLE_NB]
#define LED_TOGGLE(i)
Definition: led_hw.h:52
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
unsigned char uint8_t
Definition: types.h:14
arch independent LED (Light Emitting Diodes) API
void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
Override the default GPS packet injector to inject the data trough UART.
Definition: gps_piksi.c:408