Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
follow.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Freek van Tienen
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  */
22 
28 #include "multi/follow.h"
29 #include "generated/flight_plan.h"
30 #include "generated/airframe.h"
31 
32 #include "state.h"
33 #include "subsystems/ins/ins_int.h"
34 #include "navigation.h"
35 #include "messages.h"
36 #include "dl_protocol.h"
37 
38 #ifndef FOLLOW_OFFSET_X
39 #define FOLLOW_OFFSET_X 0.0
40 #endif
41 
42 #ifndef FOLLOW_OFFSET_Y
43 #define FOLLOW_OFFSET_Y 0.0
44 #endif
45 
46 #ifndef FOLLOW_OFFSET_Z
47 #define FOLLOW_OFFSET_Z 0.0
48 #endif
49 
50 void follow_init( void ) {
51 
52 }
53 
54 void follow_change_wp( unsigned char* buffer ) {
55  struct EcefCoor_i new_pos;
56  struct EnuCoor_i enu;
57  new_pos.x = DL_REMOTE_GPS_ecef_x(buffer);
58  new_pos.y = DL_REMOTE_GPS_ecef_y(buffer);
59  new_pos.z = DL_REMOTE_GPS_ecef_z(buffer);
60 
61  // Translate to ENU
62  enu_of_ecef_point_i(&enu, &ins_impl.ltp_def, &new_pos);
64 
65  // TODO: Add the angle to the north
66 
67  // Update the offsets
71 
72  // TODO: Remove the angle to the north
73 
74  // Move the waypoint
75  INT32_VECT3_COPY(waypoints[FOLLOW_WAYPOINT_ID], enu);
76 }
int32_t y
in centimeters
vector in EarthCenteredEarthFixed coordinates
#define FOLLOW_OFFSET_Y
Definition: follow.c:43
#define FOLLOW_OFFSET_X
Definition: follow.c:39
Follow a certain AC id.
void follow_init(void)
Definition: follow.c:50
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
unsigned char buffer[4096]
Definition: ahrs_ardrone2.c:47
int32_t z
in centimeters
int32_t z
Up.
#define FOLLOW_OFFSET_Z
Definition: follow.c:47
void enu_of_ecef_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local ENU.
int32_t x
in centimeters
#define INT32_POS_OF_CM_DEN
#define INT32_VECT3_COPY(_o, _i)
void follow_change_wp(unsigned char *buffer)
Definition: follow.c:54
API to get/set the generic vehicle states.
#define INT32_POS_OF_CM_NUM
int32_t y
North.
vector in East North Up coordinates
struct InsAltFloat ins_impl
global INS state
Definition: ins_alt_float.c:51
int32_t x
East.
INS for rotorcrafts combining vertical and horizontal filters.
#define POS_BFP_OF_REAL(_af)