Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
nav_flower.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2013 The Paparazzi Team
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 
28 #include "modules/nav/nav_flower.h"
29 
31 #include "state.h"
32 #include "autopilot.h"
33 #include "generated/flight_plan.h"
34 
35 #if USE_MISSION
37 
38 static bool nav_flower_mission(uint8_t nb, float *params, bool init)
39 {
40  if (nb != 2) {
41  return false; // wrong number of parameters
42  }
43  if (init) {
44  uint8_t center = (uint8_t)(params[0]);
45  uint8_t edge = (uint8_t)(params[1]);
46  nav_flower_setup(center, edge);
47  }
48  return nav_flower_run();
49 }
50 #endif
51 
52 
53 void nav_flower_init(void)
54 {
55 #if USE_MISSION
56  mission_register(nav_flower_mission, "FLWR");
57 #endif
58 }
59 
60 /************** Flower Navigation **********************************************/
61 
69 static float CircleX;
70 static float CircleY;
71 static float Fly2X;
72 static float Fly2Y;
73 static float FlyFromX;
74 static float FlyFromY;
75 static float TransCurrentX;
76 static float TransCurrentY;
77 static float EdgeCurrentX;
78 static float EdgeCurrentY;
79 static float DistanceFromCenter;
80 static float FlowerTheta;
81 static float Flowerradius;
82 static uint8_t Center;
83 static uint8_t Edge;
84 
85 void nav_flower_setup(uint8_t CenterWP, uint8_t EdgeWP)
86 {
87  Center = CenterWP;
88  Edge = EdgeWP;
89 
92 
94 
98 
100  Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center);
101  Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center);
104 
107  } else {
109  }
110 
111  CircleX = 0;
112  CircleY = 0;
113 }
114 
115 bool nav_flower_run(void)
116 {
120 
121  bool InCircle = true;
122  float CircleTheta;
123 
125  InCircle = false;
126  }
127 
128  NavVerticalAutoThrottleMode(0); /* No pitch */
130 
131  switch (CFlowerStatus) {
132  case Outside:
134  if (InCircle) {
137  Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center);
138  Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center);
141  nav_init_stage();
142  }
143  break;
144  case FlowerLine:
146  if (!InCircle) {
148  CircleTheta = nav_radius / Flowerradius;
149  CircleX = Flowerradius * cosf(FlowerTheta + 3.14 - CircleTheta) + WaypointX(Center);
150  CircleY = Flowerradius * sinf(FlowerTheta + 3.14 - CircleTheta) + WaypointY(Center);
151  nav_init_stage();
152  }
153  break;
154  case Circle:
156  if (InCircle) {
162  } else {
164  }
166  Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center);
167  Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center);
170  nav_init_stage();
171  }
172  break;
173 
174  default:
175  break;
176  }
177  return true;
178 }
CircleY
static float CircleY
Definition: nav_flower.c:70
FlowerLine
@ FlowerLine
Definition: nav_flower.c:67
Fly2X
static float Fly2X
Definition: nav_flower.c:71
DistanceFromCenter
static float DistanceFromCenter
Definition: nav_flower.c:79
TransCurrentX
static float TransCurrentX
Definition: nav_flower.c:75
nav_flower.h
WaypointY
#define WaypointY(_wp)
Definition: common_nav.h:46
FlyFromX
static float FlyFromX
Definition: nav_flower.c:73
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
EdgeCurrentX
static float EdgeCurrentX
Definition: nav_flower.c:77
WaypointX
#define WaypointX(_wp)
Definition: common_nav.h:45
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:109
nav_flower_run
bool nav_flower_run(void)
Definition: nav_flower.c:115
nav_flower_setup
void nav_flower_setup(uint8_t CenterWP, uint8_t EdgeWP)
Definition: nav_flower.c:85
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:171
FlowerStatus
FlowerStatus
Makes a flower pattern.
Definition: nav_flower.c:67
nav_route_xy
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
Computes the carrot position along the desired segment.
Definition: nav.c:381
Fly2Y
static float Fly2Y
Definition: nav_flower.c:72
nav_init_stage
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: nav.c:93
nav_flower_init
void nav_flower_init(void)
Definition: nav_flower.c:53
Outside
@ Outside
Definition: nav_flower.c:67
TransCurrentY
static float TransCurrentY
Definition: nav_flower.c:76
uint8_t
unsigned char uint8_t
Definition: types.h:14
CircleX
static float CircleX
Definition: nav_flower.c:69
autopilot.h
CFlowerStatus
static enum FlowerStatus CFlowerStatus
Definition: nav_flower.c:68
nav_radius
float nav_radius
Definition: nav.c:57
init
bool init
Definition: nav_gls.c:57
nav.h
mission_common.h
mission planner library
EdgeCurrentY
static float EdgeCurrentY
Definition: nav_flower.c:78
mission_register
bool mission_register(mission_custom_cb cb, char *type)
Register a new navigation or action callback function.
Definition: mission_common.c:99
Circle
@ Circle
Definition: nav_flower.c:67
FlyFromY
static float FlyFromY
Definition: nav_flower.c:74
Edge
static uint8_t Edge
Definition: nav_flower.c:83
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
state.h
FlowerTheta
static float FlowerTheta
Definition: nav_flower.c:80
Flowerradius
static float Flowerradius
Definition: nav_flower.c:81
Center
static uint8_t Center
Definition: nav_flower.c:82
NavVerticalAltitudeMode
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:185