Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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/airframe.h"
30 #include "generated/flight_plan.h"
31 
33 
34 #include "state.h"
35 
36 /* FOLLOW_OFFSET_ X Y and Z are all in ENU frame */
37 #ifndef FOLLOW_OFFSET_X
38 #define FOLLOW_OFFSET_X 0.0
39 #endif
40 
41 #ifndef FOLLOW_OFFSET_Y
42 #define FOLLOW_OFFSET_Y 0.0
43 #endif
44 
45 #ifndef FOLLOW_OFFSET_Z
46 #define FOLLOW_OFFSET_Z 0.0
47 #endif
48 
49 #ifndef FOLLOW_AC_ID
50 #error "Please define FOLLOW_AC_ID"
51 #endif
52 
53 #ifndef FOLLOW_WAYPOINT_ID
54 #error "Please define FOLLOW_WAYPOINT_ID"
55 #endif
56 
57 void follow_init(void) {}
58 
59 /*
60  * follow_wp(void)
61  * updates the FOLLOW_WAYPOINT_ID to a fixed offset from the last received location
62  * of other aircraft with id FOLLOW_AC_ID
63  */
64 void follow_wp(void)
65 {
66  struct EnuCoor_i *ac = acInfoGetPositionEnu_i(FOLLOW_AC_ID);
67 
68  struct EnuCoor_i enu = *stateGetPositionEnu_i();
69  enu.x += ac->x + POS_BFP_OF_REAL(FOLLOW_OFFSET_X);
70  enu.y += ac->y + POS_BFP_OF_REAL(FOLLOW_OFFSET_Y);
71  enu.z += ac->z + POS_BFP_OF_REAL(FOLLOW_OFFSET_Z);
72 
73  // Move the waypoint
74  waypoint_set_enu_i(FOLLOW_WAYPOINT_ID, &enu);
75 }
acInfoGetPositionEnu_i
static struct EnuCoor_i * acInfoGetPositionEnu_i(uint8_t ac_id)
Get position in local ENU coordinates (int).
Definition: traffic_info.h:350
EnuCoor_i::y
int32_t y
North.
Definition: pprz_geodetic_int.h:79
EnuCoor_i::x
int32_t x
East.
Definition: pprz_geodetic_int.h:78
stateGetPositionEnu_i
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
FOLLOW_OFFSET_Y
#define FOLLOW_OFFSET_Y
Definition: follow.c:42
waypoints.h
waypoint_set_enu_i
void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
Definition: waypoints.c:101
FOLLOW_OFFSET_X
#define FOLLOW_OFFSET_X
Definition: follow.c:38
follow_init
void follow_init(void)
Definition: follow.c:57
follow_wp
void follow_wp(void)
Definition: follow.c:64
follow.h
Follow a certain AC id.
FOLLOW_OFFSET_Z
#define FOLLOW_OFFSET_Z
Definition: follow.c:46
POS_BFP_OF_REAL
#define POS_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:216
EnuCoor_i::z
int32_t z
Up.
Definition: pprz_geodetic_int.h:80
state.h
EnuCoor_i
vector in East North Up coordinates
Definition: pprz_geodetic_int.h:77