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
rotorcraft_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 
29 
30 #include "pprzlink/dl_protocol.h"
31 
32 #ifdef USE_NAVIGATION
34 #include "math/pprz_geodetic_int.h"
35 #endif
36 
37 #include "autopilot.h"
39 
40 void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
41 {
42  uint8_t msg_id = IdOfPprzMsg(buf);
43 
44  /* parse telemetry messages coming from ground station */
45  switch (msg_id) {
46 
47 #ifdef USE_NAVIGATION
48  case DL_BLOCK : {
49  if (DL_BLOCK_ac_id(buf) != AC_ID) { break; }
50  nav_goto_block(DL_BLOCK_block_id(buf));
51  }
52  break;
53 
54  case DL_MOVE_WP : {
55  uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
56  if (ac_id != AC_ID) { break; }
58  uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
59  struct LlaCoor_i lla;
60  lla.lat = DL_MOVE_WP_lat(buf);
61  lla.lon = DL_MOVE_WP_lon(buf);
62  /* WP_alt from message is alt above MSL in mm
63  * lla.alt is above ellipsoid in mm
64  */
65  lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
67  waypoint_move_lla(wp_id, &lla);
68  }
69  }
70  break;
71 #endif /* USE_NAVIGATION */
72 
73 #ifdef AP_MODE_GUIDED
74  case DL_GUIDED_SETPOINT_NED:
75  if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID) { break; }
76 
77  autopilot_guided_update(DL_GUIDED_SETPOINT_NED_flags(buf),
78  DL_GUIDED_SETPOINT_NED_x(buf),
79  DL_GUIDED_SETPOINT_NED_y(buf),
80  DL_GUIDED_SETPOINT_NED_z(buf),
81  DL_GUIDED_SETPOINT_NED_yaw(buf));
82  break;
83 #endif
84 
85  default:
86  break;
87  }
88 }
uint8_t ac_id
Definition: sim_ap.c:45
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
Set guided setpoints using flag mask in GUIDED mode.
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
Autopilot guided mode interface.
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
struct LlaCoor_i lla
Reference point in lla.
int32_t lon
in degrees*1e7
Paparazzi fixed point math for geodetic calculations.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
Core autopilot interface common to all firmwares.
unsigned char uint8_t
Definition: types.h:14
int32_t lat
in degrees*1e7
void nav_goto_block(uint8_t b)
struct State state
Definition: state.c:36