Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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) 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 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
43 #endif
44 
45 #if USE_GPS
46 #include "subsystems/gps.h"
47 #endif
48 #if defined GPS_DATALINK
50 #endif
51 
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 #if USE_NPS
62 bool_t datalink_enabled = TRUE;
63 #endif
64 
65 void dl_parse_msg(void)
66 {
67 
68  uint8_t msg_id = IdOfMsg(dl_buffer);
69  switch (msg_id) {
70 
71  case DL_PING: {
72  DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
73  }
74  break;
75 
76  case DL_SETTING : {
77  if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
78  uint8_t i = DL_SETTING_index(dl_buffer);
79  float var = DL_SETTING_value(dl_buffer);
80  DlSetting(i, var);
81  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
82  }
83  break;
84 
85  case DL_GET_SETTING : {
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  if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
96  nav_goto_block(DL_BLOCK_block_id(dl_buffer));
97  }
98  break;
99 
100  case DL_MOVE_WP : {
101  uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
102  if (ac_id != AC_ID) { break; }
104  uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
105  struct LlaCoor_i lla;
106  lla.lat = DL_MOVE_WP_lat(dl_buffer);
107  lla.lon = DL_MOVE_WP_lon(dl_buffer);
108  /* WP_alt from message is alt above MSL in mm
109  * lla.alt is above ellipsoid in mm
110  */
111  lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl +
113  waypoint_move_lla(wp_id, &lla);
114  }
115  }
116  break;
117 #endif /* USE_NAVIGATION */
118 #ifdef RADIO_CONTROL_TYPE_DATALINK
119  case DL_RC_3CH :
120 #ifdef RADIO_CONTROL_DATALINK_LED
121  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
122 #endif
124  DL_RC_3CH_throttle_mode(dl_buffer),
125  DL_RC_3CH_roll(dl_buffer),
126  DL_RC_3CH_pitch(dl_buffer));
127  break;
128  case DL_RC_4CH :
129  if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
130 #ifdef RADIO_CONTROL_DATALINK_LED
131  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
132 #endif
133  parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer),
134  DL_RC_4CH_throttle(dl_buffer),
135  DL_RC_4CH_roll(dl_buffer),
136  DL_RC_4CH_pitch(dl_buffer),
137  DL_RC_4CH_yaw(dl_buffer));
138  }
139  break;
140 #endif // RADIO_CONTROL_TYPE_DATALINK
141 #if defined GPS_DATALINK
142 #ifdef GPS_USE_DATALINK_SMALL
143  case DL_REMOTE_GPS_SMALL :
144  // Check if the GPS is for this AC
145  if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) {
146  break;
147  }
148 
149  parse_gps_datalink_small(
150  DL_REMOTE_GPS_SMALL_numsv(dl_buffer),
151  DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer),
152  DL_REMOTE_GPS_SMALL_speed_xy(dl_buffer));
153  break;
154 #endif
155  case DL_REMOTE_GPS :
156  // Check if the GPS is for this AC
157  if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }
158 
159  // Parse the GPS
161  DL_REMOTE_GPS_numsv(dl_buffer),
162  DL_REMOTE_GPS_ecef_x(dl_buffer),
163  DL_REMOTE_GPS_ecef_y(dl_buffer),
164  DL_REMOTE_GPS_ecef_z(dl_buffer),
165  DL_REMOTE_GPS_lat(dl_buffer),
166  DL_REMOTE_GPS_lon(dl_buffer),
167  DL_REMOTE_GPS_alt(dl_buffer),
168  DL_REMOTE_GPS_hmsl(dl_buffer),
169  DL_REMOTE_GPS_ecef_xd(dl_buffer),
170  DL_REMOTE_GPS_ecef_yd(dl_buffer),
171  DL_REMOTE_GPS_ecef_zd(dl_buffer),
172  DL_REMOTE_GPS_tow(dl_buffer),
173  DL_REMOTE_GPS_course(dl_buffer));
174  break;
175 #endif
176 #if USE_GPS
177  case DL_GPS_INJECT :
178  // Check if the GPS is for this AC
179  if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
180 
181  // GPS parse data
183  DL_GPS_INJECT_packet_id(dl_buffer),
184  DL_GPS_INJECT_data_length(dl_buffer),
185  DL_GPS_INJECT_data(dl_buffer)
186  );
187  break;
188 #endif
189 
190  case DL_GUIDED_SETPOINT_NED:
191  if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
192  uint8_t flags = DL_GUIDED_SETPOINT_NED_flags(dl_buffer);
193  float x = DL_GUIDED_SETPOINT_NED_x(dl_buffer);
194  float y = DL_GUIDED_SETPOINT_NED_y(dl_buffer);
195  float z = DL_GUIDED_SETPOINT_NED_z(dl_buffer);
196  float yaw = DL_GUIDED_SETPOINT_NED_yaw(dl_buffer);
197  switch (flags) {
198  case 0x00:
199  case 0x02:
200  /* local NED position setpoints */
201  autopilot_guided_goto_ned(x, y, z, yaw);
202  break;
203  case 0x01:
204  /* local NED offset position setpoints */
206  break;
207  case 0x03:
208  /* body NED offset position setpoints */
210  break;
211  default:
212  /* others not handled yet */
213  break;
214  }
215  break;
216  default:
217  break;
218  }
219  /* Parse modules datalink */
220  modules_parse_datalink(msg_id);
221 }
uint8_t ac_id
Definition: sim_ap.c:47
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
Autopilot modes.
vector in Latitude, Longitude and Altitude
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition: waypoints.c:180
#define LED_TOGGLE(i)
Definition: led_hw.h:44
bool_t autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
Definition: autopilot.c:516
#define TRUE
Definition: std.h:4
bool_t autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
Definition: autopilot.c:527
Device independent GPS code (interface)
uint16_t val[TCOUPLE_NB]
static bool_t stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:491
struct LlaCoor_i lla
Reference point in lla.
int32_t lon
in degrees*1e7
Paparazzi fixed point math for geodetic calculations.
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.
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
arch independent LED (Light Emitting Diodes) API
bool_t autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
Definition: autopilot.c:539
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:384
int32_t lat
in degrees*1e7
void nav_goto_block(uint8_t b)
struct State state
Definition: state.c:36