Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_rover_base.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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 
27 #ifndef NAV_ROVER_BASE_H
28 #define NAV_ROVER_BASE_H
29 
31 
32 #ifndef ROVER_BASE_SEND_TRAJECTORY
33 #define ROVER_BASE_SEND_TRAJECTORY TRUE
34 #endif
35 
38 struct RoverNavGoto {
39  struct EnuCoor_f from;
40  struct EnuCoor_f to;
41  float dist2_to_wp;
42  float leg_progress;
43  float leg_length;
44 };
45 
49  struct EnuCoor_f center;
50  float radius;
51  float qdr;
52  float radians;
53 };
54 
57 enum oval_status { OR12, OC2, OR21, OC1 };
58 struct RoverNavOval {
59  enum oval_status status;
61 };
62 
65 struct RoverNavBase {
66  struct RoverNavGoto goto_wp;
67  struct RoverNavCircle circle;
68  struct RoverNavOval oval;
69 };
70 
71 extern struct RoverNavBase nav_rover_base;
72 
73 extern void nav_rover_init(void);
74 
75 
78 #define NavCircleCount() (fabsf(nav_rover_base.circle.radians) / (2*M_PI))
79 #define NavCircleQdr() ({ float qdr = DegOfRad(M_PI_2 - nav_rover_base.circle.qdr); NormCourse(qdr); qdr; })
80 
81 #define CloseDegAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
82 #define CloseRadAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })
83 
86 #define NavQdrCloseTo(x) CloseDegAngles(x, NavCircleQdr())
87 #define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(stateGetHorizontalSpeedDir_f()))
88 
89 
90 #endif
91 
oval_status
Definition: nav.h:60
struct RoverNavCircle circle
uint8_t count
number of laps
float leg_progress
progress over leg
struct RoverNavOval oval
enum oval_status status
oval status
struct RoverNavGoto goto_wp
@ OC2
@ OC1
@ OR12
@ OR21
void nav_rover_init(void)
Init and register nav functions.
float radians
incremental angular distance
struct EnuCoor_f from
start WP position
struct RoverNavBase nav_rover_base
float leg_length
leg length
struct EnuCoor_f center
center WP position
float qdr
qdr in radians
float dist2_to_wp
squared distance to next waypoint
struct EnuCoor_f to
end WP position
float radius
radius in meters
Basic Nav struct.
Circle pattern.
Waypoint and route pattern.
vector in East North Up coordinates Units: meters
Rover navigation functions.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98