Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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"
45 #endif
46 
47 #ifdef HITL
48 #include "subsystems/gps.h"
49 #endif
50 
51 
53 #include "generated/settings.h"
55 
56 #include "mcu_periph/uart.h"
58 
59 
60 #if USE_JOYSTICK
62 #include "autopilot.h"
64 #define JoystickHandeDatalink(_roll_int8, _pitch_int8, _throttle_int8) { \
65  if (pprz_mode == PPRZ_MODE_AUTO2 && nav_block == joystick_block) { \
66  h_ctl_roll_setpoint = _roll_int8 * (AUTO1_MAX_ROLL / 0x7f); \
67  h_ctl_pitch_setpoint = _pitch_int8 * (AUTO1_MAX_PITCH / 0x7f); \
68  v_ctl_throttle_setpoint = (MAX_PPRZ/0x7f) * _throttle_int8; \
69  } \
70  }
71 #endif
72 
73 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
75 #endif
76 
77 #define MOfCm(_x) (((float)(_x))/100.)
78 
79 #define SenderIdOfMsg(x) (x[0])
80 #define IdOfMsg(x) (x[1])
81 
82 void dl_parse_msg(void) {
83  datalink_time = 0;
84  uint8_t msg_id = IdOfMsg(dl_buffer);
85 #if 0 // not ready yet
86  uint8_t sender_id = SenderIdOfMsg(dl_buffer);
87 
88  /* parse telemetry messages coming from other AC */
89  if (sender_id != 0) {
90  switch (msg_id) {
91 #ifdef TCAS
92  case DL_TCAS_RA:
93  {
94  if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) {
95  uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer);
96  tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer);
97  }
98  }
99 #endif
100  }
101  return;
102  }
103 #endif
104 
105  if (msg_id == DL_PING) {
106  DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice)
107  } else
108 #ifdef TRAFFIC_INFO
109  if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) {
110  uint8_t id = DL_ACINFO_ac_id(dl_buffer);
111  float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
112  float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
113  float a = MOfCm(DL_ACINFO_alt(dl_buffer));
114  float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
115  float s = MOfCm(DL_ACINFO_speed(dl_buffer));
116  float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
117  uint32_t t = DL_ACINFO_itow(dl_buffer);
118  SetAcInfo(id, ux, uy, c, a, s, cl, t);
119  } else
120 #endif
121 #ifdef NAV
122  if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
123  uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
124  float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
125 
126  /* Computes from (lat, long) in the referenced UTM zone */
127  struct LlaCoor_f lla;
128  lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
129  lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
130  struct UtmCoor_f utm;
131  utm.zone = nav_utm_zone0;
132  utm_of_lla_f(&utm, &lla);
133  nav_move_waypoint(wp_id, utm.east, utm.north, a);
134 
135  /* Waypoint range is limited. Computes the UTM pos back from the relative
136  coordinates */
137  utm.east = waypoints[wp_id].x + nav_utm_east0;
138  utm.north = waypoints[wp_id].y + nav_utm_north0;
139  DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
140  } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) {
141  nav_goto_block(DL_BLOCK_block_id(dl_buffer));
143  } else
144 #endif
145 #ifdef WIND_INFO
146  if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
147  struct FloatVect2 wind;
148  wind.x = DL_WIND_INFO_north(dl_buffer);
149  wind.y = DL_WIND_INFO_east(dl_buffer);
151 #if !USE_AIRSPEED
152  float airspeed = DL_WIND_INFO_airspeed(dl_buffer);
153  stateSetAirspeed_f(&airspeed);
154 #endif
155 #ifdef WIND_INFO_RET
156  DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, stateGetAirspeed_f());
157 #endif
158  } else
159 #endif
161 #ifdef HITL
162 
163  if (msg_id == DL_HITL_INFRARED) {
165  infrared.roll = DL_HITL_INFRARED_roll(dl_buffer);
166  infrared.pitch = DL_HITL_INFRARED_pitch(dl_buffer);
167  infrared.top = DL_HITL_INFRARED_top(dl_buffer);
168  } else if (msg_id == DL_HITL_UBX) {
170  if (gps_msg_received) {
171  gps_nb_ovrn++;
172  } else {
173  ubx_class = DL_HITL_UBX_class(dl_buffer);
174  ubx_id = DL_HITL_UBX_id(dl_buffer);
175  uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer);
176  uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer);
177  memcpy(ubx_msg_buf, ubx_payload, l);
178  gps_msg_received = TRUE;
179  }
180  } else
181 #endif
182 #ifdef DlSetting
183  if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
184  uint8_t i = DL_SETTING_index(dl_buffer);
185  float val = DL_SETTING_value(dl_buffer);
186  DlSetting(i, val);
187  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
188  } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
189  uint8_t i = DL_GET_SETTING_index(dl_buffer);
190  float val = settings_get_value(i);
191  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
192  } else
193 #endif
194 #if USE_JOYSTICK
195  if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
196  JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer),
197  DL_JOYSTICK_RAW_pitch(dl_buffer),
198  DL_JOYSTICK_RAW_throttle(dl_buffer));
199  } else
200 #endif // USE_JOYSTICK
201 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
202  if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
203 #ifdef RADIO_CONTROL_DATALINK_LED
204  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
205 #endif
207  DL_RC_3CH_throttle_mode(dl_buffer),
208  DL_RC_3CH_roll(dl_buffer),
209  DL_RC_3CH_pitch(dl_buffer));
210  } else
211  if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
212 #ifdef RADIO_CONTROL_DATALINK_LED
213  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
214 #endif
216  DL_RC_4CH_mode(dl_buffer),
217  DL_RC_4CH_throttle(dl_buffer),
218  DL_RC_4CH_roll(dl_buffer),
219  DL_RC_4CH_pitch(dl_buffer),
220  DL_RC_4CH_yaw(dl_buffer));
221  } else
222 #endif // RC_DATALINK
223  { /* Last else */
224  /* Parse modules datalink */
225  modules_parse_datalink(msg_id);
226  }
227 }
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
Periodic telemetry system header (includes downlink utility and generated code).
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:44
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
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