Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
range_forcefield.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 K. N. McGuire
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 /*
22  * @file "modules/range_forcefield/range_forcefield.c"
23  * @author K. N. McGuire
24  * This module generates a forcefield based on range sensor measurements the use of single point range sensors.
25  */
26 
28 
29 #include "subsystems/abi.h"
30 
31 //abi for range sensors
32 #ifndef RANGE_FORCEFIELD_RECIEVE_ID
33 #define RANGE_FORCEFIELD_RECIEVE_ID ABI_BROADCAST
34 #endif
35 
36 #ifndef RANGE_FORCEFIELD_INNER_LIMIT
37 #define RANGE_FORCEFIELD_INNER_LIMIT 1.0f
38 #endif
39 PRINT_CONFIG_VAR(RANGE_FORCEFIELD_INNER_LIMIT)
40 
41 #ifndef RANGE_FORCEFIELD_OUTER_LIMIT
42 #define RANGE_FORCEFIELD_OUTER_LIMIT 1.4f
43 #endif
44 PRINT_CONFIG_VAR(RANGE_FORCEFIELD_OUTER_LIMIT)
45 
46 #ifndef RANGE_FORCEFIELD_MIN_VEL
47 #define RANGE_FORCEFIELD_MIN_VEL 0.0f
48 #endif
49 PRINT_CONFIG_VAR(RANGE_FORCEFIELD_MIN_VEL)
50 
51 #ifndef RANGE_FORCEFIELD_MAX_VEL
52 #define RANGE_FORCEFIELD_MAX_VEL 0.5f
53 #endif
54 PRINT_CONFIG_VAR(RANGE_FORCEFIELD_MAX_VEL)
55 
57 static struct FloatVect3 ff_nearest_obs_pos; // nearest obstacle on positive axis
58 static struct FloatVect3 ff_nearest_obs_neg; // nearest obstacle on negative axis
59 
60 static float compute_ff_vel(float range);
61 static void store_min_dist(float pos, float *nearest_pos, float range);
62 
64 static void obstacle_cb(uint8_t sender_id __attribute__((unused)), float range, float azimuth,
65  float bearing)
66 {
67  static struct FloatEulers body_to_sensors_eulers;
68 
69  body_to_sensors_eulers.phi = 0;
70  body_to_sensors_eulers.theta = azimuth;
71  body_to_sensors_eulers.psi = bearing;
72 
73  range_forcefield_update(range, &body_to_sensors_eulers);
74 }
75 
77 {
82 
86 
90 
91  AbiBindMsgOBSTACLE_DETECTION(RANGE_FORCEFIELD_RECIEVE_ID, &obstacle_ev, obstacle_cb);
92 }
93 
94 /* Compute range forcefield and send abi message
95  *
96  */
98 {
99  static struct FloatVect3 ff_vel_body;
103 
104  AbiSendMsgRANGE_FORCEFIELD(RANGE_FORCEFIELD_ID, ff_vel_body.x, ff_vel_body.y, ff_vel_body.z);
105 
106  // apply forgetting factor to forcefield, handles if obstacle no longer in sight / updated
110 
114 }
115 
116 /* range_sensor_update_forcefield: This stores the range sensor measurement in
117  *
118  * @param[in] range, input range measurement
119  * @param[in] body_to_sensors_eulers, euler angles of the orientation of the range sensor in question[rad]
120  * */
121 void range_forcefield_update(float range, struct FloatEulers *body_to_sensor_eulers)
122 {
123  // generate vector to obstacle location based on range and rotation
124  struct FloatVect3 obs_loc, range_vec = {range, 0, 0};
125  struct FloatRMat range_sensor_to_body;
126  float_rmat_of_eulers(&range_sensor_to_body, body_to_sensor_eulers);
127  float_rmat_transp_vmult(&obs_loc, &range_sensor_to_body, &range_vec);
128 
129  // store closest distance in each axis
130  store_min_dist(obs_loc.x, &(ff_nearest_obs_pos.x), range);
131  store_min_dist(obs_loc.x, &(ff_nearest_obs_neg.x), range);
132 
133  store_min_dist(obs_loc.y, &(ff_nearest_obs_pos.y), range);
134  store_min_dist(obs_loc.y, &(ff_nearest_obs_neg.y), range);
135 
136  store_min_dist(obs_loc.z, &(ff_nearest_obs_pos.z), range);
137  store_min_dist(obs_loc.z, &(ff_nearest_obs_neg.z), range);
138 }
139 
140 /* Compute the forcefield velocity command given the distance to the obstacle and the forcefield parameters
141  * @param range Distance to obstacle
142  * @param ff_params The forcefield settings to generate the velocity
143  */
144 static float compute_ff_vel(float range)
145 {
146  float ff_vel = 0.f;
147  // Calculate avoidance velocity
148  if (range < 0.001f) {
149  //do nothing
150  } else if (range < range_forcefield_param.inner_limit) {
152  } else if (range < range_forcefield_param.outer_limit) {
153  // Linear
156  }
157  return ff_vel;
158 }
159 
160 /* Store the minimum directional distance
161  * pos directional position on axis
162  * nearest_pos pointer to store nearest position
163  * range magnitude of the range vector being stored
164  */
165 static void store_min_dist(float pos, float *nearest_pos, float range)
166 {
167  // check if obstacle in range for this axis
168  // 1/2 of total distance vector relates to an angle of 60deg in plane
169  if (fabsf(pos) >= range / 2.f)
170  {
171  // if obstacle should be considered in x axis, remember extremum in either direction
172  if (pos > 0.f && pos < *nearest_pos)
173  {
174  *nearest_pos = pos;
175  } else if (pos < 0.f && pos > *nearest_pos)
176  {
177  *nearest_pos = pos;
178  }
179  }
180 }
float_rmat_of_eulers
#define float_rmat_of_eulers
Definition: pprz_algebra_float.h:333
range_forcefield_param_t::inner_limit
float inner_limit
Definition: range_forcefield.h:33
abi.h
range_forcefield.h
range_forcefield_param_t::outer_limit
float outer_limit
Definition: range_forcefield.h:34
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
range_forcefield_param
struct range_forcefield_param_t range_forcefield_param
Definition: range_forcefield.c:56
range_forcefield_param_t
Definition: range_forcefield.h:32
RANGE_FORCEFIELD_ID
#define RANGE_FORCEFIELD_ID
Definition: abi_sender_ids.h:466
range_forcefield_periodic
void range_forcefield_periodic(void)
Definition: range_forcefield.c:97
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
ff_nearest_obs_neg
static struct FloatVect3 ff_nearest_obs_neg
Definition: range_forcefield.c:58
FloatVect3
Definition: pprz_algebra_float.h:54
float_rmat_transp_vmult
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_float.c:120
RANGE_FORCEFIELD_MIN_VEL
#define RANGE_FORCEFIELD_MIN_VEL
Definition: range_forcefield.c:47
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
range_forcefield_init
void range_forcefield_init(void)
Definition: range_forcefield.c:76
uint8_t
unsigned char uint8_t
Definition: types.h:14
obstacle_ev
static abi_event obstacle_ev
Definition: range_forcefield.c:63
RANGE_FORCEFIELD_OUTER_LIMIT
#define RANGE_FORCEFIELD_OUTER_LIMIT
Definition: range_forcefield.c:42
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
RANGE_FORCEFIELD_MAX_VEL
#define RANGE_FORCEFIELD_MAX_VEL
Definition: range_forcefield.c:52
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
obstacle_cb
static void obstacle_cb(uint8_t sender_id, float range, float azimuth, float bearing)
Definition: range_forcefield.c:64
range_forcefield_param_t::min_vel
float min_vel
Definition: range_forcefield.h:35
compute_ff_vel
static float compute_ff_vel(float range)
Definition: range_forcefield.c:144
range_forcefield_update
void range_forcefield_update(float range, struct FloatEulers *body_to_sensor_eulers)
Definition: range_forcefield.c:121
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
RANGE_FORCEFIELD_RECIEVE_ID
#define RANGE_FORCEFIELD_RECIEVE_ID
Definition: range_forcefield.c:33
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
ff_nearest_obs_pos
static struct FloatVect3 ff_nearest_obs_pos
Definition: range_forcefield.c:57
RANGE_FORCEFIELD_INNER_LIMIT
#define RANGE_FORCEFIELD_INNER_LIMIT
Definition: range_forcefield.c:37
range_forcefield_param_t::max_vel
float max_vel
Definition: range_forcefield.h:36
store_min_dist
static void store_min_dist(float pos, float *nearest_pos, float range)
Definition: range_forcefield.c:165