Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
orange_avoider.c File Reference
#include "modules/orange_avoider/orange_avoider.h"
#include "firmwares/rotorcraft/navigation.h"
#include "generated/airframe.h"
#include "state.h"
#include "modules/core/abi.h"
#include <time.h>
#include <stdio.h>
#include "generated/flight_plan.h"
+ Include dependency graph for orange_avoider.c:

Go to the source code of this file.

Macros

#define NAV_C
 
#define ORANGE_AVOIDER_VERBOSE   TRUE
 
#define PRINT(string, ...)   fprintf(stderr, "[orange_avoider->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
 
#define VERBOSE_PRINT(...)
 
#define ORANGE_AVOIDER_VISUAL_DETECTION_ID   ABI_BROADCAST
 

Enumerations

enum  navigation_state_t {
  SAFE , OBSTACLE_FOUND , OUT_OF_BOUNDS , HOLD ,
  SAFE , OBSTACLE_FOUND , SEARCH_FOR_SAFE_HEADING , OUT_OF_BOUNDS ,
  SAFE , OBSTACLE_FOUND , SEARCH_FOR_SAFE_HEADING , OUT_OF_BOUNDS ,
  REENTER_ARENA
}
 

Functions

static uint8_t moveWaypointForward (uint8_t waypoint, float distanceMeters)
 
static uint8_t calculateForwards (struct EnuCoor_i *new_coor, float distanceMeters)
 
static uint8_t moveWaypoint (uint8_t waypoint, struct EnuCoor_i *new_coor)
 
static uint8_t increase_nav_heading (float incrementDegrees)
 
static uint8_t chooseRandomIncrementAvoidance (void)
 
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)
 
void orange_avoider_init (void)
 
void orange_avoider_periodic (void)
 

Variables

float oa_color_count_frac = 0.18f
 
enum navigation_state_t navigation_state = SEARCH_FOR_SAFE_HEADING
 
int32_t color_count = 0
 
int16_t obstacle_free_confidence = 0
 
float heading_increment = 5.f
 
float maxDistance = 2.25
 
const int16_t max_trajectory_confidence = 5
 
static abi_event color_detection_ev
 

Detailed Description

Author
Roland Meertens Example on how to use the colours detected to avoid orange pole in the cyberzoo This module is an example module for the course AE4317 Autonomous Flight of Micro Air Vehicles at the TU Delft. This module is used in combination with a color filter (cv_detect_color_object) and the navigation mode of the autopilot. The avoidance strategy is to simply count the total number of orange pixels. When above a certain percentage threshold, (given by color_count_frac) we assume that there is an obstacle and we turn.

The color filter settings are set using the cv_detect_color_object. This module can run multiple filters simultaneously so you have to define which filter to use with the ORANGE_AVOIDER_VISUAL_DETECTION_ID setting.

Definition in file orange_avoider.c.

Macro Definition Documentation

◆ NAV_C

#define NAV_C

Definition at line 28 of file orange_avoider.c.

◆ ORANGE_AVOIDER_VERBOSE

#define ORANGE_AVOIDER_VERBOSE   TRUE

Definition at line 31 of file orange_avoider.c.

◆ ORANGE_AVOIDER_VISUAL_DETECTION_ID

#define ORANGE_AVOIDER_VISUAL_DETECTION_ID   ABI_BROADCAST

Definition at line 73 of file orange_avoider.c.

◆ PRINT

#define PRINT (   string,
  ... 
)    fprintf(stderr, "[orange_avoider->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)

Definition at line 33 of file orange_avoider.c.

◆ VERBOSE_PRINT

#define VERBOSE_PRINT (   ...)

Definition at line 37 of file orange_avoider.c.

Enumeration Type Documentation

◆ navigation_state_t

Enumerator
SAFE 
OBSTACLE_FOUND 
OUT_OF_BOUNDS 
HOLD 
SAFE 
OBSTACLE_FOUND 
SEARCH_FOR_SAFE_HEADING 
OUT_OF_BOUNDS 
SAFE 
OBSTACLE_FOUND 
SEARCH_FOR_SAFE_HEADING 
OUT_OF_BOUNDS 
REENTER_ARENA 

Definition at line 46 of file orange_avoider.c.

Function Documentation

◆ calculateForwards()

uint8_t calculateForwards ( struct EnuCoor_i new_coor,
float  distanceMeters 
)
static

Definition at line 208 of file orange_avoider.c.

References heading, POS_BFP_OF_REAL, POS_FLOAT_OF_BFP, FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetPositionEnu_f(), stateGetPositionEnu_i(), VERBOSE_PRINT, EnuCoor_i::x, and EnuCoor_i::y.

Referenced by moveWaypointForward().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ chooseRandomIncrementAvoidance()

uint8_t chooseRandomIncrementAvoidance ( void  )
static

Definition at line 235 of file orange_avoider.c.

References heading_increment, and VERBOSE_PRINT.

Referenced by orange_avoider_init(), and orange_avoider_periodic().

+ Here is the caller graph for this function:

◆ color_detection_cb()

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

Definition at line 76 of file orange_avoider.c.

References color_count.

Referenced by orange_avoider_init().

+ Here is the caller graph for this function:

◆ increase_nav_heading()

static uint8_t increase_nav_heading ( float  incrementDegrees)
static

Definition at line 180 of file orange_avoider.c.

References FLOAT_ANGLE_NORMALIZE, RotorcraftNavigation::heading, nav, new_heading, FloatEulers::psi, stateGetNedToBodyEulers_f(), and VERBOSE_PRINT.

Referenced by orange_avoider_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ moveWaypoint()

static uint8_t moveWaypoint ( uint8_t  waypoint,
struct EnuCoor_i new_coor 
)
static

Definition at line 224 of file orange_avoider.c.

References POS_FLOAT_OF_BFP, VERBOSE_PRINT, waypoint_move_xy_i(), EnuCoor_i::x, and EnuCoor_i::y.

Referenced by moveWaypointForward().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ moveWaypointForward()

static uint8_t moveWaypointForward ( uint8_t  waypoint,
float  distanceMeters 
)
static

Definition at line 197 of file orange_avoider.c.

References calculateForwards(), and moveWaypoint().

Referenced by orange_avoider_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ orange_avoider_init()

void orange_avoider_init ( void  )

Definition at line 87 of file orange_avoider.c.

References chooseRandomIncrementAvoidance(), color_detection_cb(), color_detection_ev, ORANGE_AVOIDER_VISUAL_DETECTION_ID, and time.

+ Here is the call graph for this function:

◆ orange_avoider_periodic()

Variable Documentation

◆ color_count

int32_t color_count = 0

Definition at line 58 of file orange_avoider.c.

Referenced by color_detection_cb(), and orange_avoider_periodic().

◆ color_detection_ev

abi_event color_detection_ev
static

Definition at line 75 of file orange_avoider.c.

Referenced by orange_avoider_init().

◆ heading_increment

float heading_increment = 5.f

Definition at line 60 of file orange_avoider.c.

Referenced by chooseRandomIncrementAvoidance(), and orange_avoider_periodic().

◆ max_trajectory_confidence

const int16_t max_trajectory_confidence = 5

Definition at line 63 of file orange_avoider.c.

Referenced by orange_avoider_periodic().

◆ maxDistance

float maxDistance = 2.25

Definition at line 61 of file orange_avoider.c.

Referenced by orange_avoider_periodic().

◆ navigation_state

◆ oa_color_count_frac

float oa_color_count_frac = 0.18f

Definition at line 54 of file orange_avoider.c.

Referenced by orange_avoider_periodic().

◆ obstacle_free_confidence

int16_t obstacle_free_confidence = 0

Definition at line 59 of file orange_avoider.c.

Referenced by orange_avoider_periodic().