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) 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 
50 
51 #include "math/pprz_geodetic_int.h"
52 #include "subsystems/ins.h"
53 
54 #define IdOfMsg(x) (x[1])
55 
56 void dl_parse_msg(void) {
57 
58  datalink_time = 0;
59 
60  uint8_t msg_id = IdOfMsg(dl_buffer);
61  switch (msg_id) {
62 
63  case DL_PING:
64  {
65  DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
66  }
67  break;
68 
69  case DL_SETTING :
70  {
71  if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break;
72  uint8_t i = DL_SETTING_index(dl_buffer);
73  float var = DL_SETTING_value(dl_buffer);
74  DlSetting(i, var);
75  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
76  }
77  break;
78 
79  case DL_GET_SETTING :
80  {
81  if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break;
82  uint8_t i = DL_GET_SETTING_index(dl_buffer);
83  float val = settings_get_value(i);
84  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
85  }
86  break;
87 
88 #if defined USE_NAVIGATION
89  case DL_BLOCK :
90  {
91  if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) break;
92  nav_goto_block(DL_BLOCK_block_id(dl_buffer));
93  }
94  break;
95 
96  case DL_MOVE_WP :
97  {
98  uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
99  if (ac_id != AC_ID) break;
100  uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
101  struct LlaCoor_i lla;
102  struct EnuCoor_i enu;
103  lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
104  lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
105  /* WP_alt is in cm, lla.alt in mm */
106  lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
107  enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
108  enu.x = POS_BFP_OF_REAL(enu.x)/100;
109  enu.y = POS_BFP_OF_REAL(enu.y)/100;
110  enu.z = POS_BFP_OF_REAL(enu.z)/100;
111  VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
112  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
113  }
114  break;
115 #endif /* USE_NAVIGATION */
116 #ifdef RADIO_CONTROL_TYPE_DATALINK
117  case DL_RC_3CH :
118 #ifdef RADIO_CONTROL_DATALINK_LED
119  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
120 #endif
122  DL_RC_3CH_throttle_mode(dl_buffer),
123  DL_RC_3CH_roll(dl_buffer),
124  DL_RC_3CH_pitch(dl_buffer));
125  break;
126  case DL_RC_4CH :
127 #ifdef RADIO_CONTROL_DATALINK_LED
128  LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
129 #endif
131  DL_RC_4CH_mode(dl_buffer),
132  DL_RC_4CH_throttle(dl_buffer),
133  DL_RC_4CH_roll(dl_buffer),
134  DL_RC_4CH_pitch(dl_buffer),
135  DL_RC_4CH_yaw(dl_buffer));
136  break;
137 #endif // RADIO_CONTROL_TYPE_DATALINK
138  default:
139  break;
140  }
141  /* Parse modules datalink */
142  modules_parse_datalink(msg_id);
143 }
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t lat
in radians*1e7
Integrated Navigation System interface.
struct LlaCoor_i lla
Reference point in lla.
vector in Latitude, Longitude and Altitude
int32_t z
Up.
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
#define INT32_RAD_OF_DEG(_deg)
uint16_t val[TCOUPLE_NB]
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
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:97
unsigned char uint8_t
Definition: types.h:14
int32_t lon
in radians*1e7
int32_t hmsl
Height above mean sea level in mm.
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:59
struct LtpDef_i ins_ltp_def
Ins implementation state (fixed point)
Definition: ins_ardrone2.c:51
int32_t y
North.
uint8_t ac_id
Definition: jsbsim_hw.c:27
vector in East North Up coordinates
void nav_goto_block(uint8_t b)
int32_t x
East.
#define POS_BFP_OF_REAL(_af)