Paparazzi UAS  v5.12_stable-4-g9b43e9b
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 #if USE_JOYSTICK
52 #include "autopilot.h"
54 #define JoystickHandeDatalink(_roll_int8, _pitch_int8, _throttle_int8) { \
55  if (autopilot_get_mode() == AP_MODE_AUTO2 && nav_block == joystick_block) { \
56  h_ctl_roll_setpoint = _roll_int8 * (AUTO1_MAX_ROLL / 0x7f); \
57  h_ctl_pitch_setpoint = _pitch_int8 * (AUTO1_MAX_PITCH / 0x7f); \
58  v_ctl_throttle_setpoint = (MAX_PPRZ/0x7f) * _throttle_int8; \
59  } \
60  }
61 #endif
62 
63 void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
64 {
65  uint8_t msg_id = IdOfPprzMsg(buf);
66 
67  /* parse telemetry messages coming from ground station */
68  switch (msg_id) {
69 
70 #ifdef NAV
71  case DL_BLOCK: {
72  if (DL_BLOCK_ac_id(buf) != AC_ID) { break; }
73  nav_goto_block(DL_BLOCK_block_id(buf));
74  SEND_NAVIGATION(trans, dev);
75  }
76  break;
77 
78  case DL_MOVE_WP: {
79  if (DL_MOVE_WP_ac_id(buf) != AC_ID) { break; }
80  uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
81 
82  /* Computes from (lat, long) in the referenced UTM zone */
83  struct LlaCoor_f lla;
84  lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(buf) / 1e7));
85  lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(buf) / 1e7));
86  lla.alt = MOfMM(DL_MOVE_WP_alt(buf));
87  struct UtmCoor_f utm;
88  utm.zone = nav_utm_zone0;
89  utm_of_lla_f(&utm, &lla);
90  nav_move_waypoint(wp_id, utm.east, utm.north, utm.alt);
91 
92  /* Waypoint range is limited. Computes the UTM pos back from the relative
93  coordinates */
94  utm.east = waypoints[wp_id].x + nav_utm_east0;
95  utm.north = waypoints[wp_id].y + nav_utm_north0;
96  pprz_msg_send_WP_MOVED(trans, dev, AC_ID, &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0);
97  }
98  break;
99 #endif
101 #ifdef WIND_INFO
102  case DL_WIND_INFO: {
103  if (DL_WIND_INFO_ac_id(buf) != AC_ID) { break; }
104  uint8_t flags = DL_WIND_INFO_flags(buf);
105  struct FloatVect2 wind = { 0.f, 0.f };
106  float upwind = 0.f;
107  if (bit_is_set(flags, 0)) {
108  wind.x = DL_WIND_INFO_north(buf);
109  wind.y = DL_WIND_INFO_east(buf);
111  }
112  if (bit_is_set(flags, 1)) {
113  upwind = DL_WIND_INFO_up(buf);
115  }
116 #if !USE_AIRSPEED
117  if (bit_is_set(flags, 2)) {
118  stateSetAirspeed_f(DL_WIND_INFO_airspeed(buf));
119  }
120 #endif
121 #ifdef WIND_INFO_RET
122  float airspeed = stateGetAirspeed_f();
123  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &wind.y, &wind.x, &upwind, &airspeed);
124 #endif
125  }
126  break;
127 #endif
129 #ifdef HITL
130 
131  case DL_HITL_INFRARED: {
133  infrared.roll = DL_HITL_INFRARED_roll(buf);
134  infrared.pitch = DL_HITL_INFRARED_pitch(buf);
135  infrared.top = DL_HITL_INFRARED_top(buf);
136  }
137  break;
138 
139  case DL_HITL_UBX: {
141  if (gps_msg_received) {
142  gps_nb_ovrn++;
143  } else {
144  ubx_class = DL_HITL_UBX_class(buf);
145  ubx_id = DL_HITL_UBX_id(buf);
146  uint8_t l = DL_HITL_UBX_ubx_payload_length(buf);
147  uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(buf);
148  memcpy(ubx_msg_buf, ubx_payload, l);
149  gps_msg_received = true;
150  }
151  }
152  break;
153 #endif /* HITL */
154 
155 #if USE_JOYSTICK
156  case DL_JOYSTICK_RAW: {
157  if (DL_JOYSTICK_RAW_ac_id(buf) == AC_ID) {
158  JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(buf),
159  DL_JOYSTICK_RAW_pitch(buf),
160  DL_JOYSTICK_RAW_throttle(buf));
161  }
162  }
163  break;
164 #endif // USE_JOYSTICK
165  default:
166  break;
167  }
168 }
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
Fixed wing horizontal control.
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
uint8_t joystick_block
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
Core autopilot interface common to all firmwares.
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