Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
rover_datalink.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
28 
29 #include "pprzlink/dl_protocol.h"
30 
31 #ifdef USE_NAVIGATION
33 #include "math/pprz_geodetic_int.h"
34 #endif
35 
36 #include "autopilot.h"
37 
38 void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
39 {
40  uint8_t msg_id = IdOfPprzMsg(buf);
41 
42  /* parse telemetry messages coming from ground station */
43  switch (msg_id) {
44 
45 #ifdef USE_NAVIGATION
46  case DL_BLOCK : {
47  if (DL_BLOCK_ac_id(buf) != AC_ID) { break; }
48  nav_goto_block(DL_BLOCK_block_id(buf));
49  }
50  break;
51 
52  case DL_MOVE_WP : {
53  uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
54  if (ac_id != AC_ID) { break; }
56  uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
57  struct LlaCoor_i lla;
58  lla.lat = DL_MOVE_WP_lat(buf);
59  lla.lon = DL_MOVE_WP_lon(buf);
60  /* WP_alt from message is alt above MSL in mm
61  * lla.alt is above ellipsoid in mm
62  */
63  lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
65  waypoint_move_lla(wp_id, &lla);
66  }
67  }
68  break;
69 #endif /* USE_NAVIGATION */
70 
71  default:
72  break;
73  }
74 }
uint8_t ac_id
Definition: sim_ap.c:45
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
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)
Rover navigation functions.
struct State state
Definition: state.c:36