Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
copilot_rotorcraft.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 2017 Michal Podhradsky <http://github.com/podhrmic>
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  */
46 
47 // needed for WP_MOVED confirmation
49 //#include "math/pprz_geodetic_float.h"
50 #include "math/pprz_geodetic_int.h"
51 //#include "state.h"
52 
69 {
70  if (DL_MOVE_WP_ac_id(buf) == AC_ID) {
72  uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
73  if (wp_id >= nb_waypoint) {
74  return;
75  }
76 
77  struct LlaCoor_i lla;
78  lla.lat = DL_MOVE_WP_lat(buf);
79  lla.lon = DL_MOVE_WP_lon(buf);
80  /* WP_alt from message is alt above MSL in mm
81  * lla.alt is above ellipsoid in mm
82  */
83  lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
85 
86 
87  if (waypoint_is_global(wp_id)) {
88  /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */
90 
91  if (buf == extra_dl_buffer) {
92  // MOVE_WP came from extra_dl, respond over telemetry
93  DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id,
94  &lla.lat, &lla.lon, &hmsl);
95  }
96 
97  if (buf == dl_buffer) {
98  // MOVE_WP came over telemetry, respond over extra_dl
99  DOWNLINK_SEND_WP_MOVED_LLA(extra_pprz_tp, DefaultDevice, &wp_id,
100  &lla.lat, &lla.lon, &hmsl);
101  }
102  } else {
103  if (buf == extra_dl_buffer) {
104  // MOVE_WP came from extra_dl, respond over telemetry
105  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
106  &waypoints[wp_id].enu_i.x,
107  &waypoints[wp_id].enu_i.y,
108  &waypoints[wp_id].enu_i.z);
109  }
110 
111  if (buf == dl_buffer) {
112  // MOVE_WP came over telemetry, respond over extra_dl
113  DOWNLINK_SEND_WP_MOVED_ENU(extra_pprz_tp, DefaultDevice, &wp_id,
114  &waypoints[wp_id].enu_i.x,
115  &waypoints[wp_id].enu_i.y,
116  &waypoints[wp_id].enu_i.z);
117  }
118  }
119  }
120  }
121 }
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
const uint8_t nb_waypoint
Definition: common_nav.c:38
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
Mission Computer module, interfacing the mission computer (also known as Copilot),...
void copilot_parse_move_wp_dl(uint8_t *buf)
If MOVE_WP from GCS.
struct pprz_transport extra_pprz_tp
Definition: extra_pprz_dl.c:57
uint8_t extra_dl_buffer[MSG_SIZE]
Definition: extra_pprz_dl.c:55
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
int32_t lon
in degrees*1e7
vector in Latitude, Longitude and Altitude
struct State state
Definition: state.c:36
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:63
bool waypoint_is_global(uint8_t wp_id)
Definition: waypoints.c:75
Paparazzi fixed point math for geodetic calculations.
Rotorcraft navigation functions.
Periodic telemetry system header (includes downlink utility and generated code).
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98