Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
90  EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
91  EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
92 
93  Flowerradius = sqrtf(EdgeCurrentX * EdgeCurrentX + EdgeCurrentY * EdgeCurrentY);
94 
95  TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
96  TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
97  DistanceFromCenter = sqrtf(TransCurrentX * TransCurrentX + TransCurrentY * TransCurrentY);
98 
99  FlowerTheta = atan2f(TransCurrentY, TransCurrentX);
100  Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center);
101  Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center);
102  FlyFromX = stateGetPositionEnu_f()->x;
103  FlyFromY = stateGetPositionEnu_f()->y;
104 
105  if (DistanceFromCenter > Flowerradius) {
106  CFlowerStatus = Outside;
107  } else {
108  CFlowerStatus = FlowerLine;
109  }
110 
111  CircleX = 0;
112  CircleY = 0;
113 }
114 
115 bool nav_flower_run(void)
116 {
117  TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
118  TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
119  DistanceFromCenter = sqrtf(TransCurrentX * TransCurrentX + TransCurrentY * TransCurrentY);
120 
121  bool InCircle = true;
122  float CircleTheta;
123 
124  if (DistanceFromCenter > Flowerradius) {
125  InCircle = false;
126  }
127 
128  NavVerticalAutoThrottleMode(0); /* No pitch */
129  NavVerticalAltitudeMode(waypoints[Center].a, 0.);
130 
131  switch (CFlowerStatus) {
132  case Outside:
133  nav_route_xy(FlyFromX, FlyFromY, Fly2X, Fly2Y);
134  if (InCircle) {
135  CFlowerStatus = FlowerLine;
136  FlowerTheta = atan2f(TransCurrentY, TransCurrentX);
137  Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center);
138  Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center);
139  FlyFromX = stateGetPositionEnu_f()->x;
140  FlyFromY = stateGetPositionEnu_f()->y;
141  nav_init_stage();
142  }
143  break;
144  case FlowerLine:
145  nav_route_xy(FlyFromX, FlyFromY, Fly2X, Fly2Y);
146  if (!InCircle) {
147  CFlowerStatus = Circle;
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:
155  nav_circle_XY(CircleX, CircleY, nav_radius);
156  if (InCircle) {
157  EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
158  EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
159  Flowerradius = sqrtf(EdgeCurrentX * EdgeCurrentX + EdgeCurrentY * EdgeCurrentY);
160  if (DistanceFromCenter > Flowerradius) {
161  CFlowerStatus = Outside;
162  } else {
163  CFlowerStatus = FlowerLine;
164  }
165  FlowerTheta = atan2f(TransCurrentY, TransCurrentX);
166  Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center);
167  Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center);
168  FlyFromX = stateGetPositionEnu_f()->x;
169  FlyFromY = stateGetPositionEnu_f()->y;
170  nav_init_stage();
171  }
172  break;
173 
174  default:
175  break;
176  }
177  return true;
178 }
bool mission_register(mission_custom_cb cb, char *type)
Register a new navigation or action callback function.
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
mission planner library
#define WaypointY(_wp)
Definition: common_nav.h:46
Core autopilot interface common to all firmwares.
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float y
in meters