Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
datalink.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
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 
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 "state.h"
44 #include "subsystems/nav.h"
45 #endif
46 
47 #ifdef HITL
48 #include "subsystems/gps.h"
49 #endif
50 
51 
53 #include "generated/settings.h"
55 
56 #ifndef DOWNLINK_DEVICE
57 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
58 #endif
59 #include "mcu_periph/uart.h"
61 #include "ap_downlink.h"
62 
63 
64 #if USE_JOYSTICK
66 #include "autopilot.h"
68 #define JoystickHandeDatalink(_roll_int8, _pitch_int8, _throttle_int8) { \
69  if (pprz_mode == PPRZ_MODE_AUTO2 && nav_block == joystick_block) { \
70  h_ctl_roll_setpoint = _roll_int8 * (AUTO1_MAX_ROLL / 0x7f); \
71  h_ctl_pitch_setpoint = _pitch_int8 * (AUTO1_MAX_PITCH / 0x7f); \
72  v_ctl_throttle_setpoint = (MAX_PPRZ/0x7f) * _throttle_int8; \
73  } \
74  }
75 #endif
76 
77 
78 #define MOfCm(_x) (((float)(_x))/100.)
79 
80 #define SenderIdOfMsg(x) (x[0])
81 #define IdOfMsg(x) (x[1])
82 
83 void dl_parse_msg(void) {
84  datalink_time = 0;
85  uint8_t msg_id = IdOfMsg(dl_buffer);
86 #if 0 // not ready yet
87  uint8_t sender_id = SenderIdOfMsg(dl_buffer);
88 
89  /* parse telemetry messages coming from other AC */
90  if (sender_id != 0) {
91  switch (msg_id) {
92 #ifdef TCAS
93  case DL_TCAS_RA:
94  {
95  if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) {
96  uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer);
97  tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer);
98  }
99  }
100 #endif
101  }
102  return;
103  }
104 #endif
105 
106  if (msg_id == DL_PING) {
107  DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice)
108  } else
109 #ifdef TRAFFIC_INFO
110  if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) {
111  uint8_t id = DL_ACINFO_ac_id(dl_buffer);
112  float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
113  float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
114  float a = MOfCm(DL_ACINFO_alt(dl_buffer));
115  float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
116  float s = MOfCm(DL_ACINFO_speed(dl_buffer));
117  float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
118  uint32_t t = DL_ACINFO_itow(dl_buffer);
119  SetAcInfo(id, ux, uy, c, a, s, cl, t);
120  } else
121 #endif
122 #ifdef NAV
123  if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
124  uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
125  float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
126 
127  /* Computes from (lat, long) in the referenced UTM zone */
128  struct LlaCoor_f lla;
129  lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
130  lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
131  struct UtmCoor_f utm;
132  utm.zone = nav_utm_zone0;
133  utm_of_lla_f(&utm, &lla);
134  nav_move_waypoint(wp_id, utm.east, utm.north, a);
135 
136  /* Waypoint range is limited. Computes the UTM pos back from the relative
137  coordinates */
138  utm.east = waypoints[wp_id].x + nav_utm_east0;
139  utm.north = waypoints[wp_id].y + nav_utm_north0;
140  DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
141  } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) {
142  nav_goto_block(DL_BLOCK_block_id(dl_buffer));
144  } else
145 #endif
146 #ifdef WIND_INFO
147  if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
148  struct FloatVect2 wind;
149  wind.x = DL_WIND_INFO_north(dl_buffer);
150  wind.y = DL_WIND_INFO_east(dl_buffer);
152 #if !USE_AIRSPEED
153  float airspeed = DL_WIND_INFO_airspeed(dl_buffer);
154  stateSetAirspeed_f(&airspeed);
155 #endif
156 #ifdef WIND_INFO_RET
157  DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, stateGetAirspeed_f());
158 #endif
159  } else
160 #endif
162 #ifdef HITL
163 
164  if (msg_id == DL_HITL_INFRARED) {
166  infrared.roll = DL_HITL_INFRARED_roll(dl_buffer);
167  infrared.pitch = DL_HITL_INFRARED_pitch(dl_buffer);
168  infrared.top = DL_HITL_INFRARED_top(dl_buffer);
169  } else if (msg_id == DL_HITL_UBX) {
171  if (gps_msg_received) {
172  gps_nb_ovrn++;
173  } else {
174  ubx_class = DL_HITL_UBX_class(dl_buffer);
175  ubx_id = DL_HITL_UBX_id(dl_buffer);
176  uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer);
177  uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer);
178  memcpy(ubx_msg_buf, ubx_payload, l);
179  gps_msg_received = TRUE;
180  }
181  } else
182 #endif
183 #ifdef DlSetting
184  if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
185  uint8_t i = DL_SETTING_index(dl_buffer);
186  float val = DL_SETTING_value(dl_buffer);
187  DlSetting(i, val);
188  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
189  } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
190  uint8_t i = DL_GET_SETTING_index(dl_buffer);
191  float val = settings_get_value(i);
192  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
193  } else
194 #endif
195 #if USE_JOYSTICK
196  if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
197  JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer),
198  DL_JOYSTICK_RAW_pitch(dl_buffer),
199  DL_JOYSTICK_RAW_throttle(dl_buffer));
200  } else
201 #endif // USE_JOYSTICK
202 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
203  if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
204 #ifdef RADIO_CONTROL_DATALINK_LED
205  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
206 #endif
208  DL_RC_3CH_throttle_mode(dl_buffer),
209  DL_RC_3CH_roll(dl_buffer),
210  DL_RC_3CH_pitch(dl_buffer));
211  } else
212  if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
213 #ifdef RADIO_CONTROL_DATALINK_LED
214  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
215 #endif
217  DL_RC_4CH_mode(dl_buffer),
218  DL_RC_4CH_throttle(dl_buffer),
219  DL_RC_4CH_roll(dl_buffer),
220  DL_RC_4CH_pitch(dl_buffer),
221  DL_RC_4CH_yaw(dl_buffer));
222  } else
223 #endif // RC_DATALINK
224  { /* Last else */
225  /* Parse modules datalink */
226  modules_parse_datalink(msg_id);
227  }
228 }
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
struct Infrared infrared
Definition: infrared.c:34
enum tcas_resolve resolve
Definition: tcas.h:48
uint8_t the_acs_id[NB_ACS_ID]
Definition: traffic_info.c:34
uint8_t nav_utm_zone0
Definition: common_nav.c:44
float lat
in radians
vector in Latitude, Longitude and Altitude
static float * stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1199
Fixed wing horizontal control.
struct tcas_ac_status tcas_acs_status[NB_ACS]
Definition: tcas.c:47
Paparazzi floating point math for geodetic calculations.
uint8_t zone
UTM zone number.
int32_t nav_utm_north0
Definition: common_nav.c:43
uint8_t joystick_block
float north
in meters
Device independent GPS code (interface)
uint16_t val[TCOUPLE_NB]
int16_t roll
Definition: infrared.h:129
position in UTM coordinates Units: meters
unsigned long uint32_t
Definition: types.h:18
static void stateSetAirspeed_f(float *airspeed)
Set airspeed (float).
Definition: state.h:1152
#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:44
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
int32_t nav_utm_east0
Definition: common_nav.c:42
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:59
int32_t y
North.
float lon
in radians
static struct point c
Definition: discsurvey.c:39
int16_t pitch
Definition: infrared.h:130
float east
in meters
void nav_goto_block(uint8_t b)
int16_t top
Definition: infrared.h:131
int32_t x
East.
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
Information relative to the other aircrafts.
uint8_t gps_nb_ovrn
Definition: jsbsim_hw.c:19
static void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition: state.h:1144