Paparazzi UAS  v5.12_stable-4-g9b43e9b
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 
54 bool obstacle_detected = false;
56 
57 // Called once on paparazzi autopilot start
59 {
60  // Do nothing
62 }
63 
64 // Called on each vision analysis result after receiving the struct
66 {
67  // Send ALL vision data to the ground
69 
70  switch (avoid_navigation_data.mode) {
71  case 0: // Go to Goal and stop at obstacles
72  //count 4 subsequent obstacles
73  if (stereocam_data.data[0] > 60) {
74  counter = counter + 1;
75  if (counter > 1) {
76  counter = 0;
77  //Obstacle detected, go to turn until clear mode
78  obstacle_detected = true;
80  }
81  } else {
82  counter = 0;
83  }
84  break;
85  case 1: // Turn until clear
86  //count 20 subsequent free frames
87  if (stereocam_data.data[0] < 60) {
88  counter = counter + 1;
89  if (counter > 6) {
90  counter = 0;
91  //Stop and put waypoint 2.5 m ahead
92  struct EnuCoor_i new_coor;
93  struct EnuCoor_i *pos = stateGetPositionEnu_i();
94  float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
95  float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
96  new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
97  new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
98  new_coor.z = pos->z;
99  waypoint_set_xy_i(WP_W1, new_coor.x, new_coor.y);
100  obstacle_detected = false;
102  }
103  } else {
104  counter = 0;
105  }
106  break;
107  case 2:
108  break;
109  default: // do nothing
110  break;
111  }
115 }
116 
118 {
119  *heading = *heading + increment;
120 }
121 
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.
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:674
void init_avoid_navigation()
void increase_nav_heading(int32_t *heading, int32_t increment)
Increases the NAV heading.