Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/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
94  AbiBindMsgVISUAL_DETECTION(ORANGE_AVOIDER_VISUAL_DETECTION_ID, &color_detection_ev, color_detection_cb);
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 {
133  moveWaypointForward(WP_GOAL, moveDistance);
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
165  obstacle_free_confidence = 0;
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]
185  FLOAT_ANGLE_NORMALIZE(new_heading);
186 
187  // set heading, declared in firmwares/rotorcraft/navigation.h
188  // for performance reasons the navigation variables are stored and processed in Binary Fixed-Point format
189  nav_heading = ANGLE_BFP_OF_REAL(new_heading);
190 
191  VERBOSE_PRINT("Increasing heading to %f\n", DegOfRad(new_heading));
192  return false;
193 }
194 
195 /*
196  * Calculates coordinates of distance forward and sets waypoint 'waypoint' to those coordinates
197  */
198 uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
199 {
200  struct EnuCoor_i new_coor;
201  calculateForwards(&new_coor, distanceMeters);
202  moveWaypoint(waypoint, &new_coor);
203  return false;
204 }
205 
206 /*
207  * Calculates coordinates of a distance of 'distanceMeters' forward w.r.t. current position and heading
208  */
209 uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
210 {
212 
213  // Now determine where to place the waypoint you want to go to
214  new_coor->x = stateGetPositionEnu_i()->x + POS_BFP_OF_REAL(sinf(heading) * (distanceMeters));
215  new_coor->y = stateGetPositionEnu_i()->y + POS_BFP_OF_REAL(cosf(heading) * (distanceMeters));
216  VERBOSE_PRINT("Calculated %f m forward position. x: %f y: %f based on pos(%f, %f) and heading(%f)\n", distanceMeters,
217  POS_FLOAT_OF_BFP(new_coor->x), POS_FLOAT_OF_BFP(new_coor->y),
218  stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, DegOfRad(heading));
219  return false;
220 }
221 
222 /*
223  * Sets waypoint 'waypoint' to the coordinates of 'new_coor'
224  */
225 uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor)
226 {
227  VERBOSE_PRINT("Moving waypoint %d to x:%f y:%f\n", waypoint, POS_FLOAT_OF_BFP(new_coor->x),
228  POS_FLOAT_OF_BFP(new_coor->y));
229  waypoint_move_xy_i(waypoint, new_coor->x, new_coor->y);
230  return false;
231 }
232 
233 /*
234  * Sets the variable 'heading_increment' randomly positive/negative
235  */
237 {
238  // Randomly choose CW or CCW avoiding direction
239  if (rand() % 2 == 0) {
240  heading_increment = 5.f;
241  VERBOSE_PRINT("Set avoidance increment to: %f\n", heading_increment);
242  } else {
243  heading_increment = -5.f;
244  VERBOSE_PRINT("Set avoidance increment to: %f\n", heading_increment);
245  }
246  return false;
247 }
248 
#define FLOAT_ANGLE_NORMALIZE(_a)
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
void orange_avoider_init(void)
#define ORANGE_AVOIDER_VISUAL_DETECTION_ID
int32_t y
North.
int32_t x
East.
float heading
Definition: wedgebug.c:258
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
int16_t obstacle_free_confidence
#define POS_BFP_OF_REAL(_af)
Main include for ABI (AirBorneInterface).
void waypoint_move_here_2d(uint8_t wp_id)
Definition: waypoints.c:246
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
float psi
in radians
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
navigation_state
Camera focal length, in pixels (i.e. distance between camera.
Definition: wedgebug.c:209
const int16_t max_trajectory_confidence
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 WaypointX(_wp)
Definition: common_nav.h:45
static abi_event color_detection_ev
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:257
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:108
#define VERBOSE_PRINT(...)
float moveDistance
Definition: mav_exercise.c:49
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
#define WaypointY(_wp)
Definition: common_nav.h:46
static uint8_t chooseRandomIncrementAvoidance(void)
#define ANGLE_BFP_OF_REAL(_af)
float heading_increment
Rotorcraft navigation functions.
vector in East North Up coordinates
API to get/set the generic vehicle states.
static uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
int32_t color_count
static uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor)
static uint8_t increase_nav_heading(float incrementDegrees)
navigation_state_t
Definition: mav_exercise.c:37
#define POS_FLOAT_OF_BFP(_ai)
void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Definition: waypoints.c:150
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
#define front_camera
Definition: mt9f002_nps.c:3
void orange_avoider_periodic(void)
float oa_color_count_frac
float maxDistance
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float new_heading
Definition: guidance_OA.c:114