Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 /************** Flower Navigation **********************************************/
36 
44 static float CircleX;
45 static float CircleY;
46 static float Fly2X;
47 static float Fly2Y;
48 static float FlyFromX;
49 static float FlyFromY;
50 static float TransCurrentX;
51 static float TransCurrentY;
52 static float EdgeCurrentX;
53 static float EdgeCurrentY;
54 static float DistanceFromCenter;
55 static float FlowerTheta;
56 static float Flowerradius;
57 static uint8_t Center;
58 static uint8_t Edge;
59 
60 bool_t nav_flower_setup(uint8_t CenterWP, uint8_t EdgeWP)
61 {
62  Center = CenterWP;
63  Edge = EdgeWP;
64 
67 
69 
73 
74  FlowerTheta = atan2f(TransCurrentY,TransCurrentX);
79 
82  else
84 
85  CircleX = 0;
86  CircleY = 0;
87  return FALSE;
88 }
89 
90 bool_t nav_flower_run(void)
91 {
95 
96  bool_t InCircle = TRUE;
97  float CircleTheta;
98 
100  InCircle = FALSE;
101 
102  NavVerticalAutoThrottleMode(0); /* No pitch */
104 
105  switch(CFlowerStatus)
106  {
107  case Outside:
109  if(InCircle)
110  {
112  FlowerTheta = atan2f(TransCurrentY,TransCurrentX);
117  nav_init_stage();
118  }
119  break;
120  case FlowerLine:
122  if(!InCircle)
123  {
125  CircleTheta = nav_radius/Flowerradius;
126  CircleX = Flowerradius*cosf(FlowerTheta+3.14-CircleTheta)+WaypointX(Center);
127  CircleY = Flowerradius*sinf(FlowerTheta+3.14-CircleTheta)+WaypointY(Center);
128  nav_init_stage();
129  }
130  break;
131  case Circle:
133  if(InCircle)
134  {
140  else
142  FlowerTheta = atan2f(TransCurrentY,TransCurrentX);
147  nav_init_stage();
148  }
149  break;
150 
151  default:
152  break;
153  }
154  return TRUE;
155 }
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float y
in meters
#define FALSE
Definition: imu_chimu.h:141
float x
in meters
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.