Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_poles.c
Go to the documentation of this file.
1
/*
2
* Copyright (C) 2009-2015 ENAC
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, see
18
* <http://www.gnu.org/licenses/>.
19
*/
20
26
#include "
modules/nav/nav_poles.h
"
27
#include "
modules/nav/common_nav.h
"
28
29
uint8_t
nav_poles_count
= 0;
30
float
nav_poles_time
= 0.;
31
int8_t
nav_poles_land
= 1;
32
33
#define SAFETY_MARGIN 0.7
34
37
bool
nav_poles_init
(
uint8_t
wp1,
uint8_t
wp2,
38
uint8_t
wp1c,
uint8_t
wp2c,
39
float
radius)
40
{
41
float
x =
WaypointX
(wp2) -
WaypointX
(wp1);
42
float
y =
WaypointY
(wp2) -
WaypointY
(wp1);
43
float
d = sqrt(x * x + y * y);
44
45
/* Unit vector from wp1 to wp2 */
46
x /= d;
47
y /= d;
48
49
WaypointX
(wp2c) =
WaypointX
(wp2) - (x *
SAFETY_MARGIN
+ y) * radius;
50
WaypointY
(wp2c) =
WaypointY
(wp2) - (y *
SAFETY_MARGIN
- x) * radius;
51
52
WaypointX
(wp1c) =
WaypointX
(wp1) + (x *
SAFETY_MARGIN
- y) * radius;
53
WaypointY
(wp1c) =
WaypointY
(wp1) + (y *
SAFETY_MARGIN
+ x) * radius;
54
55
return
false
;
56
}
57
common_nav.h
WaypointX
#define WaypointX(_wp)
Definition:
common_nav.h:45
WaypointY
#define WaypointY(_wp)
Definition:
common_nav.h:46
nav_poles_count
uint8_t nav_poles_count
Definition:
nav_poles.c:29
nav_poles_land
int8_t nav_poles_land
Definition:
nav_poles.c:31
nav_poles_time
float nav_poles_time
Definition:
nav_poles.c:30
nav_poles_init
bool nav_poles_init(uint8_t wp1, uint8_t wp2, uint8_t wp1c, uint8_t wp2c, float radius)
computes position of wp1c and wp2c, reference points for an oval around waypoints wp1 and wp2
Definition:
nav_poles.c:37
SAFETY_MARGIN
#define SAFETY_MARGIN
Definition:
nav_poles.c:33
nav_poles.h
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition:
vl53l1_types.h:98
int8_t
signed char int8_t
Typedef defining 8 bit char type.
Definition:
vl53l1_types.h:103
sw
airborne
modules
nav
nav_poles.c
Generated on Fri Nov 8 2024 14:10:46 for Paparazzi UAS by
1.9.1