Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 */
19
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#include "generated/flight_plan.h"
29
30#define ORANGE_AVOIDER_VERBOSE TRUE
31
32#define PRINT(string,...) fprintf(stderr, "[orange_avoider->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
33#if ORANGE_AVOIDER_VERBOSE
34#define VERBOSE_PRINT PRINT
35#else
36#define VERBOSE_PRINT(...)
37#endif
38
39static uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters);
40static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters);
41static uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor);
42static uint8_t increase_nav_heading(float incrementDegrees);
44
51
52// define settings
53float oa_color_count_frac = 0.18f;
54
55// define and initialise global variables
57int32_t color_count = 0; // orange color count from color filter for obstacle detection
58int16_t obstacle_free_confidence = 0; // a measure of how certain we are that the way ahead is safe.
59float heading_increment = 5.f; // heading angle increment [deg]
60float maxDistance = 2.25; // max waypoint displacement [m]
61
62const int16_t max_trajectory_confidence = 5; // number of consecutive negative object detections to be sure we are obstacle free
63
64/*
65 * This next section defines an ABI messaging event (http://wiki.paparazziuav.org/wiki/ABI), necessary
66 * any time data calculated in another module needs to be accessed. Including the file where this external
67 * data is defined is not enough, since modules are executed parallel to each other, at different frequencies,
68 * in different threads. The ABI event is triggered every time new data is sent out, and as such the function
69 * defined in this file does not need to be explicitly called, only bound in the init function
70 */
71#ifndef ORANGE_AVOIDER_VISUAL_DETECTION_ID
72#define ORANGE_AVOIDER_VISUAL_DETECTION_ID ABI_BROADCAST
73#endif
75static void color_detection_cb(uint8_t __attribute__((unused)) sender_id,
76 int16_t __attribute__((unused)) pixel_x, int16_t __attribute__((unused)) pixel_y,
77 int16_t __attribute__((unused)) pixel_width, int16_t __attribute__((unused)) pixel_height,
78 int32_t quality, int16_t __attribute__((unused)) extra)
79{
80 color_count = quality;
81}
82
83/*
84 * Initialisation function, setting the colour filter, random seed and heading_increment
85 */
87{
88 // Initialise random values
89 srand(time(NULL));
91
92 // bind our colorfilter callbacks to receive the color filter outputs
94}
95
96/*
97 * Function that checks it is safe to move forwards, and then moves a waypoint forward or changes the heading
98 */
100{
101 // only evaluate our state machine if we are flying
102 if(!autopilot_in_flight()){
103 return;
104 }
105
106 // compute current color thresholds
107 int32_t color_count_threshold = oa_color_count_frac * front_camera.output_size.w * front_camera.output_size.h;
108
109 VERBOSE_PRINT("Color_count: %d threshold: %d state: %d \n", color_count, color_count_threshold, navigation_state);
110
111 // update our safe confidence using color threshold
112 if(color_count < color_count_threshold){
114 } else {
115 obstacle_free_confidence -= 2; // be more cautious with positive obstacle detections
116 }
117
118 // bound obstacle_free_confidence
120
122
123 switch (navigation_state){
124 case SAFE:
125 // Move waypoint forward
126 moveWaypointForward(WP_TRAJECTORY, 1.5f * moveDistance);
127 if (!InsideObstacleZone(WaypointX(WP_TRAJECTORY),WaypointY(WP_TRAJECTORY))){
129 } else if (obstacle_free_confidence == 0){
131 } else {
133 moveWaypointForward(WP_RETREAT, -1.0f * moveDistance);
134 }
135
136 break;
137 case OBSTACLE_FOUND:
138 // stop
139 waypoint_move_here_2d(WP_GOAL);
140 waypoint_move_here_2d(WP_RETREAT);
141 waypoint_move_here_2d(WP_TRAJECTORY);
142
143 // randomly select new search direction
145
147
148 break;
151
152 // make sure we have a couple of good readings before declaring the way safe
153 if (obstacle_free_confidence >= 2){
155 }
156 break;
157 case OUT_OF_BOUNDS:
159 moveWaypointForward(WP_TRAJECTORY, 1.5f);
160 moveWaypointForward(WP_RETREAT, -1.0f);
161
162 if (InsideObstacleZone(WaypointX(WP_TRAJECTORY),WaypointY(WP_TRAJECTORY))){
163 // add offset to head back into arena
165
166 // reset safe counter
168
169 // ensure direction is safe before continuing
171 }
172 break;
173 default:
174 break;
175 }
176 return;
177}
178
179/*
180 * Increases the NAV heading. Assumes heading is an INT32_ANGLE. It is bound in this function.
181 */
182uint8_t increase_nav_heading(float incrementDegrees)
183{
184 float new_heading = stateGetNedToBodyEulers_f()->psi + RadOfDeg(incrementDegrees);
185
186 // normalize heading to [-pi, pi]
188
189 // set heading, declared in firmwares/rotorcraft/navigation.h
190 nav.heading = new_heading;
191
192 VERBOSE_PRINT("Increasing heading to %f\n", DegOfRad(new_heading));
193 return false;
194}
195
196/*
197 * Calculates coordinates of distance forward and sets waypoint 'waypoint' to those coordinates
198 */
199uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
200{
201 struct EnuCoor_i new_coor;
202 calculateForwards(&new_coor, distanceMeters);
203 moveWaypoint(waypoint, &new_coor);
204 return false;
205}
206
207/*
208 * Calculates coordinates of a distance of 'distanceMeters' forward w.r.t. current position and heading
209 */
210uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
211{
213
214 // Now determine where to place the waypoint you want to go to
215 new_coor->x = stateGetPositionEnu_i()->x + POS_BFP_OF_REAL(sinf(heading) * (distanceMeters));
216 new_coor->y = stateGetPositionEnu_i()->y + POS_BFP_OF_REAL(cosf(heading) * (distanceMeters));
217 VERBOSE_PRINT("Calculated %f m forward position. x: %f y: %f based on pos(%f, %f) and heading(%f)\n", distanceMeters,
218 POS_FLOAT_OF_BFP(new_coor->x), POS_FLOAT_OF_BFP(new_coor->y),
220 return false;
221}
222
223/*
224 * Sets waypoint 'waypoint' to the coordinates of 'new_coor'
225 */
226uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor)
227{
228 VERBOSE_PRINT("Moving waypoint %d to x:%f y:%f\n", waypoint, POS_FLOAT_OF_BFP(new_coor->x),
229 POS_FLOAT_OF_BFP(new_coor->y));
230 waypoint_move_xy_i(waypoint, new_coor->x, new_coor->y);
231 return false;
232}
233
234/*
235 * Sets the variable 'heading_increment' randomly positive/negative
236 */
238{
239 // Randomly choose CW or CCW avoiding direction
240 if (rand() % 2 == 0) {
241 heading_increment = 5.f;
242 VERBOSE_PRINT("Set avoidance increment to: %f\n", heading_increment);
243 } else {
244 heading_increment = -5.f;
245 VERBOSE_PRINT("Set avoidance increment to: %f\n", heading_increment);
246 }
247 return false;
248}
249
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:351
volatile int color_count
Definition colorfilter.c:54
#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:1325
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition state.h:814
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
float new_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)
static abi_event color_detection_ev
float moveDistance
int16_t obstacle_free_confidence
float oa_color_count_frac
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
void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Definition waypoints.c:240
void waypoint_move_here_2d(uint8_t wp_id)
Definition waypoints.c:332
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)
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)
static uint8_t increase_nav_heading(float incrementDegrees)
@ SEARCH_FOR_SAFE_HEADING
void orange_avoider_init(void)
float maxDistance
struct RotorcraftNavigation nav
Definition navigation.c:51
Rotorcraft navigation functions.
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
float heading
Definition wedgebug.c:258