Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
orange_avoider.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Roland Meertens
3  *
4  * This file is part of paparazzi
5  *
6  */
22 #include "generated/airframe.h"
23 #include "state.h"
24 #include "modules/core/abi.h"
25 #include <time.h>
26 #include <stdio.h>
27 
28 #define NAV_C // needed to get the nav functions like Inside...
29 #include "generated/flight_plan.h"
30 
31 #define ORANGE_AVOIDER_VERBOSE TRUE
32 
33 #define PRINT(string,...) fprintf(stderr, "[orange_avoider->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
34 #if ORANGE_AVOIDER_VERBOSE
35 #define VERBOSE_PRINT PRINT
36 #else
37 #define VERBOSE_PRINT(...)
38 #endif
39 
40 static uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters);
41 static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters);
42 static uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor);
43 static uint8_t increase_nav_heading(float incrementDegrees);
45 
51 };
52 
53 // define settings
54 float oa_color_count_frac = 0.18f;
55 
56 // define and initialise global variables
58 int32_t color_count = 0; // orange color count from color filter for obstacle detection
59 int16_t obstacle_free_confidence = 0; // a measure of how certain we are that the way ahead is safe.
60 float heading_increment = 5.f; // heading angle increment [deg]
61 float maxDistance = 2.25; // max waypoint displacement [m]
62 
63 const int16_t max_trajectory_confidence = 5; // number of consecutive negative object detections to be sure we are obstacle free
64 
65 /*
66  * This next section defines an ABI messaging event (http://wiki.paparazziuav.org/wiki/ABI), necessary
67  * any time data calculated in another module needs to be accessed. Including the file where this external
68  * data is defined is not enough, since modules are executed parallel to each other, at different frequencies,
69  * in different threads. The ABI event is triggered every time new data is sent out, and as such the function
70  * defined in this file does not need to be explicitly called, only bound in the init function
71  */
72 #ifndef ORANGE_AVOIDER_VISUAL_DETECTION_ID
73 #define ORANGE_AVOIDER_VISUAL_DETECTION_ID ABI_BROADCAST
74 #endif
76 static void color_detection_cb(uint8_t __attribute__((unused)) sender_id,
77  int16_t __attribute__((unused)) pixel_x, int16_t __attribute__((unused)) pixel_y,
78  int16_t __attribute__((unused)) pixel_width, int16_t __attribute__((unused)) pixel_height,
79  int32_t quality, int16_t __attribute__((unused)) extra)
80 {
81  color_count = quality;
82 }
83 
84 /*
85  * Initialisation function, setting the colour filter, random seed and heading_increment
86  */
88 {
89  // Initialise random values
90  srand(time(NULL));
92 
93  // bind our colorfilter callbacks to receive the color filter outputs
95 }
96 
97 /*
98  * Function that checks it is safe to move forwards, and then moves a waypoint forward or changes the heading
99  */
101 {
102  // only evaluate our state machine if we are flying
103  if(!autopilot_in_flight()){
104  return;
105  }
106 
107  // compute current color thresholds
108  int32_t color_count_threshold = oa_color_count_frac * front_camera.output_size.w * front_camera.output_size.h;
109 
110  VERBOSE_PRINT("Color_count: %d threshold: %d state: %d \n", color_count, color_count_threshold, navigation_state);
111 
112  // update our safe confidence using color threshold
113  if(color_count < color_count_threshold){
115  } else {
116  obstacle_free_confidence -= 2; // be more cautious with positive obstacle detections
117  }
118 
119  // bound obstacle_free_confidence
121 
122  float moveDistance = fminf(maxDistance, 0.2f * obstacle_free_confidence);
123 
124  switch (navigation_state){
125  case SAFE:
126  // Move waypoint forward
127  moveWaypointForward(WP_TRAJECTORY, 1.5f * moveDistance);
128  if (!InsideObstacleZone(WaypointX(WP_TRAJECTORY),WaypointY(WP_TRAJECTORY))){
130  } else if (obstacle_free_confidence == 0){
132  } else {
134  }
135 
136  break;
137  case OBSTACLE_FOUND:
138  // stop
139  waypoint_move_here_2d(WP_GOAL);
140  waypoint_move_here_2d(WP_TRAJECTORY);
141 
142  // randomly select new search direction
144 
146 
147  break;
150 
151  // make sure we have a couple of good readings before declaring the way safe
152  if (obstacle_free_confidence >= 2){
154  }
155  break;
156  case OUT_OF_BOUNDS:
158  moveWaypointForward(WP_TRAJECTORY, 1.5f);
159 
160  if (InsideObstacleZone(WaypointX(WP_TRAJECTORY),WaypointY(WP_TRAJECTORY))){
161  // add offset to head back into arena
163 
164  // reset safe counter
166 
167  // ensure direction is safe before continuing
169  }
170  break;
171  default:
172  break;
173  }
174  return;
175 }
176 
177 /*
178  * Increases the NAV heading. Assumes heading is an INT32_ANGLE. It is bound in this function.
179  */
180 uint8_t increase_nav_heading(float incrementDegrees)
181 {
182  float new_heading = stateGetNedToBodyEulers_f()->psi + RadOfDeg(incrementDegrees);
183 
184  // normalize heading to [-pi, pi]
186 
187  // set heading, declared in firmwares/rotorcraft/navigation.h
189 
190  VERBOSE_PRINT("Increasing heading to %f\n", DegOfRad(new_heading));
191  return false;
192 }
193 
194 /*
195  * Calculates coordinates of distance forward and sets waypoint 'waypoint' to those coordinates
196  */
197 uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
198 {
199  struct EnuCoor_i new_coor;
200  calculateForwards(&new_coor, distanceMeters);
201  moveWaypoint(waypoint, &new_coor);
202  return false;
203 }
204 
205 /*
206  * Calculates coordinates of a distance of 'distanceMeters' forward w.r.t. current position and heading
207  */
208 uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
209 {
211 
212  // Now determine where to place the waypoint you want to go to
213  new_coor->x = stateGetPositionEnu_i()->x + POS_BFP_OF_REAL(sinf(heading) * (distanceMeters));
214  new_coor->y = stateGetPositionEnu_i()->y + POS_BFP_OF_REAL(cosf(heading) * (distanceMeters));
215  VERBOSE_PRINT("Calculated %f m forward position. x: %f y: %f based on pos(%f, %f) and heading(%f)\n", distanceMeters,
216  POS_FLOAT_OF_BFP(new_coor->x), POS_FLOAT_OF_BFP(new_coor->y),
218  return false;
219 }
220 
221 /*
222  * Sets waypoint 'waypoint' to the coordinates of 'new_coor'
223  */
224 uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor)
225 {
226  VERBOSE_PRINT("Moving waypoint %d to x:%f y:%f\n", waypoint, POS_FLOAT_OF_BFP(new_coor->x),
227  POS_FLOAT_OF_BFP(new_coor->y));
228  waypoint_move_xy_i(waypoint, new_coor->x, new_coor->y);
229  return false;
230 }
231 
232 /*
233  * Sets the variable 'heading_increment' randomly positive/negative
234  */
236 {
237  // Randomly choose CW or CCW avoiding direction
238  if (rand() % 2 == 0) {
239  heading_increment = 5.f;
240  VERBOSE_PRINT("Set avoidance increment to: %f\n", heading_increment);
241  } else {
242  heading_increment = -5.f;
243  VERBOSE_PRINT("Set avoidance increment to: %f\n", heading_increment);
244  }
245  return false;
246 }
247 
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:340
#define WaypointX(_wp)
Definition: common_nav.h:45
#define WaypointY(_wp)
Definition: common_nav.h:46
float psi
in radians
#define FLOAT_ANGLE_NORMALIZE(_a)
#define POS_FLOAT_OF_BFP(_ai)
#define POS_BFP_OF_REAL(_af)
int32_t y
North.
int32_t x
East.
vector in East North Up coordinates
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
float new_heading
Definition: guidance_OA.c:114
float moveDistance
Definition: mav_exercise.c:49
navigation_state_t
Definition: mav_exercise.c:37
#define front_camera
Definition: mt9f002_nps.c:3
void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Definition: waypoints.c:214
void waypoint_move_here_2d(uint8_t wp_id)
Definition: waypoints.c:310
static void color_detection_cb(uint8_t sender_id, int16_t pixel_x, int16_t pixel_y, int16_t pixel_width, int16_t pixel_height, int32_t quality, int16_t extra)
#define VERBOSE_PRINT(...)
static uint8_t chooseRandomIncrementAvoidance(void)
static uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
static abi_event color_detection_ev
float heading_increment
static uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor)
void orange_avoider_periodic(void)
static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
int16_t obstacle_free_confidence
float oa_color_count_frac
const int16_t max_trajectory_confidence
static uint8_t increase_nav_heading(float incrementDegrees)
int32_t color_count
@ OUT_OF_BOUNDS
@ SAFE
@ SEARCH_FOR_SAFE_HEADING
@ OBSTACLE_FOUND
#define ORANGE_AVOIDER_VISUAL_DETECTION_ID
void orange_avoider_init(void)
float maxDistance
struct RotorcraftNavigation nav
Definition: navigation.c:51
Rotorcraft navigation functions.
float heading
heading setpoint (in radians)
Definition: navigation.h:133
API to get/set the generic vehicle states.
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
navigation_state
Camera focal length, in pixels (i.e. distance between camera.
Definition: wedgebug.c:209
float heading
Definition: wedgebug.c:258