Paparazzi UAS  v5.15_devel-46-gd23ed71
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  */
16 #include "generated/flight_plan.h"
17 #include "generated/airframe.h"
18 #include "state.h"
19 #include <time.h>
20 #include <math.h>
21 #include <stdio.h>
22 #include <stdlib.h>
23 
24 
25 #define ORANGE_AVOIDER_VERBOSE TRUE
26 
27 #define PRINT(string,...) fprintf(stderr, "[orange_avoider->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
28 #if ORANGE_AVOIDER_VERBOSE
29 #define VERBOSE_PRINT PRINT
30 #else
31 #define VERBOSE_PRINT(...)
32 #endif
33 
34 #ifndef ORANGE_AVOIDER_LUM_MIN
35 #define ORANGE_AVOIDER_LUM_MIN 41
36 #endif
37 
38 #ifndef ORANGE_AVOIDER_LUM_MAX
39 #define ORANGE_AVOIDER_LUM_MAX 183
40 #endif
41 
42 #ifndef ORANGE_AVOIDER_CB_MIN
43 #define ORANGE_AVOIDER_CB_MIN 53
44 #endif
45 
46 #ifndef ORANGE_AVOIDER_CB_MAX
47 #define ORANGE_AVOIDER_CB_MAX 121
48 #endif
49 
50 #ifndef ORANGE_AVOIDER_CR_MIN
51 #define ORANGE_AVOIDER_CR_MIN 134
52 #endif
53 
54 #ifndef ORANGE_AVOIDER_CR_MAX
55 #define ORANGE_AVOIDER_CR_MAX 249
56 #endif
57 
58 
60 int tresholdColorCount = 0.05 * 124800; // 520 x 240 = 124.800 total pixels
63 float maxDistance = 2.25;
64 
65 /*
66  * Initialisation function, setting the colour filter, random seed and incrementForAvoidance
67  */
69 {
70  // Initialise the variables of the colorfilter to accept orange
77  // Initialise random values
78  srand(time(NULL));
80 }
81 
82 /*
83  * Function that checks it is safe to move forwards, and then moves a waypoint forward or changes the heading
84  */
86 {
87  // Check the amount of orange. If this is above a threshold
88  // you want to turn a certain amount of degrees
90  VERBOSE_PRINT("Color_count: %d threshold: %d safe: %d \n", color_count, tresholdColorCount, safeToGoForwards);
91  float moveDistance = fmin(maxDistance, 0.05 * trajectoryConfidence);
92  if (safeToGoForwards) {
93  moveWaypointForward(WP_GOAL, moveDistance);
94  moveWaypointForward(WP_TRAJECTORY, 1.25 * moveDistance);
98  } else {
99  waypoint_set_here_2d(WP_GOAL);
100  waypoint_set_here_2d(WP_TRAJECTORY);
102  if (trajectoryConfidence > 5) {
104  } else {
106  }
107  }
108  return;
109 }
110 
111 /*
112  * Increases the NAV heading. Assumes heading is an INT32_ANGLE. It is bound in this function.
113  */
114 uint8_t increase_nav_heading(int32_t *heading, float incrementDegrees)
115 {
116  struct Int32Eulers *eulerAngles = stateGetNedToBodyEulers_i();
117  int32_t newHeading = eulerAngles->psi + ANGLE_BFP_OF_REAL(RadOfDeg(incrementDegrees));
118  // Check if your turn made it go out of bounds...
119  INT32_ANGLE_NORMALIZE(newHeading); // HEADING HAS INT32_ANGLE_FRAC....
120  *heading = newHeading;
121  VERBOSE_PRINT("Increasing heading to %f\n", DegOfRad(ANGLE_FLOAT_OF_BFP(*heading)));
122  return false;
123 }
124 
125 /*
126  * Calculates coordinates of a distance of 'distanceMeters' forward w.r.t. current position and heading
127  */
128 static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
129 {
130  struct EnuCoor_i *pos = stateGetPositionEnu_i(); // Get your current position
131  struct Int32Eulers *eulerAngles = stateGetNedToBodyEulers_i();
132  // Calculate the sine and cosine of the heading the drone is keeping
133  float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(eulerAngles->psi));
134  float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(eulerAngles->psi));
135  // Now determine where to place the waypoint you want to go to
136  new_coor->x = pos->x + POS_BFP_OF_REAL(sin_heading * (distanceMeters));
137  new_coor->y = pos->y + POS_BFP_OF_REAL(cos_heading * (distanceMeters));
138  VERBOSE_PRINT("Calculated %f m forward position. x: %f y: %f based on pos(%f, %f) and heading(%f)\n", distanceMeters,
139  POS_FLOAT_OF_BFP(new_coor->x), POS_FLOAT_OF_BFP(new_coor->y), POS_FLOAT_OF_BFP(pos->x), POS_FLOAT_OF_BFP(pos->y),
140  DegOfRad(ANGLE_FLOAT_OF_BFP(eulerAngles->psi)) );
141  return false;
142 }
143 
144 /*
145  * Sets waypoint 'waypoint' to the coordinates of 'new_coor'
146  */
147 uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor)
148 {
149  VERBOSE_PRINT("Moving waypoint %d to x:%f y:%f\n", waypoint, POS_FLOAT_OF_BFP(new_coor->x),
150  POS_FLOAT_OF_BFP(new_coor->y));
151  waypoint_set_xy_i(waypoint, new_coor->x, new_coor->y);
152  return false;
153 }
154 
155 /*
156  * Calculates coordinates of distance forward and sets waypoint 'waypoint' to those coordinates
157  */
158 uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
159 {
160  struct EnuCoor_i new_coor;
161  calculateForwards(&new_coor, distanceMeters);
162  moveWaypoint(waypoint, &new_coor);
163  return false;
164 }
165 
166 /*
167  * Sets the variable 'incrementForAvoidance' randomly positive/negative
168  */
170 {
171  // Randomly choose CW or CCW avoiding direction
172  int r = rand() % 2;
173  if (r == 0) {
174  incrementForAvoidance = 10.0;
175  VERBOSE_PRINT("Set avoidance increment to: %f\n", incrementForAvoidance);
176  } else {
177  incrementForAvoidance = -10.0;
178  VERBOSE_PRINT("Set avoidance increment to: %f\n", incrementForAvoidance);
179  }
180  return false;
181 }
182 
int32_t psi
in rad with INT32_ANGLE_FRAC
unsigned short uint16_t
Definition: types.h:16
int tresholdColorCount
#define ORANGE_AVOIDER_CB_MIN
void orange_avoider_init()
uint8_t safeToGoForwards
#define ORANGE_AVOIDER_LUM_MAX
uint8_t increase_nav_heading(int32_t *heading, float incrementDegrees)
int32_t y
North.
int32_t x
East.
uint8_t color_cb_max
Definition: colorfilter.c:49
#define POS_BFP_OF_REAL(_af)
uint8_t color_lum_max
Definition: colorfilter.c:47
#define ORANGE_AVOIDER_CB_MAX
uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
uint8_t chooseRandomIncrementAvoidance()
#define ANGLE_FLOAT_OF_BFP(_ai)
static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
void waypoint_set_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Set only local XY coordinates of waypoint without update altitude.
Definition: waypoints.c:138
int color_count
Definition: colorfilter.c:54
static float heading
Definition: ahrs_infrared.c:45
uint8_t color_cr_max
Definition: colorfilter.c:51
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:104
#define VERBOSE_PRINT(...)
euler angles
#define INT32_ANGLE_NORMALIZE(_a)
#define ORANGE_AVOIDER_CR_MIN
uint8_t color_cb_min
Definition: colorfilter.c:48
float incrementForAvoidance
signed long int32_t
Definition: types.h:19
#define ANGLE_BFP_OF_REAL(_af)
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)
void orange_avoider_periodic()
#define ORANGE_AVOIDER_LUM_MIN
#define ORANGE_AVOIDER_CR_MAX
void waypoint_set_here_2d(uint8_t wp_id)
set waypoint to current horizontal location without modifying altitude
Definition: waypoints.c:225
uint8_t color_lum_min
Definition: colorfilter.c:46
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition: navigation.c:510
#define POS_FLOAT_OF_BFP(_ai)
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
uint8_t color_cr_min
Definition: colorfilter.c:50
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
float maxDistance
uint16_t trajectoryConfidence