Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
datalink.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 #define DATALINK_C
25 #define MODULES_DATALINK_C
26 
28 
29 #include "generated/modules.h"
30 
31 #include "generated/settings.h"
33 #include "messages.h"
34 #include "dl_protocol.h"
35 #include "mcu_periph/uart.h"
36 
37 #ifdef BOOZ_FMS_TYPE
38 #include "booz_fms.h"
39 #endif
40 
42 
43 #include "math/pprz_geodetic_int.h"
44 #include "subsystems/ins.h"
45 
46 #define IdOfMsg(x) (x[1])
47 
48 void dl_parse_msg(void) {
49 
50  datalink_time = 0;
51 
52  uint8_t msg_id = IdOfMsg(dl_buffer);
53  switch (msg_id) {
54 
55  case DL_PING:
56  {
57  DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
58  }
59  break;
60 
61  case DL_SETTING :
62  {
63  if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break;
64  uint8_t i = DL_SETTING_index(dl_buffer);
65  float var = DL_SETTING_value(dl_buffer);
66  DlSetting(i, var);
67  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
68  }
69  break;
70 
71  case DL_GET_SETTING :
72  {
73  if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break;
74  uint8_t i = DL_GET_SETTING_index(dl_buffer);
75  float val = settings_get_value(i);
76  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
77  }
78  break;
79 
80 #if defined USE_NAVIGATION
81  case DL_BLOCK :
82  {
83  if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) break;
84  nav_goto_block(DL_BLOCK_block_id(dl_buffer));
85  }
86  break;
87 
88  case DL_MOVE_WP :
89  {
90  uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
91  if (ac_id != AC_ID) break;
92  uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
93  struct LlaCoor_i lla;
94  struct EnuCoor_i enu;
95  lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
96  lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
97  /* WP_alt is in cm, lla.alt in mm */
98  lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
99  enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
100  enu.x = POS_BFP_OF_REAL(enu.x)/100;
101  enu.y = POS_BFP_OF_REAL(enu.y)/100;
102  enu.z = POS_BFP_OF_REAL(enu.z)/100;
103  VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
104  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
105  }
106  break;
107 #endif /* USE_NAVIGATION */
108  default:
109  break;
110  }
111  /* Parse modules datalink */
112  modules_parse_datalink(msg_id);
113 }
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t lat
in radians*1e7
struct LtpDef_i ins_ltp_def
Definition: ins.c:53
Device independent INS code.
struct LlaCoor_i lla
Reference point in lla.
vector in Latitude, Longitude and Altitude
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.
uint16_t datalink_time
Definition: sim_ap.c:46
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:99
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 ac_id
Definition: jsbsim_hw.c:28
vector in East North Up coordinates
void nav_goto_block(uint8_t b)
#define POS_BFP_OF_REAL(_af)