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) 2008-2009 Antoine Drouin <poinix@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 
32 
33 #include "generated/modules.h"
34 
35 #include "generated/settings.h"
37 #include "messages.h"
38 #include "dl_protocol.h"
39 #include "mcu_periph/uart.h"
40 
41 #ifdef BOOZ_FMS_TYPE
42 #include "booz_fms.h"
43 #endif
44 
45 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
47 #endif
48 
49 #if defined GPS_DATALINK
51 #endif
52 
54 
55 #include "math/pprz_geodetic_int.h"
56 #include "state.h"
57 #include "led.h"
58 
59 #define IdOfMsg(x) (x[1])
60 
61 void dl_parse_msg(void) {
62 
63  datalink_time = 0;
64 
65  uint8_t msg_id = IdOfMsg(dl_buffer);
66  switch (msg_id) {
67 
68  case DL_PING:
69  {
70  DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
71  }
72  break;
73 
74  case DL_SETTING :
75  {
76  if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break;
77  uint8_t i = DL_SETTING_index(dl_buffer);
78  float var = DL_SETTING_value(dl_buffer);
79  DlSetting(i, var);
80  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
81  }
82  break;
83 
84  case DL_GET_SETTING :
85  {
86  if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break;
87  uint8_t i = DL_GET_SETTING_index(dl_buffer);
88  float val = settings_get_value(i);
89  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
90  }
91  break;
92 
93 #if defined USE_NAVIGATION
94  case DL_BLOCK :
95  {
96  if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) break;
97  nav_goto_block(DL_BLOCK_block_id(dl_buffer));
98  }
99  break;
100 
101  case DL_MOVE_WP :
102  {
103  uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
104  if (ac_id != AC_ID) break;
106  uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
107  struct LlaCoor_i lla;
108  lla.lat = DL_MOVE_WP_lat(dl_buffer);
109  lla.lon = DL_MOVE_WP_lon(dl_buffer);
110  /* WP_alt from message is alt above MSL in cm
111  * lla.alt is above ellipsoid in mm
112  */
113  lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - state.ned_origin_i.hmsl +
115  nav_move_waypoint_lla(wp_id, &lla);
116  }
117  }
118  break;
119 #endif /* USE_NAVIGATION */
120 #ifdef RADIO_CONTROL_TYPE_DATALINK
121  case DL_RC_3CH :
122 #ifdef RADIO_CONTROL_DATALINK_LED
123  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
124 #endif
126  DL_RC_3CH_throttle_mode(dl_buffer),
127  DL_RC_3CH_roll(dl_buffer),
128  DL_RC_3CH_pitch(dl_buffer));
129  break;
130  case DL_RC_4CH :
131 #ifdef RADIO_CONTROL_DATALINK_LED
132  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
133 #endif
135  DL_RC_4CH_mode(dl_buffer),
136  DL_RC_4CH_throttle(dl_buffer),
137  DL_RC_4CH_roll(dl_buffer),
138  DL_RC_4CH_pitch(dl_buffer),
139  DL_RC_4CH_yaw(dl_buffer));
140  break;
141 #endif // RADIO_CONTROL_TYPE_DATALINK
142 #if defined GPS_DATALINK
143  case DL_REMOTE_GPS :
144  // Check if the GPS is for this AC
145  if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) break;
146 
147  // Parse the GPS
149  DL_REMOTE_GPS_numsv(dl_buffer),
150  DL_REMOTE_GPS_ecef_x(dl_buffer),
151  DL_REMOTE_GPS_ecef_y(dl_buffer),
152  DL_REMOTE_GPS_ecef_z(dl_buffer),
153  DL_REMOTE_GPS_lat(dl_buffer),
154  DL_REMOTE_GPS_lon(dl_buffer),
155  DL_REMOTE_GPS_alt(dl_buffer),
156  DL_REMOTE_GPS_hmsl(dl_buffer),
157  DL_REMOTE_GPS_ecef_xd(dl_buffer),
158  DL_REMOTE_GPS_ecef_yd(dl_buffer),
159  DL_REMOTE_GPS_ecef_zd(dl_buffer),
160  DL_REMOTE_GPS_tow(dl_buffer),
161  DL_REMOTE_GPS_course(dl_buffer));
162  break;
163 #endif
164  default:
165  break;
166  }
167  /* Parse modules datalink */
168  modules_parse_datalink(msg_id);
169 }
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t lat
in degrees*1e7
struct LlaCoor_i lla
Reference point in lla.
vector in Latitude, Longitude and Altitude
uint16_t val[TCOUPLE_NB]
static bool_t stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:489
int32_t alt
in millimeters above WGS84 reference ellipsoid
Paparazzi fixed point math for geodetic calculations.
#define LED_TOGGLE(i)
Definition: led_hw.h:30
uint16_t datalink_time
Definition: sim_ap.c:44
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
int32_t lon
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:59
arch independent LED (Light Emitting Diodes) API
uint8_t ac_id
Definition: jsbsim_hw.c:27
void nav_goto_block(uint8_t b)
struct State state
Definition: state.c:36