#include "modules/orange_avoider/orange_avoider_guided.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "generated/airframe.h"
#include "state.h"
#include "modules/core/abi.h"
#include <stdio.h>
#include <time.h>

Include dependency graph for orange_avoider_guided.c:

Macros
#define	ORANGE_AVOIDER_VERBOSE TRUE

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

#define	VERBOSE_PRINT(...)

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
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)

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_init (void)

void	orange_avoider_guided_periodic (void)

Variables
float	oag_color_count_frac = 0.18f

float	oag_floor_count_frac = 0.05f

float	oag_max_speed = 0.5f

float	oag_heading_rate = RadOfDeg(20.f)

enum navigation_state_t	navigation_state = SEARCH_FOR_SAFE_HEADING

int32_t	color_count = 0

int32_t	floor_count = 0

int32_t	floor_centroid = 0

float	avoidance_heading_direction = 0

int16_t	obstacle_free_confidence = 0

const int16_t	max_trajectory_confidence = 5

static abi_event	color_detection_ev

static abi_event	floor_detection_ev

Detailed Description

Author: Kirk Scheper 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 guided 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. This module differs from the simpler orange_avoider.xml in that this is flown in guided mode. This flight mode is less dependent on a global positioning estimate as witht the navigation mode. This module can be used with a simple speed estimate rather than a global position.

Here we also need to use our onboard sensors to stay inside of the cyberzoo and not collide with the nets. For this we employ a simple color detector, similar to the orange poles but for green to detect the floor. When the total amount of green drops below a given threshold (given by floor_count_frac) we assume we are near the edge of the zoo and turn around. The color detection is done by the cv_detect_color_object module, use the FLOOR_VISUAL_DETECTION_ID setting to define which filter to use.

Definition in file orange_avoider_guided.c.

Macro Definition Documentation

◆ ORANGE_AVOIDER_VERBOSE

#define ORANGE_AVOIDER_VERBOSE TRUE

Definition at line 36 of file orange_avoider_guided.c.

◆ PRINT

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

Definition at line 38 of file orange_avoider_guided.c.

◆ VERBOSE_PRINT

#define VERBOSE_PRINT ( ... )

Definition at line 42 of file orange_avoider_guided.c.

Enumeration Type Documentation

◆ navigation_state_t

enum 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 47 of file orange_avoider_guided.c.

Function Documentation

◆ chooseRandomIncrementAvoidance()

uint8_t chooseRandomIncrementAvoidance ( void )

Definition at line 208 of file orange_avoider_guided.c.

References avoidance_heading_direction, oag_heading_rate, and VERBOSE_PRINT.

Referenced by orange_avoider_guided_init(), and orange_avoider_guided_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 77 of file orange_avoider_guided.c.

References color_count.

Referenced by orange_avoider_guided_init().

Here is the caller graph for this function:

◆ floor_detection_cb()

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
	)