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
avoid_navigation.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 
28 // Own Header
29 #include "avoid_navigation.h"
30 
31 // Paparazzi Data
32 #include "state.h"
33 
34 // Interact with navigation
35 #include "navigation.h"
36 
37 // Know waypoint numbers and blocks
38 #include "generated/flight_plan.h"
39 
40 // To get data from the stereo cam
42 
43 // Downlink
44 #ifndef DOWNLINK_DEVICE
45 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
46 #endif
47 #include "pprzlink/messages.h"
49 
50 #include "led.h"
51 
52 
53 
54 
55 
57 bool obstacle_detected = false;
59 
60 // Called once on paparazzi autopilot start
62 {
63  // Do nothing
65 }
66 
67 // Called on each vision analysis result after receiving the struct
69 {
70  // Send ALL vision data to the ground
72 
73  switch (avoid_navigation_data.mode) {
74  case 0: // Go to Goal and stop at obstacles
75  //count 4 subsequent obstacles
76  if (stereocam_data.data[0] > 60) {
77  counter = counter + 1;
78  if (counter > 1) {
79  counter = 0;
80  //Obstacle detected, go to turn until clear mode
81  obstacle_detected = true;
83  }
84  } else {
85  counter = 0;
86  }
87  break;
88  case 1: // Turn until clear
89  //count 20 subsequent free frames
90  if (stereocam_data.data[0] < 60) {
91  counter = counter + 1;
92  if (counter > 6) {
93  counter = 0;
94  //Stop and put waypoint 2.5 m ahead
95  struct EnuCoor_i new_coor;
96  struct EnuCoor_i *pos = stateGetPositionEnu_i();
97  float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
98  float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
99  new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
100  new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
101  new_coor.z = pos->z;
102  waypoint_set_xy_i(WP_W1, new_coor.x, new_coor.y);
103  obstacle_detected = false;
105  }
106  } else {
107  counter = 0;
108  }
109  break;
110  case 2:
111  break;
112  default: // do nothing
113  break;
114  }
118 }
119 
121 {
122  *heading = *heading + increment;
123 }
124 
int32_t y
North.
int32_t x
East.
void run_avoid_navigation_onvision(void)
#define POS_BFP_OF_REAL(_af)
struct AvoidNavigationStruct avoid_navigation_data
global VIDEO state
#define ANGLE_FLOAT_OF_BFP(_ai)
void waypoint_set_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Set only local XY coordinates of waypoint without update altitude.
Definition: waypoints.c:138
static float heading
Definition: ahrs_infrared.c:45
interface to the TU Delft serial stereocam
int32_t counter
int32_t z
Up.
uint8array stereocam_data
Definition: stereocam.c:57
signed long int32_t
Definition: types.h:19
vector in East North Up coordinates
API to get/set the generic vehicle states.
#define NAV_LINE_AVOID_SEGMENT_LENGTH
bool obstacle_detected
arch independent LED (Light Emitting Diodes) API
uint8_t mode
0 = straight, 1 = right, 2 = left, ...
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:668
uint8_t * data
Definition: stereocam.h:34
void init_avoid_navigation()
void increase_nav_heading(int32_t *heading, int32_t increment)
Increases the NAV heading.