Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
26
27
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
54bool obstacle_detected = false;
56
57// Called once on paparazzi autopilot start
59{
60 // Do nothing
61 avoid_navigation_data.mode = 0;
62}
63
64// Called on each vision analysis result after receiving the struct
66{
67 // Send ALL vision data to the ground
68 DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 5, avoid_navigation_data.stereo_bin);
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;
79 avoid_navigation_data.mode = 1;
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(nav.heading);
95 float cos_heading = cosf(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;
101 avoid_navigation_data.mode = 0;
102 }
103 } else {
104 counter = 0;
105 }
106 break;
107 case 2:
108 break;
109 default: // do nothing
110 break;
111 }
112 avoid_navigation_data.stereo_bin[2] = avoid_navigation_data.stereo_bin[0] > 20;
113 avoid_navigation_data.stereo_bin[3] = avoid_navigation_data.mode;
114 avoid_navigation_data.stereo_bin[4] = counter;
115}
116
117void increase_nav_heading(float *heading, float increment)
118{
119 *heading = *heading + increment;
120}
121
void init_avoid_navigation()
void increase_nav_heading(float *heading, float increment)
bool obstacle_detected
void run_avoid_navigation_onvision(void)
#define NAV_LINE_AVOID_SEGMENT_LENGTH
#define POS_BFP_OF_REAL(_af)
int32_t y
North.
int32_t z
Up.
int32_t x
East.
vector in East North Up coordinates
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition state.h:814
uint32_t counter
Definition ins_flow.c:187
arch independent LED (Light Emitting Diodes) API
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:228
struct RotorcraftNavigation nav
Definition navigation.c:51
API to get/set the generic vehicle states.
interface to the TU Delft serial stereocam
struct AvoidNavigationStruct avoid_navigation_data
global VIDEO state
int int32_t
Typedef defining 32 bit int type.
float heading
Definition wedgebug.c:258