Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
datalink.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
28 #define DATALINK_C
29 
30 #define MODULES_DATALINK_C
31 
32 #include <inttypes.h>
33 #include <string.h>
35 
36 #include "generated/modules.h"
37 
38 #ifdef TRAFFIC_INFO
40 #endif // TRAFFIC_INFO
41 
42 #if defined NAV || defined WIND_INFO
43 #include "estimator.h"
44 #include "subsystems/nav.h"
45 #endif
46 
47 #if USE_JOYSTICK
48 #include "joystick.h"
49 #endif
50 
51 #ifdef HITL
52 #include "subsystems/gps.h"
53 #endif
54 
55 
57 #include "generated/settings.h"
59 
60 #ifndef DOWNLINK_DEVICE
61 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
62 #endif
63 #include "mcu_periph/uart.h"
65 #include "ap_downlink.h"
66 
67 #define MOfCm(_x) (((float)(_x))/100.)
68 
69 #define SenderIdOfMsg(x) (x[0])
70 #define IdOfMsg(x) (x[1])
71 
72 void dl_parse_msg(void) {
73  datalink_time = 0;
74  uint8_t msg_id = IdOfMsg(dl_buffer);
75 #if 0 // not ready yet
76  uint8_t sender_id = SenderIdOfMsg(dl_buffer);
77 
78  /* parse telemetry messages coming from other AC */
79  if (sender_id != 0) {
80  switch (msg_id) {
81 #ifdef TCAS
82  case DL_TCAS_RA:
83  {
84  if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) {
85  uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer);
86  tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer);
87  }
88  }
89 #endif
90  }
91  return;
92  }
93 #endif
94 
95  if (msg_id == DL_PING) {
96  DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice)
97  } else
98 #ifdef TRAFFIC_INFO
99  if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) {
100  uint8_t id = DL_ACINFO_ac_id(dl_buffer);
101  float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
102  float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
103  float a = MOfCm(DL_ACINFO_alt(dl_buffer));
104  float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
105  float s = MOfCm(DL_ACINFO_speed(dl_buffer));
106  float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
107  uint32_t t = DL_ACINFO_itow(dl_buffer);
108  SetAcInfo(id, ux, uy, c, a, s, cl, t);
109  } else
110 #endif
111 #ifdef NAV
112  if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
113  uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
114  float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
115 
116  /* Computes from (lat, long) in the referenced UTM zone */
117  struct LlaCoor_f lla;
118  lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
119  lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
120  struct UtmCoor_f utm;
121  utm.zone = nav_utm_zone0;
122  utm_of_lla_f(&utm, &lla);
123  nav_move_waypoint(wp_id, utm.east, utm.north, a);
124 
125  /* Waypoint range is limited. Computes the UTM pos back from the relative
126  coordinates */
127  utm.east = waypoints[wp_id].x + nav_utm_east0;
128  utm.north = waypoints[wp_id].y + nav_utm_north0;
129  DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
130  } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) {
131  nav_goto_block(DL_BLOCK_block_id(dl_buffer));
133  } else
134 #endif
135 #ifdef WIND_INFO
136  if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
137  wind_east = DL_WIND_INFO_east(dl_buffer);
138  wind_north = DL_WIND_INFO_north(dl_buffer);
139 #if !USE_AIRSPEED
140  estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer);
141 #endif
142 #ifdef WIND_INFO_RET
143  DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind_east, &wind_north, &estimator_airspeed);
144 #endif
145  } else
146 #endif
148 #ifdef HITL
149 
150  if (msg_id == DL_HITL_INFRARED) {
152  infrared.roll = DL_HITL_INFRARED_roll(dl_buffer);
153  infrared.pitch = DL_HITL_INFRARED_pitch(dl_buffer);
154  infrared.top = DL_HITL_INFRARED_top(dl_buffer);
155  } else if (msg_id == DL_HITL_UBX) {
157  if (gps_msg_received) {
158  gps_nb_ovrn++;
159  } else {
160  ubx_class = DL_HITL_UBX_class(dl_buffer);
161  ubx_id = DL_HITL_UBX_id(dl_buffer);
162  uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer);
163  uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer);
164  memcpy(ubx_msg_buf, ubx_payload, l);
165  gps_msg_received = TRUE;
166  }
167  } else
168 #endif
169 #ifdef DlSetting
170  if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
171  uint8_t i = DL_SETTING_index(dl_buffer);
172  float val = DL_SETTING_value(dl_buffer);
173  DlSetting(i, val);
174  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
175  } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
176  uint8_t i = DL_GET_SETTING_index(dl_buffer);
177  float val = settings_get_value(i);
178  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
179  } else
180 #endif
181 #if USE_JOYSTICK
182  if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
183  JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer),
184  DL_JOYSTICK_RAW_pitch(dl_buffer),
185  DL_JOYSTICK_RAW_throttle(dl_buffer));
186  } else
187 #endif // USE_JOYSTICK
188 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
189  if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
190 #ifdef RADIO_CONTROL_DATALINK_LED
191  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
192 #endif
194  DL_RC_3CH_throttle_mode(dl_buffer),
195  DL_RC_3CH_roll(dl_buffer),
196  DL_RC_3CH_pitch(dl_buffer));
197  } else
198  if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
199 #ifdef RADIO_CONTROL_DATALINK_LED
200  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
201 #endif
203  DL_RC_4CH_mode(dl_buffer),
204  DL_RC_4CH_throttle(dl_buffer),
205  DL_RC_4CH_roll(dl_buffer),
206  DL_RC_4CH_pitch(dl_buffer),
207  DL_RC_4CH_yaw(dl_buffer));
208  } else
209 #endif // RC_DATALINK
210  { /* Last else */
211  /* Parse modules datalink */
212  modules_parse_datalink(msg_id);
213  }
214 }
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
struct Infrared infrared
Definition: infrared.c:36
enum tcas_resolve resolve
Definition: tcas.h:50
#define JoystickHandeDatalink(_roll_int8, _pitch_int8, _throttle_int8)
Definition: joystick.h:10
uint8_t the_acs_id[NB_ACS_ID]
Definition: traffic_info.c:34
uint8_t nav_utm_zone0
Definition: common_nav.c:43
float lat
in radians
vector in Latitude, Longitude and Altitude
struct tcas_ac_status tcas_acs_status[NB_ACS]
Definition: tcas.c:46
Paparazzi floating point math for geodetic calculations.
uint8_t zone
UTM zone number.
int32_t nav_utm_north0
Definition: common_nav.c:42
float wind_east
Definition: estimator.c:68
float north
in meters
Device independent GPS code (interface)
uint16_t val[TCOUPLE_NB]
float estimator_airspeed
m/s
Definition: estimator.c:69
int16_t roll
Definition: infrared.h:131
position in UTM coordinates Units: meters
unsigned long uint32_t
Definition: types.h:18
#define LED_TOGGLE(i)
Definition: led_hw.h:30
#define SetAcInfo(_id, _utm_x, _utm_y, _course, _alt, _gspeed, _climb, _itow)
Definition: traffic_info.h:52
#define TRUE
Definition: imu_chimu.h:144
uint16_t datalink_time
Definition: sim_ap.c:46
unsigned char uint8_t
Definition: types.h:14
int32_t nav_utm_east0
Definition: common_nav.c:41
State estimation, fusioning sensors.
float wind_north
Definition: estimator.c:68
float lon
in radians
static struct point c
Definition: discsurvey.c:13
int16_t pitch
Definition: infrared.h:132
float east
in meters
void nav_goto_block(uint8_t b)
int16_t top
Definition: infrared.h:133
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
Informations relative to the other aircrafts.
uint8_t gps_nb_ovrn
Definition: jsbsim_hw.c:19