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_guided.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Kirk Scheper <kirkscheper@gmail.com>
3  *
4  * This file is part of paparazzi
5  *
6  */
30 #include "generated/airframe.h"
31 #include "state.h"
32 #include "subsystems/abi.h"
33 #include <stdio.h>
34 #include <time.h>
35 
36 #define ORANGE_AVOIDER_VERBOSE TRUE
37 
38 #define PRINT(string,...) fprintf(stderr, "[orange_avoider_guided->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
39 #if ORANGE_AVOIDER_VERBOSE
40 #define VERBOSE_PRINT PRINT
41 #else
42 #define VERBOSE_PRINT(...)
43 #endif
44 
46 
53 };
54 
55 // define settings
56 float oag_color_count_frac = 0.18f; // obstacle detection threshold as a fraction of total of image
57 float oag_floor_count_frac = 0.05f; // floor detection threshold as a fraction of total of image
58 float oag_max_speed = 0.5f; // max flight speed [m/s]
59 float oag_heading_rate = RadOfDeg(20.f); // heading change setpoint for avoidance [rad/s]
60 
61 // define and initialise global variables
62 enum navigation_state_t navigation_state = SEARCH_FOR_SAFE_HEADING; // current state in state machine
63 int32_t color_count = 0; // orange color count from color filter for obstacle detection
64 int32_t floor_count = 0; // green color count from color filter for floor detection
65 int32_t floor_centroid = 0; // floor detector centroid in y direction (along the horizon)
66 float avoidance_heading_direction = 0; // heading change direction for avoidance [rad/s]
67 int16_t obstacle_free_confidence = 0; // a measure of how certain we are that the way ahead if safe.
68 
69 const int16_t max_trajectory_confidence = 5; // number of consecutive negative object detections to be sure we are obstacle free
70 
71 // This call back will be used to receive the color count from the orange detector
72 #ifndef ORANGE_AVOIDER_VISUAL_DETECTION_ID
73 #error This module requires two color filters, as such you have to define ORANGE_AVOIDER_VISUAL_DETECTION_ID to the orange filter
74 #error Please define ORANGE_AVOIDER_VISUAL_DETECTION_ID to be COLOR_OBJECT_DETECTION1_ID or COLOR_OBJECT_DETECTION2_ID in your airframe
75 #endif
77 static void color_detection_cb(uint8_t __attribute__((unused)) sender_id,
78  int16_t __attribute__((unused)) pixel_x, int16_t __attribute__((unused)) pixel_y,
79  int16_t __attribute__((unused)) pixel_width, int16_t __attribute__((unused)) pixel_height,
80  int32_t quality, int16_t __attribute__((unused)) extra)
81 {
82  color_count = quality;
83 }
84 
85 #ifndef FLOOR_VISUAL_DETECTION_ID
86 #error This module requires two color filters, as such you have to define FLOOR_VISUAL_DETECTION_ID to the orange filter
87 #error Please define FLOOR_VISUAL_DETECTION_ID to be COLOR_OBJECT_DETECTION1_ID or COLOR_OBJECT_DETECTION2_ID in your airframe
88 #endif
90 static void floor_detection_cb(uint8_t __attribute__((unused)) sender_id,
91  int16_t __attribute__((unused)) pixel_x, int16_t pixel_y,
92  int16_t __attribute__((unused)) pixel_width, int16_t __attribute__((unused)) pixel_height,
93  int32_t quality, int16_t __attribute__((unused)) extra)
94 {
95  floor_count = quality;
96  floor_centroid = pixel_y;
97 }
98 
99 /*
100  * Initialisation function
101  */
103 {
104  // Initialise random values
105  srand(time(NULL));
107 
108  // bind our colorfilter callbacks to receive the color filter outputs
109  AbiBindMsgVISUAL_DETECTION(ORANGE_AVOIDER_VISUAL_DETECTION_ID, &color_detection_ev, color_detection_cb);
110  AbiBindMsgVISUAL_DETECTION(FLOOR_VISUAL_DETECTION_ID, &floor_detection_ev, floor_detection_cb);
111 }
112 
113 /*
114  * Function that checks it is safe to move forwards, and then sets a forward velocity setpoint or changes the heading
115  */
117 {
118  // Only run the mudule if we are in the correct flight mode
122  return;
123  }
124 
125  // compute current color thresholds
126  int32_t color_count_threshold = oag_color_count_frac * front_camera.output_size.w * front_camera.output_size.h;
127  int32_t floor_count_threshold = oag_floor_count_frac * front_camera.output_size.w * front_camera.output_size.h;
128  float floor_centroid_frac = floor_centroid / (float)front_camera.output_size.h / 2.f;
129 
130  VERBOSE_PRINT("Color_count: %d threshold: %d state: %d \n", color_count, color_count_threshold, navigation_state);
131  VERBOSE_PRINT("Floor count: %d, threshold: %d\n", floor_count, floor_count_threshold);
132  VERBOSE_PRINT("Floor centroid: %f\n", floor_centroid_frac);
133 
134  // update our safe confidence using color threshold
135  if(color_count < color_count_threshold){
137  } else {
138  obstacle_free_confidence -= 2; // be more cautious with positive obstacle detections
139  }
140 
141  // bound obstacle_free_confidence
143 
144  float speed_sp = fminf(oag_max_speed, 0.2f * obstacle_free_confidence);
145 
146  switch (navigation_state){
147  case SAFE:
148  if (floor_count < floor_count_threshold || fabsf(floor_centroid_frac) > 0.12){
150  } else if (obstacle_free_confidence == 0){
152  } else {
153  guidance_h_set_guided_body_vel(speed_sp, 0);
154  }
155 
156  break;
157  case OBSTACLE_FOUND:
158  // stop
160 
161  // randomly select new search direction
163 
165 
166  break;
169 
170  // make sure we have a couple of good readings before declaring the way safe
171  if (obstacle_free_confidence >= 2){
174  }
175  break;
176  case OUT_OF_BOUNDS:
177  // stop
179 
180  // start turn back into arena
182 
184 
185  break;
186  case REENTER_ARENA:
187  // force floor center to opposite side of turn to head back into arena
188  if (floor_count >= floor_count_threshold && avoidance_heading_direction * floor_centroid_frac >= 0.f){
189  // return to heading mode
191 
192  // reset safe counter
193  obstacle_free_confidence = 0;
194 
195  // ensure direction is safe before continuing
197  }
198  break;
199  default:
200  break;
201  }
202  return;
203 }
204 
205 /*
206  * Sets the variable 'incrementForAvoidance' randomly positive/negative
207  */
209 {
210  // Randomly choose CW or CCW avoiding direction
211  if (rand() % 2 == 0) {
213  VERBOSE_PRINT("Set avoidance increment to: %f\n", avoidance_heading_direction * oag_heading_rate);
214  } else {
216  VERBOSE_PRINT("Set avoidance increment to: %f\n", avoidance_heading_direction * oag_heading_rate);
217  }
218  return false;
219 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
float oag_heading_rate
uint8_t chooseRandomIncrementAvoidance(void)
#define GUIDANCE_H_MODE_GUIDED
Definition: guidance_h.h:66
#define VERBOSE_PRINT(...)
#define ORANGE_AVOIDER_VISUAL_DETECTION_ID
bool guidance_h_set_guided_heading_rate(float rate)
Set heading rate setpoint in GUIDED mode.
Definition: guidance_h.c:738
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
Main include for ABI (AirBorneInterface).
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)
bool guidance_h_set_guided_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint in GUIDED mode.
Definition: guidance_h.c:719
float oag_color_count_frac
enum navigation_state_t navigation_state
static abi_event color_detection_ev
navigation_state_t
float oag_floor_count_frac
int16_t obstacle_free_confidence
float avoidance_heading_direction
signed short int16_t
Definition: types.h:17
int32_t floor_count
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:80
int32_t color_count
signed long int32_t
Definition: types.h:19
static abi_event floor_detection_ev
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float oag_max_speed
const int16_t max_trajectory_confidence
void orange_avoider_guided_init(void)
bool guidance_h_set_guided_heading(float heading)
Set heading setpoint in GUIDED mode.
Definition: guidance_h.c:708
void orange_avoider_guided_periodic(void)
Horizontal guidance for rotorcrafts.
#define front_camera
Definition: mt9f002_nps.c:3
int32_t floor_centroid
static void floor_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)