Paparazzi UAS  v5.15_devel-81-gd13dafb
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 funcitons 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 uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters);
41 uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor);
42 uint8_t increase_nav_heading(float incrementDegrees);
44 
50 };
51 
52 // define settings
53 float oa_color_count_frac = 0.18f;
54 
55 // define and initialise global variables
57 int32_t color_count = 0; // orange color count from color filter for obstacle detection
58 int16_t obstacle_free_confidence = 0; // a measure of how certain we are that the way ahead is safe.
59 float heading_increment = 5.f; // heading angle increment [deg]
60 float maxDistance = 2.25; // max waypoint displacement [m]
61 
62 const int16_t max_trajectory_confidence = 5; // number of consecutive negative object detections to be sure we are obstacle free
63 
64 #ifndef ORANGE_AVOIDER_VISUAL_DETECTION_ID
65 #define ORANGE_AVOIDER_VISUAL_DETECTION_ID ABI_BROADCAST
66 #endif
68 static void color_detection_cb(uint8_t __attribute__((unused)) sender_id,
69  int16_t __attribute__((unused)) pixel_x, int16_t __attribute__((unused)) pixel_y,
70  int16_t __attribute__((unused)) pixel_width, int16_t __attribute__((unused)) pixel_height,
71  int32_t quality, int16_t __attribute__((unused)) extra)
72 {
73  color_count = quality;
74 }
75 
76 /*
77  * Initialisation function, setting the colour filter, random seed and heading_increment
78  */
80 {
81  // Initialise random values
82  srand(time(NULL));
84 
85  // bind our colorfilter callbacks to receive the color filter outputs
86  AbiBindMsgVISUAL_DETECTION(ORANGE_AVOIDER_VISUAL_DETECTION_ID, &color_detection_ev, color_detection_cb);
87 }
88 
89 /*
90  * Function that checks it is safe to move forwards, and then moves a waypoint forward or changes the heading
91  */
93 {
94  // only evaluate our state machine if we are flying
95  if(!autopilot_in_flight()){
96  return;
97  }
98 
99  // compute current color thresholds
100  int32_t color_count_threshold = oa_color_count_frac * front_camera.output_size.w * front_camera.output_size.h;
101 
102  VERBOSE_PRINT("Color_count: %d threshold: %d state: %d \n", color_count, color_count, navigation_state);
103 
104  // update our safe confidence using color threshold
105  if(color_count < color_count_threshold){
107  } else {
108  obstacle_free_confidence -= 2; // be more cautious with positive obstacle detections
109  }
110 
111  // bound obstacle_free_confidence
113 
114  float moveDistance = fminf(maxDistance, 0.2f * obstacle_free_confidence);
115 
116  switch (navigation_state){
117  case SAFE:
118  // Move waypoint forward
119  moveWaypointForward(WP_TRAJECTORY, 1.5f * moveDistance);
120  if (!InsideObstacleZone(WaypointX(WP_TRAJECTORY),WaypointY(WP_TRAJECTORY))){
122  } else if (obstacle_free_confidence == 0){
124  } else {
125  moveWaypointForward(WP_GOAL, moveDistance);
126  }
127 
128  break;
129  case OBSTACLE_FOUND:
130  // stop
131  waypoint_move_here_2d(WP_GOAL);
132  waypoint_move_here_2d(WP_TRAJECTORY);
133 
134  // randomly select new search direction
136 
138 
139  break;
142 
143  // make sure we have a couple of good readings before declaring the way safe
144  if (obstacle_free_confidence >= 2){
146  }
147  break;
148  case OUT_OF_BOUNDS:
150  moveWaypointForward(WP_TRAJECTORY, 1.5f);
151 
152  if (InsideObstacleZone(WaypointX(WP_TRAJECTORY),WaypointY(WP_TRAJECTORY))){
153  // add offset to head back into arena
155 
156  // reset safe counter
157  obstacle_free_confidence = 0;
158 
159  // ensure direction is safe before continuing
161  }
162  break;
163  default:
164  break;
165  }
166  return;
167 }
168 
169 /*
170  * Increases the NAV heading. Assumes heading is an INT32_ANGLE. It is bound in this function.
171  */
172 uint8_t increase_nav_heading(float incrementDegrees)
173 {
174  float new_heading = stateGetNedToBodyEulers_f()->psi + RadOfDeg(incrementDegrees);
175 
176  // normalize heading to [-pi, pi]
177  FLOAT_ANGLE_NORMALIZE(new_heading);
178 
179  // set heading
180  nav_heading = ANGLE_BFP_OF_REAL(new_heading);
181 
182  VERBOSE_PRINT("Increasing heading to %f\n", DegOfRad(new_heading));
183  return false;
184 }
185 
186 /*
187  * Calculates coordinates of a distance of 'distanceMeters' forward w.r.t. current position and heading
188  */
189 static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
190 {
192 
193  // Now determine where to place the waypoint you want to go to
194  new_coor->x = stateGetPositionEnu_i()->x + POS_BFP_OF_REAL(sinf(heading) * (distanceMeters));
195  new_coor->y = stateGetPositionEnu_i()->y + POS_BFP_OF_REAL(cosf(heading) * (distanceMeters));
196  VERBOSE_PRINT("Calculated %f m forward position. x: %f y: %f based on pos(%f, %f) and heading(%f)\n", distanceMeters,
197  POS_FLOAT_OF_BFP(new_coor->x), POS_FLOAT_OF_BFP(new_coor->y),
198  stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, DegOfRad(heading));
199  return false;
200 }
201 
202 /*
203  * Sets waypoint 'waypoint' to the coordinates of 'new_coor'
204  */
205 uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor)
206 {
207  VERBOSE_PRINT("Moving waypoint %d to x:%f y:%f\n", waypoint, POS_FLOAT_OF_BFP(new_coor->x),
208  POS_FLOAT_OF_BFP(new_coor->y));
209  waypoint_move_xy_i(waypoint, new_coor->x, new_coor->y);
210  return false;
211 }
212 
213 /*
214  * Calculates coordinates of distance forward and sets waypoint 'waypoint' to those coordinates
215  */
216 uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
217 {
218  struct EnuCoor_i new_coor;
219  calculateForwards(&new_coor, distanceMeters);
220  moveWaypoint(waypoint, &new_coor);
221  return false;
222 }
223 
224 /*
225  * Sets the variable 'heading_increment' randomly positive/negative
226  */
228 {
229  // Randomly choose CW or CCW avoiding direction
230  if (rand() % 2 == 0) {
231  heading_increment = 5.f;
232  VERBOSE_PRINT("Set avoidance increment to: %f\n", heading_increment);
233  } else {
234  heading_increment = -5.f;
235  VERBOSE_PRINT("Set avoidance increment to: %f\n", heading_increment);
236  }
237  return false;
238 }
239 
#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.
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)
uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
Main include for ABI (AirBorneInterface).
void waypoint_move_here_2d(uint8_t wp_id)
Definition: waypoints.c:246
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_t
const int16_t max_trajectory_confidence
static float heading
Definition: ahrs_infrared.c:45
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:104
#define VERBOSE_PRINT(...)
signed short int16_t
Definition: types.h:17
#define WaypointY(_wp)
Definition: common_nav.h:46
signed long int32_t
Definition: types.h:19
uint8_t chooseRandomIncrementAvoidance(void)
#define ANGLE_BFP_OF_REAL(_af)
float heading_increment
Rotorcraft navigation functions.
vector in East North Up coordinates
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor)
int32_t color_count
uint8_t increase_nav_heading(float incrementDegrees)
#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
float new_heading
Definition: guidance_OA.c:114
enum navigation_state_t navigation_state