Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stereo_avoid_sim.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013
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 
22 
23 // Own header
24 #include "stereo_avoid.h"
25 
26 // Navigate Based On Vision
27 #include "avoid_navigation.h"
28 
29 // Paparazzi State: Attitude -> Vision
30 #include "state.h" // for attitude
31 
32 #include "std.h"
33 
34 
36 {
37  // Simulated vision: obstacle at 0,0
40 
41  // Navigation Code
43 }
44 
46 #define NormAngleRad(x) { \
47  int dont_loop_forever = 0; \
48  while ((x < (-M_PI)) && ++dont_loop_forever) x += (2 * M_PI); \
49  while ((x >= (M_PI)) && ++dont_loop_forever) x -= (2 * M_PI); \
50  }
51 
52 
53 void stereo_avoid_run(void)
54 {
55  static int counter = 0;
56 
57  counter++;
58  // Read Latest GST Module Results
59  if (counter >= (2)) {
60  // Vertical
61  float dx = sqrt((stateGetPositionEnu_f()->x * stateGetPositionEnu_f()->x) + (stateGetPositionEnu_f()->y *
62  stateGetPositionEnu_f()->y));
63 
64  /*
65  float dh = (17.0f - stateGetPositionEnu_f()->z);
66  float vang = atan2(dh,dx) * 80.9F * 2.0f; // 255 / PI
67 
68  if (vang < 0.0f)
69  vang = 0.0f;
70  if (vang > 255.0f)
71  vang = 255.0f;
72  */
73 
74 
75  // Horizontal
76  float bearing = atan2(- stateGetPositionEnu_f()->x, - stateGetPositionEnu_f()->y);
78  float diff = bearing - heading;
79  NormAngleRad(diff);
80  diff = DegOfRad(diff);
81 
82  //float hang = DegOfRad(atan2(10,dx)); // 255 / PI
83 
84 
85  float viewangle = 50; // degrees
86  float range = viewangle / 2.0f;
87  //float deg_per_bin = viewangle / ((float) 2);
88 
89  float disperity = 0;
90 
91  if (fabs(diff) > range) {
92  disperity = 0;
93  } else if (dx > 2) {
94  disperity = 0;
95  } else {
96  disperity = 30;
97  }
98 
99  //float bin_nr_mid = range/deg_per_bin;
100  //float bin_nr_start = bin_nr_mid + diff/deg_per_bin - hang/deg_per_bin;
101  //float bin_nr_stop = bin_nr_mid + diff/deg_per_bin + hang/deg_per_bin;
102 
103  avoid_navigation_data.stereo_bin[0] = disperity;
105 
106  counter = 0;
108  }
109 }
110 
111 
113 {
114 }
115 
116 
118 {
119 }
void run_avoid_navigation_onvision(void)
void stereo_avoid_run(void)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1125
struct AvoidNavigationStruct avoid_navigation_data
global VIDEO state
void stereo_avoid_init(void)
float psi
in radians
void stereo_avoid_stop(void)
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:713
void stereo_avoid_start(void)
static float heading
Definition: ahrs_infrared.c:45
int32_t counter
API to get/set the generic vehicle states.
#define NormAngleRad(x)
Normalize a degree angle between 0 and 359.
void init_avoid_navigation()