Paparazzi UAS  v5.18.0_stable
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 }
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
dl_buffer
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
copilot.h
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
stateIsLocalCoordinateValid
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
pprz_geodetic_int.h
Paparazzi fixed point math for geodetic calculations.
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
telemetry.h
copilot_parse_move_wp_dl
void copilot_parse_move_wp_dl(uint8_t *buf)
If MOVE_WP from GCS.
Definition: copilot_rotorcraft.c:68
nb_waypoint
const uint8_t nb_waypoint
Definition: common_nav.c:37
uint8_t
unsigned char uint8_t
Definition: types.h:14
point::x
float x
Definition: common_nav.h:40
extra_pprz_tp
struct pprz_transport extra_pprz_tp
Definition: extra_pprz_dl.c:57
extra_dl_buffer
uint8_t extra_dl_buffer[MSG_SIZE]
Definition: extra_pprz_dl.c:55
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
int32_t
signed long int32_t
Definition: types.h:19
navigation.h
point::y
float y
Definition: common_nav.h:41
waypoint_is_global
bool waypoint_is_global(uint8_t wp_id)
Definition: waypoints.c:55
state
struct State state
Definition: state.c:36
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100