Paparazzi UAS  v6.0_unstable-39-g758f8ef-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
mav_exercise.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Matteo Barbera <matteo.barbera97@gmail.com>
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with Paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
21 #include "mav_exercise.h"
22 #include "subsystems/abi.h"
24 #include "state.h"
25 #include "autopilot_static.h"
26 #include <stdio.h>
27 
28 #define NAV_C // needed to get the nav functions like Inside...
29 #include "generated/flight_plan.h"
30 
31 #define PRINT(string, ...) fprintf(stderr, "[mav_exercise->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
32 
33 uint8_t increase_nav_heading(float incrementDegrees);
34 uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters);
35 uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor);
36 
42 };
43 
44 // define and initialise global variables
45 float oa_color_count_frac = 0.18f;
47 int32_t color_count = 0; // orange color count from color filter for obstacle detection
48 int16_t obstacle_free_confidence = 0; // a measure of how certain we are that the way ahead is safe.
49 float moveDistance = 2; // waypoint displacement [m]
50 float oob_haeding_increment = 5.f; // heading angle increment if out of bounds [deg]
51 const int16_t max_trajectory_confidence = 5; // number of consecutive negative object detections to be sure we are obstacle free
52 
53 
54 // needed to receive output from a separate module running on a parallel process
55 #ifndef ORANGE_AVOIDER_VISUAL_DETECTION_ID
56 #define ORANGE_AVOIDER_VISUAL_DETECTION_ID ABI_BROADCAST
57 #endif
59 static void color_detection_cb(uint8_t __attribute__((unused)) sender_id,
60  int16_t __attribute__((unused)) pixel_x, int16_t __attribute__((unused)) pixel_y,
61  int16_t __attribute__((unused)) pixel_width,
62  int16_t __attribute__((unused)) pixel_height,
63  int32_t quality, int16_t __attribute__((unused)) extra) {
64  color_count = quality;
65 }
66 
67 void mav_exercise_init(void) {
68  // bind our colorfilter callbacks to receive the color filter outputs
69  AbiBindMsgVISUAL_DETECTION(ORANGE_AVOIDER_VISUAL_DETECTION_ID, &color_detection_ev, color_detection_cb);
70 }
71 
73  // only evaluate our state machine if we are flying
74  if (!autopilot_in_flight()) {
75  return;
76  }
77 
78  // compute current color thresholds
79  // front_camera defined in airframe xml, with the video_capture module
80  int32_t color_count_threshold = oa_color_count_frac * front_camera.output_size.w * front_camera.output_size.h;
81 
82  PRINT("Color_count: %d threshold: %d state: %d \n", color_count, color_count_threshold, navigation_state);
83 
84  // update our safe confidence using color threshold
85  if (color_count < color_count_threshold) {
87  } else {
88  obstacle_free_confidence -= 2; // be more cautious with positive obstacle detections
89  }
90 
91  // bound obstacle_free_confidence
93 
94  switch (navigation_state) {
95  case SAFE:
96  moveWaypointForward(WP_TRAJECTORY, 1.5f * moveDistance);
97  if (!InsideObstacleZone(WaypointX(WP_TRAJECTORY), WaypointY(WP_TRAJECTORY))) {
99  } else if (obstacle_free_confidence == 0) {
101  } else {
102  moveWaypointForward(WP_GOAL, moveDistance);
103  }
104  break;
105  case OBSTACLE_FOUND:
106  // TODO Change behavior
107  // stop as soon as obstacle is found
108  waypoint_move_here_2d(WP_GOAL);
109  waypoint_move_here_2d(WP_TRAJECTORY);
110 
112  break;
113  case OUT_OF_BOUNDS:
114  // stop
115  waypoint_move_here_2d(WP_GOAL);
116  waypoint_move_here_2d(WP_TRAJECTORY);
117 
119  moveWaypointForward(WP_TRAJECTORY, 1.5f);
120 
121  if (InsideObstacleZone(WaypointX(WP_TRAJECTORY), WaypointY(WP_TRAJECTORY))) {
122  // add offset to head back into arena
125  }
126  break;
127  case HOLD:
128  default:
129  break;
130  }
131 }
132 
133 /*
134  * Increases the NAV heading. Assumes heading is an INT32_ANGLE. It is bound in this function.
135  */
136 uint8_t increase_nav_heading(float incrementDegrees) {
137  float new_heading = stateGetNedToBodyEulers_f()->psi + RadOfDeg(incrementDegrees);
138 
139  // normalize heading to [-pi, pi]
140  FLOAT_ANGLE_NORMALIZE(new_heading);
141 
142  // set heading
143  nav_heading = ANGLE_BFP_OF_REAL(new_heading);
144 
145  return false;
146 }
147 
148 /*
149  * Calculates coordinates of a distance of 'distanceMeters' forward w.r.t. current position and heading
150  */
151 static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters) {
153 
154  // Now determine where to place the waypoint you want to go to
155  new_coor->x = stateGetPositionEnu_i()->x + POS_BFP_OF_REAL(sinf(heading) * (distanceMeters));
156  new_coor->y = stateGetPositionEnu_i()->y + POS_BFP_OF_REAL(cosf(heading) * (distanceMeters));
157  return false;
158 }
159 
160 /*
161  * Sets waypoint 'waypoint' to the coordinates of 'new_coor'
162  */
163 uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor) {
164  waypoint_move_xy_i(waypoint, new_coor->x, new_coor->y);
165  return false;
166 }
167 
168 /*
169  * Calculates coordinates of distance forward and sets waypoint 'waypoint' to those coordinates
170  */
171 uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters) {
172  struct EnuCoor_i new_coor;
173  calculateForwards(&new_coor, distanceMeters);
174  moveWaypoint(waypoint, &new_coor);
175  return false;
176 }
#define FLOAT_ANGLE_NORMALIZE(_a)
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
int16_t obstacle_free_confidence
Definition: mav_exercise.c:48
#define PRINT(string,...)
Definition: mav_exercise.c:31
int32_t y
North.
int32_t x
East.
float heading
Definition: wedgebug.c:258
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
#define POS_BFP_OF_REAL(_af)
const int16_t max_trajectory_confidence
Definition: mav_exercise.c:51
Main include for ABI (AirBorneInterface).
void waypoint_move_here_2d(uint8_t wp_id)
Definition: waypoints.c:246
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
float psi
in radians
static abi_event color_detection_ev
Definition: mav_exercise.c:58
uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor)
Definition: mav_exercise.c:163
void mav_exercise_init(void)
Definition: mav_exercise.c:67
navigation_state
Camera focal length, in pixels (i.e. distance between camera.
Definition: wedgebug.c:209
float oa_color_count_frac
Definition: mav_exercise.c:45
#define ORANGE_AVOIDER_VISUAL_DETECTION_ID
Definition: mav_exercise.c:56
uint8_t increase_nav_heading(float incrementDegrees)
Definition: mav_exercise.c:136
#define WaypointX(_wp)
Definition: common_nav.h:45
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:257
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:108
float moveDistance
Definition: mav_exercise.c:49
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)
Definition: mav_exercise.c:59
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
#define WaypointY(_wp)
Definition: common_nav.h:46
uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
Definition: mav_exercise.c:171
float oob_haeding_increment
Definition: mav_exercise.c:50
#define ANGLE_BFP_OF_REAL(_af)
Rotorcraft navigation functions.
vector in East North Up coordinates
API to get/set the generic vehicle states.
int32_t color_count
Definition: mav_exercise.c:47
navigation_state_t
Definition: mav_exercise.c:37
static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
Definition: mav_exercise.c:151
void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Definition: waypoints.c:150
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
#define front_camera
Definition: mt9f002_nps.c:3
void mav_exercise_periodic(void)
Definition: mav_exercise.c:72
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float new_heading
Definition: guidance_OA.c:114