Paparazzi UAS  v5.15_devel-109-gee85905
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
fixedwing_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 
30 
31 #include "pprzlink/messages.h"
32 #include "pprzlink/dl_protocol.h"
33 
34 #if defined NAV || defined WIND_INFO
35 #include "state.h"
36 #endif
37 
38 #ifdef NAV
42 #endif
43 
44 #ifdef HITL
45 #include "subsystems/gps.h"
46 #endif
47 
48 #define MOfMM(_x) (((float)(_x))/1000.)
49 
50 void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
51 {
52  uint8_t msg_id = IdOfPprzMsg(buf);
53 
54  /* parse telemetry messages coming from ground station */
55  switch (msg_id) {
56 
57 #ifdef NAV
58  case DL_BLOCK: {
59  if (DL_BLOCK_ac_id(buf) != AC_ID) { break; }
60  nav_goto_block(DL_BLOCK_block_id(buf));
61  SEND_NAVIGATION(trans, dev);
62  }
63  break;
64 
65  case DL_MOVE_WP: {
66  if (DL_MOVE_WP_ac_id(buf) != AC_ID) { break; }
67  uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
68 
69  /* Computes from (lat, long) in the referenced UTM zone */
70  struct LlaCoor_f lla;
71  lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(buf) / 1e7));
72  lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(buf) / 1e7));
73  lla.alt = MOfMM(DL_MOVE_WP_alt(buf));
74  struct UtmCoor_f utm;
75  utm.zone = nav_utm_zone0;
76  utm_of_lla_f(&utm, &lla);
77  nav_move_waypoint(wp_id, utm.east, utm.north, utm.alt);
78 
79  /* Waypoint range is limited. Computes the UTM pos back from the relative
80  coordinates */
81  utm.east = waypoints[wp_id].x + nav_utm_east0;
82  utm.north = waypoints[wp_id].y + nav_utm_north0;
83  pprz_msg_send_WP_MOVED(trans, dev, AC_ID, &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0);
84  }
85  break;
86 #endif
88 #ifdef WIND_INFO
89  case DL_WIND_INFO: {
90  if (DL_WIND_INFO_ac_id(buf) != AC_ID) { break; }
91  uint8_t flags = DL_WIND_INFO_flags(buf);
92  struct FloatVect2 wind = { 0.f, 0.f };
93  float upwind = 0.f;
94  if (bit_is_set(flags, 0)) {
95  wind.x = DL_WIND_INFO_north(buf);
96  wind.y = DL_WIND_INFO_east(buf);
98  }
99  if (bit_is_set(flags, 1)) {
100  upwind = DL_WIND_INFO_up(buf);
102  }
103 #if !USE_AIRSPEED
104  if (bit_is_set(flags, 2)) {
105  stateSetAirspeed_f(DL_WIND_INFO_airspeed(buf));
106  }
107 #endif
108 #ifdef WIND_INFO_RET
109  float airspeed = stateGetAirspeed_f();
110  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &wind.y, &wind.x, &upwind, &airspeed);
111 #endif
112  }
113  break;
114 #endif
116 #ifdef HITL
117 
118  case DL_HITL_INFRARED: {
120  infrared.roll = DL_HITL_INFRARED_roll(buf);
121  infrared.pitch = DL_HITL_INFRARED_pitch(buf);
122  infrared.top = DL_HITL_INFRARED_top(buf);
123  }
124  break;
125 
126  case DL_HITL_UBX: {
128  if (gps_msg_received) {
129  gps_nb_ovrn++;
130  } else {
131  ubx_class = DL_HITL_UBX_class(buf);
132  ubx_id = DL_HITL_UBX_id(buf);
133  uint8_t l = DL_HITL_UBX_ubx_payload_length(buf);
134  uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(buf);
135  memcpy(ubx_msg_buf, ubx_payload, l);
136  gps_msg_received = true;
137  }
138  }
139  break;
140 #endif /* HITL */
141 
142  default:
143  break;
144  }
145 }
struct Infrared infrared
Definition: infrared.c:30
float x
Definition: common_nav.h:40
float east
in meters
float north
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
int16_t top
Definition: infrared.h:131
uint8_t gps_nb_ovrn
Definition: sim_ap.c:38
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
uint8_t nav_utm_zone0
Definition: common_nav.c:44
void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt)
Move a waypoint to given UTM coordinates.
Definition: common_nav.c:128
position in UTM coordinates Units: meters
Paparazzi floating point math for geodetic calculations.
vector in Latitude, Longitude and Altitude
float y
Definition: common_nav.h:41
int32_t nav_utm_north0
Definition: common_nav.c:43
Device independent GPS code (interface)
uint8_t zone
UTM zone number.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
float alt
in meters (normally above WGS84 reference ellipsoid)
int16_t roll
Definition: infrared.h:129
static void stateSetVerticalWindspeed_f(float v_windspeed)
Set vertical windspeed (float).
Definition: state.h:1300
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
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1309
float lon
in radians
float lat
in radians
int16_t pitch
Definition: infrared.h:130
void nav_goto_block(uint8_t b)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
static void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition: state.h:1291