Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 */
27
30#include "generated/airframe.h"
31#include "state.h"
32#include "modules/core/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
55
56// define settings
57float oag_color_count_frac = 0.18f; // obstacle detection threshold as a fraction of total of image
58float oag_floor_count_frac = 0.05f; // floor detection threshold as a fraction of total of image
59float oag_max_speed = 0.5f; // max flight speed [m/s]
60float oag_heading_rate = RadOfDeg(20.f); // heading change setpoint for avoidance [rad/s]
61
62// define and initialise global variables
63enum navigation_state_t navigation_state = SEARCH_FOR_SAFE_HEADING; // current state in state machine
64int32_t color_count = 0; // orange color count from color filter for obstacle detection
65int32_t floor_count = 0; // green color count from color filter for floor detection
66int32_t floor_centroid = 0; // floor detector centroid in y direction (along the horizon)
67float avoidance_heading_direction = 0; // heading change direction for avoidance [rad/s]
68int16_t obstacle_free_confidence = 0; // a measure of how certain we are that the way ahead if safe.
69
70const int16_t max_trajectory_confidence = 5; // number of consecutive negative object detections to be sure we are obstacle free
71
72// This call back will be used to receive the color count from the orange detector
73#ifndef ORANGE_AVOIDER_VISUAL_DETECTION_ID
74#error This module requires two color filters, as such you have to define ORANGE_AVOIDER_VISUAL_DETECTION_ID to the orange filter
75#error Please define ORANGE_AVOIDER_VISUAL_DETECTION_ID to be COLOR_OBJECT_DETECTION1_ID or COLOR_OBJECT_DETECTION2_ID in your airframe
76#endif
78static void color_detection_cb(uint8_t __attribute__((unused)) sender_id,
79 int16_t __attribute__((unused)) pixel_x, int16_t __attribute__((unused)) pixel_y,
80 int16_t __attribute__((unused)) pixel_width, int16_t __attribute__((unused)) pixel_height,
81 int32_t quality, int16_t __attribute__((unused)) extra)
82{
83 color_count = quality;
84}
85
86#ifndef FLOOR_VISUAL_DETECTION_ID
87#error This module requires two color filters, as such you have to define FLOOR_VISUAL_DETECTION_ID to the orange filter
88#error Please define FLOOR_VISUAL_DETECTION_ID to be COLOR_OBJECT_DETECTION1_ID or COLOR_OBJECT_DETECTION2_ID in your airframe
89#endif
91static void floor_detection_cb(uint8_t __attribute__((unused)) sender_id,
92 int16_t __attribute__((unused)) pixel_x, int16_t pixel_y,
93 int16_t __attribute__((unused)) pixel_width, int16_t __attribute__((unused)) pixel_height,
94 int32_t quality, int16_t __attribute__((unused)) extra)
95{
96 floor_count = quality;
97 floor_centroid = pixel_y;
98}
99
100/*
101 * Initialisation function
102 */
104{
105 // Initialise random values
106 srand(time(NULL));
108
109 // bind our colorfilter callbacks to receive the color filter outputs
111 AbiBindMsgVISUAL_DETECTION(FLOOR_VISUAL_DETECTION_ID, &floor_detection_ev, floor_detection_cb);
112}
113
114/*
115 * Fly backwards until the flight plan leaves guided mode (e.g. after a timeout)
116 */
121
122/*
123 * Function that checks it is safe to move forwards, and then sets a forward velocity setpoint or changes the heading
124 */
126{
127 // Only run the mudule if we are in the correct flight mode
131 return;
132 }
133
134 // compute current color thresholds
135 int32_t color_count_threshold = oag_color_count_frac * front_camera.output_size.w * front_camera.output_size.h;
136 int32_t floor_count_threshold = oag_floor_count_frac * front_camera.output_size.w * front_camera.output_size.h;
137 float floor_centroid_frac = floor_centroid / (float)front_camera.output_size.h / 2.f;
138
139 VERBOSE_PRINT("Color_count: %d threshold: %d state: %d \n", color_count, color_count_threshold, navigation_state);
140 VERBOSE_PRINT("Floor count: %d, threshold: %d\n", floor_count, floor_count_threshold);
141 VERBOSE_PRINT("Floor centroid: %f\n", floor_centroid_frac);
142
143 // update our safe confidence using color threshold
144 if(color_count < color_count_threshold){
146 } else {
147 obstacle_free_confidence -= 2; // be more cautious with positive obstacle detections
148 }
149
150 // bound obstacle_free_confidence
152
153 float speed_sp = fminf(oag_max_speed, 0.2f * obstacle_free_confidence);
154
155 switch (navigation_state){
156 case SAFE:
157 if (floor_count < floor_count_threshold || fabsf(floor_centroid_frac) > 0.12){
159 } else if (obstacle_free_confidence == 0){
161 } else {
163 }
164
165 break;
166 case OBSTACLE_FOUND:
167 // stop
169
170 // randomly select new search direction
172
174
175 break;
178
179 // make sure we have a couple of good readings before declaring the way safe
180 if (obstacle_free_confidence >= 2){
183 }
184 break;
185 case OUT_OF_BOUNDS:
186 // stop
188
189 // start turn back into arena
191
193
194 break;
195 case REENTER_ARENA:
196 // force floor center to opposite side of turn to head back into arena
197 if (floor_count >= floor_count_threshold && avoidance_heading_direction * floor_centroid_frac >= 0.f){
198 // return to heading mode
200
201 // reset safe counter
203
204 // ensure direction is safe before continuing
206 }
207 break;
208 case RETREAT:
210 break;
211 default:
212 break;
213 }
214 return;
215}
216
217/*
218 * Sets the variable 'incrementForAvoidance' randomly positive/negative
219 */
221{
222 // Randomly choose CW or CCW avoiding direction
223 if (rand() % 2 == 0) {
225 VERBOSE_PRINT("Set avoidance increment to: %f\n", avoidance_heading_direction * oag_heading_rate);
226 } else {
228 VERBOSE_PRINT("Set avoidance increment to: %f\n", avoidance_heading_direction * oag_heading_rate);
229 }
230 return false;
231}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
volatile int color_count
Definition colorfilter.c:54
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
struct FloatVect3 speed_sp
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)
static abi_event color_detection_ev
int16_t obstacle_free_confidence
const int16_t max_trajectory_confidence
navigation_state_t
@ OUT_OF_BOUNDS
@ SAFE
@ OBSTACLE_FOUND
#define ORANGE_AVOIDER_VISUAL_DETECTION_ID
#define front_camera
Definition mt9f002_nps.c:3
@ SEARCH_FOR_SAFE_HEADING
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(...)
uint8_t chooseRandomIncrementAvoidance(void)
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)
void orange_avoider_guided_periodic(void)
float oag_floor_count_frac
int32_t floor_count
float avoidance_heading_direction
static abi_event floor_detection_ev
void orange_avoider_guided_retreat(void)
float oag_color_count_frac
float oag_max_speed
@ REENTER_ARENA
float oag_heading_rate
void orange_avoider_guided_init(void)
void guidance_h_set_heading(float heading)
Set heading setpoint.
Definition guidance_h.c:487
struct HorizontalGuidance guidance_h
Definition guidance_h.c:45
void guidance_h_set_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint.
Definition guidance_h.c:494
void guidance_h_set_heading_rate(float rate)
Set heading rate setpoint.
Definition guidance_h.c:530
Horizontal guidance for rotorcrafts.
#define GUIDANCE_H_MODE_GUIDED
Definition guidance_h.h:59
API to get/set the generic vehicle states.
int int32_t
Typedef defining 32 bit int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
navigation_state
Camera focal length, in pixels (i.e. distance between camera.
Definition wedgebug.c:209