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
copilot_fixedwing.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
51 
68 {
69  if (DL_MOVE_WP_ac_id(buf) == AC_ID) {
70  uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
71 
72  if (wp_id >= nb_waypoint) {
73  return;
74  }
75 
76  /* Computes from (lat, long) in the referenced UTM zone */
77  struct LlaCoor_f lla;
78  lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(buf) / 1e7));
79  lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(buf) / 1e7));
80  lla.alt = ((float)(DL_MOVE_WP_alt(buf)))/1000.;
81  struct UtmCoor_f utm;
82  utm.zone = nav_utm_zone0;
83  utm_of_lla_f(&utm, &lla);
84 
85  // Waypoint range is limited. Computes the UTM pos back from the relative
86  // coordinates */
87  utm.east = waypoints[wp_id].x + nav_utm_east0;
88  utm.north = waypoints[wp_id].y + nav_utm_north0;
89 
90  if (buf == extra_dl_buffer) {
91  // MOVE_WP came from extra_dl, respond over telemetry
92  DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice,
93  &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0);
94  }
95 
96  if (buf == dl_buffer) {
97  // MOVE_WP came over telemetry, respond over extra_dl
98  DOWNLINK_SEND_WP_MOVED(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
99  &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0);
100  }
101 
102  }
103 }
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)
Periodic telemetry system header (includes downlink utility and generated code).
uint8_t nav_utm_zone0
Definition: common_nav.c:44
position in UTM coordinates Units: meters
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
struct pprz_transport extra_pprz_tp
Definition: extra_pprz_dl.c:52
void copilot_parse_move_wp_dl(uint8_t *buf)
If MOVE_WP from GCS.
uint8_t zone
UTM zone number.
const uint8_t nb_waypoint
Definition: common_nav.c:37
float alt
in meters (normally above WGS84 reference ellipsoid)
uint8_t extra_dl_buffer[MSG_SIZE]
Definition: extra_pprz_dl.c:50
unsigned char uint8_t
Definition: types.h:14
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
Mission Computer module, interfacing the mission computer (also known as Copilot), based losely on ISaAC: The Intelligent Safety and Airworthiness Co-Pilot module Based on paper "A Payload Verification and Management Framework for Small UAV-based Personal Remote Sensing Systems" by Cal Coopmans and Chris Coffin.
float lon
in radians
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
float lat
in radians
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)