Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
obstacle_avoidance.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Roland + Clint
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
27 #include <stdio.h>
28 #include <sys/fcntl.h>
29 #include <math.h>
30 #include <unistd.h>
31 #include <inttypes.h>
32 #include "state.h"
34 #include "math/pprz_geodetic_int.h"
39 
40 #ifndef AVOIDANCES_DISTANCES_HOR_COUNT
41 #define AVOIDANCES_DISTANCES_HOR_COUNT 36
42 #endif
43 
44 #ifndef AVOIDANCE_AMOUNT_OF_BOARDS
45 #define AVOIDANCE_AMOUNT_OF_BOARDS 6
46 #endif
47 
48 #ifndef AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES
49 #define AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES 6
50 #endif
51 
52 #ifndef AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES
53 #define AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES 6
54 #endif
55 //sensor info
58 float stereo_fow[2] = {1.0018, 0.7767};//based on FOW of 57.4, by 44.5
59 float angle_hor_board[] = {0, 1.0472, 2.0944, 3.1416, -2.0944, -1.0472};
60 int16_t focal = 118 * 6;
61 float baseline = 60;
62 float ref_yaw = 0.0;
63 //tuning info
64 float reference_pitch = 0.1;
65 float reference_roll = 0.1;
66 float dist_treshold = 1.25;
68 
70 //for messages
71 //general
72 uint8_t *READimageBuffer_old; //USED for butterworth filter
73 float heading_goal_f = 0;
76 //target
79 struct FloatVect2 init_target = {0, 0};
80 float dx_ref = 0;
81 float dy_ref = 0;
82 //Variables Kalman filter
83 float A_kal = 1;
84 float B_kal = 0;
85 float H_kal = 1;
86 float Q_kal = 0.05;//TODO: clean variables
87 float R_kal = 2;
88 float K_gain_send = 0;
93 //variables butterworth filter
94 struct FloatVect3 filter_repforce_old = {0.0, 0.0, 0.0};
95 struct FloatVect3 Repulsionforce_Kan_old = {0.0, 0.0, 0.0};
98 //Vector Method
99 float F1 = 0.1;
100 float F2 = 0.9;
101 float Cfreq = 1;
102 float Ko = 60;//1000;//10000
103 float Kg = 100;//100;//100
104 float Dist_offset = 0;//-0.125;
109 //Potential Method
110 float b_damp = 0;
111 float K_goal = 1;
112 float K_obst = 40;
113 float c1_oa = 0.4;
114 float c2_oa = 0.4;
115 float c3_oa = 5.0;
116 float c4_oa = 0.5;
117 float c5_oa = 0.9;
118 float kv = 0.05;
119 float epsilon = 0.0;
120 float vmin = 0.2;
122 //variables escape method
126 float waypoint_rot = 0;
129 float v_min = 0.3;
130 float v_max = 0.6;
131 //variables hysteris
132 float V_hys_low = 5;
133 float V_hys_high = 10;
135 //variables for logger
138 
139 //static void send_OA_DATA(void)
140 //{
141 // //DOWNLINK_SEND_OA_DATA(DefaultChannel, DefaultDevice, &target.x, &target.y, &pos_diff.x, &pos_diff.y, &ref_pitch,
144 //}
145 
146 void serial_init(void)
147 {
148 
149  for (int i_fill = 0;
151  i_fill++) {
152  Pest_new[i_fill] = 1;
153  Xest_new[i_fill] = 1;
154  butter_old[i_fill] = 0;
155  }
156 // register_periodic_telemetry(DefaultPeriodic, "OA_DATA", send_OA_DATA);
157 }
158 
159 void serial_start(void)
160 {
161  //printf("serial start\n");
162 }
163 
164 void setAnglesMeasurements(float *anglesMeasurements, float *centersensorRad, float *fieldOfViewRad,
165  uint16_t *size_matrix_local)
166 {
167 
168  for (int i1 = 0; i1 < size_matrix_local[0]; i1++) {
169  for (int i3 = 0; i3 < size_matrix_local[2]; i3++) {
170  anglesMeasurements[i1 * size_matrix_local[0] + i3] = centersensorRad[i1] - 0.5 * fieldOfViewRad[0] + fieldOfViewRad[0] /
171  size_matrix_local[2] / 2 + (fieldOfViewRad[0] / size_matrix_local[2]) * i3;
172  }
173  }
174 }
175 
176 void serial_update(void)
177 {
178  if (stereocam_data.fresh) {
179  printf("Stereo fresh: %i \n", stereocam_data.len);
180 
181  float distancesMeters[stereocam_data.len];
182  float anglesMeasurements[stereocam_data.matrix_width];
183 
184  //stereocam_disparity_to_meters(stereocam_data.data,distancesMeters,stereocam_data.len);
185  for (int i_print1 = 0; i_print1 < AVOIDANCE_AMOUNT_OF_BOARDS; i_print1++) {
186  for (int i_print2 = 0; i_print2 < (stereocam_data.len / AVOIDANCE_AMOUNT_OF_BOARDS); i_print2++) {
187  printf("%3d,", stereocam_data.data[i_print1 * (stereocam_data.len / AVOIDANCE_AMOUNT_OF_BOARDS) + i_print2]);
188  }
189  printf("\n");
190  }
191 
193  if (OA_method_flag == PINGPONG) {
194  //Calculate angles + distances
196  stereocam_disparity_to_meters(stereocam_data.data, distancesMeters, stereocam_data.len);
197 
198  matrix_2_pingpong(distancesMeters, size_matrix, distances_hor);
199  pingpong_euler(distances_hor, anglesMeasurements, stereocam_data.matrix_width, reference_pitch, reference_roll,
200  dist_treshold);
201  }
202 
203  if (OA_method_flag == POT_HEADING) {
206  }
207 
208  if (OA_method_flag == POT_VEL) {
211  }
212 
213  if (OA_method_flag == VECTOR) {
216  }
217  if (OA_method_flag == SAFETYZONE) {
220  }
221  if (OA_method_flag == LOGICBASED) {
224  }
225 
226  stereocam_data.fresh = 0;
227  if (stereocam_data.len > 50) {
228  DOWNLINK_SEND_MULTIGAZE_METERS(DefaultChannel, DefaultDevice, 50, distancesMeters);
229 
230  } else {
231  DOWNLINK_SEND_MULTIGAZE_METERS(DefaultChannel, DefaultDevice, stereocam_data.len, distancesMeters);
232 
233  }
234  }
235 
236 }
237 void matrix_2_pingpong(float *distancesMeters, uint16_t *size_matrix_local, float *distances_hor_local)
238 {
239  float distances_hor_local_old[AVOIDANCES_DISTANCES_HOR_COUNT];
240  float distances_hor_local_new[AVOIDANCES_DISTANCES_HOR_COUNT];
241 
242  for (int i_m = 0; i_m < size_matrix_local[0]; i_m++) {
243  for (int i_m3 = 0; i_m3 < size_matrix_local[2]; i_m3++) {
244  distances_hor_local[i_m * size_matrix_local[2] + i_m3] = 10000;
245  distances_hor_local_new[i_m * size_matrix_local[2] + i_m3] = 10000;
246  for (int i_m2 = 0; i_m2 < 4; i_m2++) {
247  if (distancesMeters[i_m * size_matrix_local[1] + i_m2 * size_matrix_local[0]*size_matrix_local[2] + i_m3] <
248  distances_hor_local[i_m * size_matrix_local[2] + i_m3]) {
249  distances_hor_local_old[i_m * size_matrix_local[2] + i_m3] = distances_hor_local_new[i_m * size_matrix_local[2] + i_m3];
250  distances_hor_local_new[i_m * size_matrix_local[2] + i_m3] = distancesMeters[i_m * size_matrix_local[1] + i_m2 *
251  size_matrix_local[0] * size_matrix_local[2] + i_m3];
252 
253  distances_hor_local[i_m * size_matrix_local[2] + i_m3] = (distances_hor_local_old[i_m * size_matrix_local[2] + i_m3] +
254  distances_hor_local_new[i_m * size_matrix_local[2] + i_m3]) / 2;
255  }
256  }
257  // printf("index: %i %i, %f",i_m,i_m3,distances_hor[i_m*size_matrix[2] + i_m3]);
258  }
259  }
260 
261 }
262 
264 {
265  //TODO implement kalman filter
266  //Parameters for Kalman filter
267  float Xpred_new[size_matrix[0]*size_matrix[1]*size_matrix[2]];
268  float Xest_old[size_matrix[0]*size_matrix[1]*size_matrix[2]];
269  float Ppred_new[size_matrix[0]*size_matrix[1]*size_matrix[2]];
270  float Pest_old[size_matrix[0]*size_matrix[1]*size_matrix[2]];
271  float K_gain[size_matrix[0]*size_matrix[1]*size_matrix[2]];
272 
273  //Kallman filter on disparity matrix
274  for (int i_k = 0; i_k < (size_matrix[0]*size_matrix[1]*size_matrix[2]); i_k++) {
275  Pest_old[i_k] = Pest_new[i_k];
276  Xest_old[i_k] = Xest_new[i_k];
277 
278  //one step ahead prediction
279  Xpred_new[i_k] = A_kal * Xest_old[i_k];
280 
281  //Covariance matrix of state prediction error
282  Ppred_new[i_k] = A_kal * Pest_old[i_k] * A_kal + Q_kal;
283 
284  //Kalman gain calculation
285  K_gain[i_k] = Ppred_new[i_k] * H_kal * 1.0 / (H_kal * Ppred_new[i_k] * H_kal + R_kal);
286 
287  //Measurement update
288  Xest_new[i_k] = Xpred_new[i_k] + K_gain[i_k] * (float)(stereocam_data.data[i_k] - H_kal * Xpred_new[i_k]);
289 
290  //Covariance matrix of state estimation error
291  Pest_new[i_k] = (1 - K_gain[i_k] * H_kal) * Ppred_new[i_k];
292  }
293 }
294 
296 {
297  float butter[size_matrix[0]*size_matrix[1]*size_matrix[2]];
298  float A_butter = -0.7265;
299  float B_butter[2] = {0.1367, 0.1367};
300 
301 
302  for (int i_k = 0; i_k < (size_matrix[0] * size_matrix[1] * size_matrix[2]); i_k++) {
303 
304  if ((READimageBuffer_old[i_k] - stereocam_data.data[i_k]) <= 1
305  && (READimageBuffer_old[i_k] - stereocam_data.data[i_k]) > 0) {
306  stereocam_data.data[i_k] = READimageBuffer_old[i_k];
307 
308  }
309 
310  butter[i_k] = B_butter[0] * (float)stereocam_data.data[i_k] + B_butter[1] * (float)READimageBuffer_old[i_k] - A_butter *
311  butter_old[i_k];
312  butter_old[i_k] = butter[i_k];
313  }
314 
315  for (int ifill = 0; ifill < (size_matrix[0] * size_matrix[1] * size_matrix[2]); ifill++) {
316  READimageBuffer_old[ifill] = stereocam_data.data[ifill];
317  }
318 
319 }
320 
322 {
324 
325  float psi = stateGetNedToBodyEulers_f()->psi;
326  float s_psi = sin(psi);
327  float c_psi = cos(psi);
328  float s_waypoint_rot = sin(waypoint_rot);
329  float c_waypoint_rot = cos(waypoint_rot);
330  float dx_ref_NED;
331  float dy_ref_NED;
332 
333  if (OA_method_flag == 2) {
334  dx_ref_NED = dx_ref;
335  dy_ref_NED = dy_ref;
336 
337  } else {
338  dx_ref_NED = c_psi * dx_ref - s_psi * dy_ref;
339  dy_ref_NED = s_psi * dx_ref + c_psi * dy_ref;
340  }
341 
342  target.x = init_target.x + dx_ref_NED;
343  target.y = init_target.y + dy_ref_NED;
344 
345  //apply rotation of waypoint
346  if (OA_method_flag == 6) {
347  target.x = c_waypoint_rot * target.x - s_waypoint_rot * target.y;
348  target.y = s_waypoint_rot * target.x + c_waypoint_rot * target.y;
349  }
350 
353  heading_goal_f = atan2(pos_diff.y, pos_diff.x);
354 
355 }
356 
357 void pingpong_euler(float *distances_hor_local, float *horizontalAnglesMeasurements, int horizontalAmountOfMeasurements,
358  float attitude_reference_pitch, float attitude_reference_roll, float dist_treshold_local)
359 {
360 
361  //init
362  float sumPitch = 0.0;
363  float sumRoll = 0.0;
364 
365  float oa_pitch_angle[horizontalAmountOfMeasurements];
366  float oa_roll_angle[horizontalAmountOfMeasurements];
367 
368  for (int horizontal_index = 0; horizontal_index < horizontalAmountOfMeasurements; horizontal_index++) {
369 
370  // printf("index: %i,distance %f",horizontal_index, distances_hor[horizontal_index]);
371  if (distances_hor_local[horizontal_index] < dist_treshold_local) {
372 
373  oa_pitch_angle[horizontal_index] = cos(horizontalAnglesMeasurements[horizontal_index]) * attitude_reference_pitch;
374  oa_roll_angle[horizontal_index] = -sin(horizontalAnglesMeasurements[horizontal_index]) * attitude_reference_roll;
375  sumPitch += oa_pitch_angle[horizontal_index];
376  sumRoll += oa_roll_angle[horizontal_index];
377 
378  }
379  }
380 
381  if (sumPitch > attitude_reference_pitch) {
382  ref_pitch = attitude_reference_pitch;
383  } else if (sumPitch < -attitude_reference_pitch) {
384  ref_pitch = -attitude_reference_pitch;
385  } else {
386  ref_pitch = sumPitch;
387  }
388 
389 
390  if (sumRoll > attitude_reference_roll) {
391  ref_roll = attitude_reference_roll;
392  } else if (sumRoll < -attitude_reference_roll) {
393  ref_roll = -attitude_reference_roll;
394  } else {
395  ref_roll = sumRoll;
396  }
397 
398 
399  printf("DegOfRad data %f %f\n", ref_pitch, ref_roll);
400 
401 
402 }
403 
405 {
406  //float OF_Result_Vy = 0;
407  //float OF_Result_Vx = 0
408 
409  //Initialize
410  float potential_obst = 0; //define potential field variables
411  float potential_obst_temp = 0;
412  float potential_obst_integrated = 0;
413  float Distance_est;
414  float angle_hor = - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2] /
415  2; //calculate position angle of stereo
416  float angle_hor_abs;
417  float obst_count = 0.0;
418  //float total_vel;
419  //float current_heading;
420  //float fp_angle;
421 
422  //Tune
423  //float dt = 0.5; //define delta t for integration! check response under 0.1 and 1
424  int8_t min_disparity = 40;
425 
426  //Current state of system;
427  float r_old = stateGetBodyRates_f()->r;
428  //current_heading = stateGetNedToBodyEulers_f()->psi; //check maybe cause of errors
429  //current_heading = 0;
430 
431  //check if side slip is zero
432  //total_vel = pow((OF_Result_Vy*OF_Result_Vy + OF_Result_Vx*OF_Result_Vx),0.5);
433 
434  //if (total_vel>vmin){
435  // fp_angle = atan2(OF_Result_Vx,OF_Result_Vy);
436  //}
437  //else{
438  // fp_angle = 0;
439  //}
440  //fp_angle = 0;
441 
444 
445 
446  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
447  angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
448  / 2; //Check if bodyframe is correct with current_heading correction
449  for (int i3 = 0; i3 < size_matrix[2]; i3++) {
450 
451  angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
452  FLOAT_ANGLE_NORMALIZE(angle_hor);
453  potential_obst_temp = 0.0;
454  obst_count = 0;
455 
456  for (int i2 = 0; i2 < 4; i2++) {
457 
458  if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
459  Distance_est = ((baseline * (float)focal / (float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
460  size_matrix[2] + i3] - 18.0)) / 1000;
461 
462  if (angle_hor < 0) {
463  angle_hor_abs = -1 * angle_hor;
464  } else {
465  angle_hor_abs = angle_hor;
466  }
467  potential_obst_temp = potential_obst_temp - K_obst * (angle_hor) * exp(-c3_oa * angle_hor_abs) * exp(
468  -c4_oa * Distance_est);//(tan(obst_width[i]+c5)-tan(c5));
469  potential_obst_integrated = potential_obst_integrated + K_obst * c3_oa * (fabs(angle_hor) + 1) / (c3_oa * c3_oa) * exp(
470  -c3_oa * fabs(angle_hor)) * exp(-c4_oa * Distance_est); // (tan(obst_width[i]+c5)-tan(c5));
471  //printf("angle_hor: %f, angle_hor_abs: %f, c3: %f, potential_obst: %f", angle_hor, angle_hor_abs,c3_oa,potential_obst);
472  obst_count++;
473 
474  } else {
475  Distance_est = 2000;
476  }
477 
478  //potential_obst = potential_obst + K_obst*(angle_hor);//*exp(-c3*abs(angle_hor))*exp(-c4*Distance_est);//(tan(obst_width[i]+c5)-tan(c5));
479  //potential_obst_write = potential_obst;
480  //potential_obst_integrated = potential_obst_integrated + K_obst*c3*(abs(angle_hor)+1)/(c3*c3)*exp(-c3*abs(angle_hor))*exp(-c4*Distance_est);// (tan(obst_width[i]+c5)-tan(c5));
481 
482  }
483  if (obst_count != 0) {
484  potential_obst = potential_obst + potential_obst_temp / ((float)obst_count);
485  }
486  }
487  }
488 
489  //printf("current_heading, %f heading_goal_f: %f",current_heading, heading_goal_ref);
490  //calculate angular accelaration from potential field
491 
492  r_dot_new = -b_damp * r_old - K_goal * (heading_goal_ref) * (exp(-c1_oa * sqrt(VECT2_NORM2(
493  pos_diff))) + c2_oa) + potential_obst;
494 
495  // Calculate velocity from potential
496  speed_pot = vref_max * exp(-kv * potential_obst_integrated) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff))) - epsilon;
497  if (speed_pot <= 0) {
498  speed_pot = 0;
499  }
500 
501  //calculate new_heading(ref_pitch)
502  //ref_pitch = (current_state(3)+sideslip) + alpha_fil*r_dot_new;)
503 
504 }
505 
507 {
508  float OF_Result_Vy = 0;
509  float OF_Result_Vx = 0;
510 
511  //Constants
512  float new_heading;
513  float alpha_fil = 0.2;
514 
515  //Initialize
516  float potential_obst = 0; //define potential field variables
517  float potential_obst_integrated = 0;
518  float Distance_est;
519  float fp_angle;
520  float diff_angle;
521  float total_vel;
522  float current_heading;
523  float angle_hor = - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2] /
524  2; //calculate position angle of stereo
525 
526  //Tune
527  //float dt = 0.5; //define delta t for integration! check response under 0.1 and 1
528  int8_t min_disparity = 40;
529 
530  //Current state of system;
531  float r_old = stateGetBodyRates_f()->r;
532  current_heading = stateGetNedToBodyEulers_f()->psi; //check maybe cause of errors
533  //current_heading = 0;
534 
535  total_vel = pow((OF_Result_Vy * OF_Result_Vy + OF_Result_Vx * OF_Result_Vx), 0.5);
536 
537  if (total_vel > vmin) {
538  fp_angle = atan2(OF_Result_Vx, OF_Result_Vy);
539  } else {
540  fp_angle = 0;
541  }
542 
543  heading_goal_ref = (fp_angle + current_heading) - heading_goal_f;
545 
546 
547  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
548  angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
549  / 2; //Check if bodyframe is correct with current_heading correction
550  for (int i3 = 0; i3 < size_matrix[2]; i3++) {
551  angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
552  FLOAT_ANGLE_NORMALIZE(angle_hor);
553 
554  for (int i2 = 0; i2 < size_matrix[1]; i2++) {
555 
556  if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
557  Distance_est = ((baseline * (float)focal / (float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
558  size_matrix[2] + i3] - 18.0)) / 1000;
559  } else {
560  Distance_est = 2000;
561  }
562  diff_angle = fp_angle - angle_hor;
563  FLOAT_ANGLE_NORMALIZE(diff_angle);
564  potential_obst = potential_obst + K_obst * (diff_angle) * exp(-c3_oa * fabs(diff_angle)) * exp(
565  -c4_oa * Distance_est); //(tan(obst_width[i]+c5)-tan(c5));
566  potential_obst_integrated = potential_obst_integrated + K_obst * c3_oa * (fabs(diff_angle) + 1) / (c3_oa * c3_oa) * exp(
567  -c3_oa * fabs(diff_angle)) * exp(-c4_oa * Distance_est); // (tan(obst_width[i]+c5)-tan(c5));
568 
569  }
570  }
571  }
572 
573  //calculate angular accelaration from potential field
574  r_dot_new = -b_damp * r_old - K_goal * (heading_goal_ref) * (exp(-c1_oa * sqrt(VECT2_NORM2(
575  pos_diff))) + c2_oa) + potential_obst;
576 
577  // Calculate velocity from potential
578  speed_pot = vref_max * exp(-kv * potential_obst_integrated) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff))) - epsilon;
579  if (speed_pot <= 0) {
580  speed_pot = 0;
581  }
582 
583  //calculate new_ref_speeds
584  new_heading = fp_angle + alpha_fil * r_dot_new;
585  FLOAT_ANGLE_NORMALIZE(new_heading);
586 
588 }
589 
591 {
592 
593  //Constants
594  //Parameters for Butterworth filter
595  float A_butter = -0.8541;//-0.7265;
596  float B_butter[2] = {0.0730 , 0.0730 };//{0.1367, 0.1367};
597 
598  //Initalize
599  int8_t disp_count = 0;
600  float escape_angle = 0;
601  float y_goal_frame;
602  //float total_vel;
603  float Distance_est;
604  float Ca;
605  float Cv;
606  float angle_ver = 0;
607  float angle_hor = 0;
608  //struct FloatVect3 Repulsionforce_Kan = {0,0,0};
609  Repulsionforce_Kan.x = 0;
610  Repulsionforce_Kan.y = 0;
611  Repulsionforce_Kan.z = 0;
612  //struct FloatVect3 Attractforce_goal = {0,0,0};
613  //Attractforce_goal.x = 0;
614  //Attractforce_goal.y = 0;
615  //Attractforce_goal.z = 0;
616  //struct FloatRMat T;
617  //struct FloatEulers current_attitude = {0,0,0};
618  struct FloatVect3 Total_Kan = {0, 0, 0};
619  //Tuning variables
620  //float force_max = 200;
621  int8_t min_disparity = 45;
622 
623  //Flight plath angle calculation
624  // TODO make algorithm dependent on angle of obstacle.....
625  // total_vel = pow((OF_Result_Vy*OF_Result_Vy + OF_Result_Vx*OF_Result_Vx),0.5);
626 
627  // if (total_vel>vmin){
628  // fp_angle = atan2(OF_Result_Vx,OF_Result_Vy);
629  // }
630  // else{
631  // fp_angle = 0;
632  // }
633 
635  //FLOAT_ANGLE_NORMALIZE(heading_goal_ref);
636 
637  //Calculate Attractforce_goal size = 1;
638  Attractforce_goal.x = cos(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
639  Attractforce_goal.y = sin(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
640  Attractforce_goal.z = 0;
641 
642 
643  //printf("current_heading, %f heading_goal_f: %f, heading_goal_ref: %f",stateGetNedToBodyEulers_f()->psi, heading_goal_f, heading_goal_ref);
644  //Transform to body frame
645  //current_attitude.psi = stateGetNedToBodyEulers_f()->psi;Attractforce_already in body frame, check when using data form Roland
646  //float_rmat_of_eulers_312(&T, &current_attitude);
647  //MAT33_VECT3_MUL(Attractforce_goal, T, Attractforce_goal);
648 
649  //Attractforce_goal_send.x = Attractforce_goal.x;
650  //Attractforce_goal_send.y = Attractforce_goal.y;
651  //Attractforce_goal_send.z = Attractforce_goal.z;
652 
653 
654  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
655  angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
656  / 2; //Check if bodyframe is correct with current_heading correction
657 
658  for (int i3 = 0; i3 < size_matrix[2]; i3++) {
659  angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
660  angle_ver = 0.5 * stereo_fow[1] + stereo_fow[1] / size_matrix[1] - stereo_fow[1] / size_matrix[1] / 2;
661 
662  for (int i2 = 0; i2 < 4; i2++) {
663  angle_ver = angle_ver - stereo_fow[1] / size_matrix[1];
664 
665  Ca = cos((angle_hor - heading_goal_ref) * Cfreq) * erf(1 * sqrt(VECT2_NORM2(pos_diff)));
666  if (Ca < 0) {
667  Ca = 0;
668  }
669 
670  //TODO make dependent on speed: total_vel/vref_max
671  Cv = F1 + F2 * Ca;
672  if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
673  Distance_est = (baseline * (float)focal / ((float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
674  size_matrix[2] + i3]) - 18.0) / 1000;
675  disp_count++;
676  Repulsionforce_Kan.x = Repulsionforce_Kan.x - pow(Cv / (Distance_est + Dist_offset),
677  2) * cos(angle_hor) * cos(angle_ver);
678  Repulsionforce_Kan.y = Repulsionforce_Kan.y - pow(Cv / (Distance_est + Dist_offset),
679  2) * sin(angle_hor) * cos(angle_ver);
680  Repulsionforce_Kan.z = Repulsionforce_Kan.z - pow(Cv / (Distance_est + Dist_offset), 2) * sin(angle_ver);
681 
682  printf("rep.x %f index %d %d %d disp: %d cv: %f angle_hor: %f angle_ver: %f \n", Repulsionforce_Kan.x, i1, i2, i3,
683  stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3], Cv, angle_hor, angle_ver);
684 
685  }
686 // else {
687 // Distance_est = 2000;
688 // }
689 //
690 // if(pow(Cv/(READimageBuffer_offset[i1*size_matrix[1]+i2*size_matrix[0]*size_matrix[2] + i3]-0.2),2)<force_max){
691 // Repulsionforce_Kan.x = Repulsionforce_Kan.x - pow(Cv/(Distance_est+Dist_offset),2)*cos(angle_hor)*cos(angle_ver);
692 // Repulsionforce_Kan.y = Repulsionforce_Kan.y - pow(Cv/(Distance_est+Dist_offset),2)*sin(angle_hor)*cos(angle_ver);
693 // Repulsionforce_Kan.z = Repulsionforce_Kan.z - pow(Cv/(Distance_est+Dist_offset),2)*sin(angle_ver);
694 // }
695 // else{
696 // Repulsionforce_Kan.x = Repulsionforce_Kan.x - force_max*sin(angle_hor)*cos(angle_ver);
697 // Repulsionforce_Kan.y = Repulsionforce_Kan.y - force_max*cos(angle_hor)*cos(angle_ver);
698 // Repulsionforce_Kan.z = Repulsionforce_Kan.z - force_max*sin(angle_ver);
699 // }
700 
701  }
702  }
703  }
704 
705  //Normalize for ammount entries in Matrix
706  //Repulsionforce_Kan = Repulsionforce_Kan/(float)(size_matrix[1]*size_matrix[2]);
707  VECT3_SMUL(Repulsionforce_Kan, Repulsionforce_Kan, 1.0 / (float)disp_count);
708  printf("After multiplication: %f", Repulsionforce_Kan.x);
709 
710  if (repulsionforce_filter_flag == 1) {
711 
712  Repulsionforce_Used.x = B_butter[0] * Repulsionforce_Kan.x + B_butter[1] * Repulsionforce_Kan_old.x - A_butter *
713  filter_repforce_old.x;
714  Repulsionforce_Used.y = B_butter[0] * Repulsionforce_Kan.y + B_butter[1] * Repulsionforce_Kan_old.y - A_butter *
715  filter_repforce_old.y;
716  Repulsionforce_Used.z = B_butter[0] * Repulsionforce_Kan.z + B_butter[1] * Repulsionforce_Kan_old.z - A_butter *
717  filter_repforce_old.z;
718 
719  VECT3_COPY(Repulsionforce_Kan_old, Repulsionforce_Kan);
720  VECT3_COPY(filter_repforce_old, Repulsionforce_Used);
721 
722  }
723 
724  else {
726  }
727 
730 
731  //Total force
732  VECT3_ADD(Total_Kan, Repulsionforce_Used);
733  VECT3_ADD(Total_Kan, Attractforce_goal);
734 
735  //set variable for stabilization_opticflow
736  //printf("RepulsionforceX: %f AttractX: %f \n",Repulsionforce_Used.x,Attractforce_goal.x);
737  //printf("RepulsionforceY: %f AttractY: %f \n",Repulsionforce_Used.y,Attractforce_goal.y);
738 
739 
740  //hysteris
741  if (sqrt(VECT2_NORM2(Total_Kan)) < V_hys_low && sqrt(VECT2_NORM2(pos_diff)) > 1) {
742  hysteris_flag = 1;
743 
744  //x_goal_frame= cos() * Total_Kan.x + sin() * Total_Kan.y;
745  y_goal_frame = -sin(heading_goal_ref) * Total_Kan.x + cos(heading_goal_ref) * Total_Kan.y;
746 
747  if (y_goal_frame >= 0) {
748  escape_angle = 0.5 * M_PI;
749  } else if (y_goal_frame < 0) {
750  escape_angle = -0.5 * M_PI;
751  }
752  }
753 
754 
755  if (sqrt(VECT2_NORM2(Total_Kan)) > V_hys_high || sqrt(VECT2_NORM2(pos_diff)) < 1) {
756  hysteris_flag = 0;
757  }
758 
759  if (hysteris_flag == 1) {
760 
761  Total_Kan.x = cos(heading_goal_ref + escape_angle) * V_hys_high;
762  Total_Kan.y = -sin(heading_goal_ref + escape_angle) * V_hys_high;
763 
764  }
765 
766 
767  ref_pitch = Total_Kan.x;
768  ref_roll = Total_Kan.y;
769  printf("ref_pitch: %f ref_roll: %f disp_count: %d", ref_pitch, ref_roll, disp_count);
770  //set write values for logger
771  //Attractforce_goal_send.x = Attractforce_goal.x;
772  //Attractforce_goal_send.y = Attractforce_goal.y;
773  //Repulsionforce_Kan_send.x = Repulsionforce_Used.x;
774  //Repulsionforce_Kan_send.y = Repulsionforce_Used.y;
775 }
776 
778 {
779 
781  //Constants
782  //Parameters for Butterworth filter
783  float A_butter = -0.8541;//-0.7265;
784  float B_butter[2] = {0.0730 , 0.0730 };//{0.1367, 0.1367};
785 
786  int8_t disp_count = 0;
787  //Initalize
788  //float fp_angle;
789  //float total_vel;
790  float Ca;
791  float Cv;
792  float angle_ver = 0;
793  float angle_hor = 0;
794  Repulsionforce_Kan.x = 0;
795  Repulsionforce_Kan.y = 0;
796  Repulsionforce_Kan.z = 0;
797  //struct FloatRMat T;
798  //struct FloatEulers current_attitude = {0,0,0};
799  struct FloatVect3 Total_Kan = {0, 0, 0};
800  //Tuning variables
801  //float force_max = 200;
802 
804  //Constants
805  int8_t min_disparity = 45;
806  int8_t number_of_buffer = 20;
807  float bias = 2 * M_PI;
808  vref_max = 0.1; //CHECK!
809  //init
810  float available_heading[size_matrix[0] * size_matrix[2]];
811  float diff_available_heading[size_matrix[0] * size_matrix[2]];
812  float new_heading_diff = 1000;
813  float new_heading = 1000;
814  int8_t i = 0;
815  int8_t i_buffer = 0;
816  float Distance_est;
817  float V_total = 0;
819 
821  //FLOAT_ANGLE_NORMALIZE(heading_goal_ref);
822 
823  //Calculate Attractforce_goal
824  Attractforce_goal.x = cos(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
825  Attractforce_goal.y = sin(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
826  Attractforce_goal.z = 0;
827 
828  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
829  angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
830  / 2; //Check if bodyframe is correct with current_heading correction
831 
832  for (int i3 = 0; i3 < size_matrix[2]; i3++) {
833  angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
834  angle_ver = 0.5 * stereo_fow[1] + stereo_fow[1] / size_matrix[1] - stereo_fow[1] / size_matrix[1] / 2;
835 
836  for (int i2 = 0; i2 < 4; i2++) {
837  angle_ver = angle_ver - stereo_fow[1] / size_matrix[1];
838 
839  Ca = cos((angle_hor - heading_goal_ref) * Cfreq) * erf(1 * sqrt(VECT2_NORM2(pos_diff)));
840  if (Ca < 0) {
841  Ca = 0;
842  }
843 
844  //TODO make dependent on speed: total_vel/vref_max
845  Cv = F1 + F2 * Ca;
846 
847  if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
848  Distance_est = (baseline * (float)focal / ((float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
849  size_matrix[2] + i3]) - 18.0) / 1000;
850  disp_count++;
851  Repulsionforce_Kan.x = Repulsionforce_Kan.x - pow(Cv / (Distance_est + Dist_offset),
852  2) * cos(angle_hor) * cos(angle_ver);
853  Repulsionforce_Kan.y = Repulsionforce_Kan.y - pow(Cv / (Distance_est + Dist_offset),
854  2) * sin(angle_hor) * cos(angle_ver);
855  Repulsionforce_Kan.z = Repulsionforce_Kan.z - pow(Cv / (Distance_est + Dist_offset), 2) * sin(angle_ver);
856 
857 
858  }
859 
860  }
861  }
862  }
863 
864  //Normalize for ammount entries in Matrix
865  VECT3_SMUL(Repulsionforce_Kan, Repulsionforce_Kan, 1.0 / (float)disp_count);
866 
867  if (repulsionforce_filter_flag == 1) {
868 
869  Repulsionforce_Used.x = B_butter[0] * Repulsionforce_Kan.x + B_butter[1] * Repulsionforce_Kan_old.x - A_butter *
870  filter_repforce_old.x;
871  Repulsionforce_Used.y = B_butter[0] * Repulsionforce_Kan.y + B_butter[1] * Repulsionforce_Kan_old.y - A_butter *
872  filter_repforce_old.y;
873  Repulsionforce_Used.z = B_butter[0] * Repulsionforce_Kan.z + B_butter[1] * Repulsionforce_Kan_old.z - A_butter *
874  filter_repforce_old.z;
875 
876  VECT3_COPY(Repulsionforce_Kan_old, Repulsionforce_Kan);
877  VECT3_COPY(filter_repforce_old, Repulsionforce_Used);
878 
879  }
880 
881  else {
883  }
884 
887 
888  //Total force
889  VECT3_ADD(Total_Kan, Repulsionforce_Used);
890  VECT3_ADD(Total_Kan, Attractforce_goal);
891 
892  //set variable for stabilization_opticflow
893  printf("RepulsionforceX: %f AttractX: %f \n", Repulsionforce_Used.x, Attractforce_goal.x);
894  printf("RepulsionforceY: %f AttractY: %f \n", Repulsionforce_Used.y, Attractforce_goal.y);
895 
896  //set write values for logger
897  //Attractforce_goal_send.x = Attractforce_goal.x;
898  //Attractforce_goal_send.y = Attractforce_goal.y;
899  //Repulsionforce_Kan_send.x = Repulsionforce_Used.x;
900  //Repulsionforce_Kan_send.y = Repulsionforce_Used.y;
901 
902  V_total = sqrt(pow(Total_Kan.x, 2) + pow(Total_Kan.y, 2));
903 
904  if (V_total < v_min && sqrt(VECT2_NORM2(pos_diff)) > 1) {
905  escape_flag = 1;
906  }
907 
908  else if (V_total > v_max && obstacle_flag == 0) {
909  escape_flag = 0;
910  }
911 
912  printf("V_total: %f", V_total);
913  printf("escape_flag: %i, obstacle_flag: %i, set_bias: %i", escape_flag, obstacle_flag, set_bias);
914  printf("escape_flag: %i, obstacle_flag: %i, set_bias: %i", escape_flag, obstacle_flag, set_bias);
915 
916  if (escape_flag == 0) {
917  ref_pitch = Total_Kan.x;
918  ref_roll = Total_Kan.y;
919  } else if (escape_flag == 1) {
921 
922  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
923  angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2];
924  for (int i3 = 0; i3 < size_matrix[2]; i3++) {
925  angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
926  available_heading[i] = angle_hor;
927  diff_available_heading[i] = heading_goal_ref - available_heading[i];
928  FLOAT_ANGLE_NORMALIZE(diff_available_heading[i]);
929  i++;
930 
931  }
932  }
933 
934  i = 0;
935  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
936  for (int i3 = 0; i3 < size_matrix[2]; i3++) {
937  for (int i2 = 0; i2 < 4; i2++) {
938  if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
939  //distance_est = (baseline*(float)focal/((float)READimageBuffer_offset[i1*size_matrix[1]+i2*size_matrix[0]*size_matrix[2] + i3])-18.0)/1000;
940 
941  diff_available_heading[i] = 100;
942  for (int i_diff = -number_of_buffer; i_diff <= number_of_buffer + 1; i_diff++) {
943  i_buffer = i + i_diff;
944 
945 
946  if (i_buffer < 1) {
947  i_buffer = i_buffer + size_matrix[0] * size_matrix[2];
948  }
949 
950  if (i_buffer > size_matrix[0]*size_matrix[2]) {
951  i_buffer = i_buffer - size_matrix[0] * size_matrix[2];
952  }
953 
954  diff_available_heading[i_buffer] = 100;
955  }
956  }
957  }
958  i++;
959  }
960  }
961 
962  //set bias
963  if (set_bias == 1) {
964  for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
965  if (diff_available_heading[i100] <= 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
966  diff_available_heading[i100] = diff_available_heading[i100] - bias;
967  }
968  }
969  } else if (set_bias == 2) {
970  for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
971  if (diff_available_heading[i100] > 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
972  diff_available_heading[i100] = diff_available_heading[i100] + bias;
973  }
974  }
975  }
976 
977  for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
978  if (fabs(diff_available_heading[i100]) < fabs(new_heading_diff)) {
979  new_heading_diff = diff_available_heading[i100];
980  new_heading = available_heading[i100];
981  }
982  }
983 
984  if (obstacle_flag == 0 && fabs(new_heading_diff) > (2 * M_PI / 36.0)) {
985  obstacle_flag = 1;
986  if (new_heading_diff > 0) {
987  set_bias = 1;
988  direction = -1;
989  } else if (new_heading_diff <= 0) {
990  set_bias = 2;
991  direction = 1;
992  }
993  }
994 
995  if (fabs(new_heading_diff) <= (2 * M_PI / 36.0)) {
996  if (waypoint_rot == 0) {
997  obstacle_flag = 0;
998  set_bias = 0;
999  } else {
1000  waypoint_rot = waypoint_rot - direction * 0.05 * M_PI;
1001  }
1002  }
1003 
1004  if (fabs(new_heading_diff) >= 0.5 * M_PI) {
1005  waypoint_rot = waypoint_rot + direction * 0.25 * M_PI;
1006  }
1007 
1008  //vref_max should be low
1009  speed_pot = vref_max * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
1010  if (speed_pot <= 0) {
1011  speed_pot = 0;
1012  }
1013 
1015 
1016 #if PRINT_STUFF
1017  for (int i100 = 0; i100 < (size_matrix[0] * size_matrix[2]); i100++) {
1018  printf("%i diff_available_heading: %f available_heading: %f \n", i100, diff_available_heading[i100],
1019  available_heading[i100]);
1020  }
1021  printf("new_heading: %f current_heading: %f speed_pot: %f\n", new_heading, stateGetNedToBodyEulers_f()->psi,
1022  speed_pot);
1023  printf("set_bias: %i obstacle_flag: %i", set_bias, obstacle_flag);
1024 #endif
1025 
1026  ref_pitch = cos(new_heading) * speed_pot;
1027  ref_roll = sin(new_heading) * speed_pot;
1028 
1029  }
1030 }
1031 
1033 {
1034 
1035  //Constants
1036  int8_t min_disparity = 45;
1037  int8_t number_of_buffer = 20;
1038  float bias = 2 * M_PI;
1039  vref_max = 0.1; //CHECK!
1040 
1041  //init variables
1042  float angle_hor = 0;
1043  float available_heading[size_matrix[0]*size_matrix[2]];
1044  float diff_available_heading[size_matrix[0]*size_matrix[2]];
1045  float new_heading_diff = 1000;
1046  float new_heading = 1000;
1047  int8_t i = 0;
1048  int8_t i_buffer = 0;
1049  float distance_est;
1050  float distance_heading = 0;
1051 
1052  //generate available_headings
1054 
1055  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
1056  angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2];
1057  for (int i3 = 0; i3 < size_matrix[2]; i3++) {
1058  angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
1059  available_heading[i] = angle_hor;
1060  diff_available_heading[i] = heading_goal_ref - available_heading[i];
1061  FLOAT_ANGLE_NORMALIZE(diff_available_heading[i]);
1062  i++;
1063 
1064  }
1065  }
1066 
1067  //set unavailable headings to 100;
1068  i = 0;
1069  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
1070  for (int i3 = 0; i3 < size_matrix[2]; i3++) {
1071  for (int i2 = 0; i2 < 4; i2++) {
1072  if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
1073  distance_est = (baseline * (float)focal / ((float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
1074  size_matrix[2] + i3]) - 18.0) / 1000;
1075 
1076  if (distance_est < distance_heading) {
1077  distance_heading = distance_est;
1078  }
1079 
1080  diff_available_heading[i] = 100;
1081  for (int i_diff = -number_of_buffer; i_diff <= number_of_buffer + 1; i_diff++) {
1082  i_buffer = i + i_diff;
1083 
1084 
1085  if (i_buffer < 1) {
1086  i_buffer = i_buffer + size_matrix[0] * size_matrix[2];
1087  }
1088 
1089  if (i_buffer > size_matrix[0]*size_matrix[2]) {
1090  i_buffer = i_buffer - size_matrix[0] * size_matrix[2];
1091  }
1092 
1093  diff_available_heading[i_buffer] = 100;
1094  }
1095 
1096  }
1097  }
1098  i++;
1099  }
1100  }
1101 
1102  //set bias
1103  if (set_bias == 1) {
1104  for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
1105  if (diff_available_heading[i100] <= 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
1106  diff_available_heading[i100] = diff_available_heading[i100] - bias;
1107  }
1108  }
1109  } else if (set_bias == 2) {
1110  for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
1111  if (diff_available_heading[i100] > 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
1112  diff_available_heading[i100] = diff_available_heading[i100] + bias;
1113  }
1114  }
1115  }
1116 
1117  // select minimum available heading
1118  for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
1119  if (fabs(diff_available_heading[i100]) < fabs(new_heading_diff)) {
1120  new_heading_diff = diff_available_heading[i100];
1121  new_heading = available_heading[i100];
1122  }
1123  }
1124 
1125  if (obstacle_flag == 0 && distance_heading > 2.0) {
1126  new_heading = new_heading_old;
1127  } else if (obstacle_flag == 0 && fabs(new_heading_diff) > (2 * M_PI / 36.0)) {
1128  obstacle_flag = 1;
1129  if (new_heading_diff > 0) {
1130  set_bias = 1;
1131  direction = -1;
1132  } else if (new_heading_diff <= 0) {
1133  set_bias = 2;
1134  direction = 1;
1135  }
1136  }
1137 
1138  // Rotate waypoint
1139  if (fabs(new_heading_diff) >= 0.5 * M_PI) {
1140  waypoint_rot = waypoint_rot + direction * 0.25 * M_PI;
1141  }
1142 
1143  if (fabs(new_heading_diff) <= (2 * M_PI / 36.0)) {
1144  if (waypoint_rot == 0) {
1145  obstacle_flag = 0;
1146  set_bias = 0;
1147  } else {
1148  waypoint_rot = waypoint_rot - direction * 0.05 * M_PI;
1149  }
1150  }
1151 
1152  //vref_max should be low
1153  speed_pot = vref_max * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
1154  if (speed_pot <= 0) {
1155  speed_pot = 0;
1156  }
1157 
1158  ref_pitch = cos(new_heading) * speed_pot;
1159  ref_roll = sin(new_heading) * speed_pot;
1161 
1162 #if PRINT_STUFF
1163  for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
1164  printf("%i diff_available_heading: %f available_heading: %f \n", i100, diff_available_heading[i100],
1165  available_heading[i100]);
1166  }
1167  printf("new_heading: %f current_heading: %f speed_pot: %f\n", new_heading, stateGetNedToBodyEulers_f()->psi,
1168  speed_pot);
1169  printf("set_bias: %i obstacle_flag: %i", set_bias, obstacle_flag);
1170 #endif
1171 
1172 }
1173 
1174 
#define FLOAT_ANGLE_NORMALIZE(_a)
unsigned short uint16_t
Definition: types.h:16
float b_damp
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
float epsilon
float stereo_fow[2]
float F2
float y
in meters
float v_max
float Cfreq
float V_hys_high
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
float distances_hor[AVOIDANCES_DISTANCES_HOR_COUNT]
int8_t dis_treshold
void pingpong_euler(float *distances_hor_local, float *horizontalAnglesMeasurements, int horizontalAmountOfMeasurements, float attitude_reference_pitch, float attitude_reference_roll, float dist_treshold_local)
Periodic telemetry system header (includes downlink utility and generated code).
float c1_oa
void serial_update(void)
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
float r
in rad/s
float heading_goal_ref
float H_kal
float butter_old[AVOIDANCE_AMOUNT_OF_BOARDS *AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES *AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES]
struct FloatVect2 init_target
uint16_t size_matrix[]
void CN_potential_velocity(void)
float angle_hor_board[]
float Xest_new[AVOIDANCE_AMOUNT_OF_BOARDS *AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES *AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES]
void serial_init(void)
float psi
in radians
void CN_potential_heading(void)
uint8_t obstacle_flag
#define AVOIDANCES_DISTANCES_HOR_COUNT
uint8_t * READimageBuffer_old
float c5_oa
float K_gain_send
void CN_escape_velocity(void)
uint8_t escape_flag
float r_dot_new
Definition: guidance_OA.c:107
float heading_goal_f
void matrix_2_pingpong(float *distancesMeters, uint16_t *size_matrix_local, float *distances_hor_local)
struct FloatVect3 filter_repforce_old
struct FloatVect2 pos_diff
float B_kal
float dx_ref
Guidance for the obstacle avoidance methods.
float Dist_offset
Definition: point.c:84
vector in North East Down coordinates Units: meters
Paparazzi floating point algebra.
float reference_pitch
float baseline
float dist_treshold
float F1
float ref_pitch
Definition: guidance_OA.c:103
void CN_calculate_target(void)
int8_t repulsionforce_filter_flag
Definition: guidance_OA.c:94
struct NedCoor_f current_pos
interface to the TU Delft serial stereocam
uint8_t hysteris_flag
float A_kal
float R_kal
signed short int16_t
Definition: types.h:17
void CN_matrix_butterworth(void)
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
float waypoint_rot
float V_hys_low
#define AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES
Obstacle avoidance methods.
float alpha_fil
Definition: guidance_OA.c:112
float ref_roll
Definition: guidance_OA.c:104
Paparazzi fixed point math for geodetic calculations.
void CN_matrix_Kalman_filter(void)
int16_t focal
uint8_t point_index
float vmin
float Kg
float ref_yaw
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
float K_goal
float Ko
float v_min
void setAnglesMeasurements(float *anglesMeasurements, float *centersensorRad, float *fieldOfViewRad, uint16_t *size_matrix_local)
float Q_kal
struct FloatVect3 Repulsionforce_Used
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
float speed_pot
Definition: guidance_OA.c:110
struct FloatVect3 Attractforce_goal_send
unsigned char uint8_t
Definition: types.h:14
#define AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES
API to get/set the generic vehicle states.
uint8_t direction
float c2_oa
float kv
float vref_max
Definition: guidance_OA.c:99
#define AVOIDANCE_AMOUNT_OF_BOARDS
float reference_roll
struct FloatVect3 Repulsionforce_Kan
float Pest_new[AVOIDANCE_AMOUNT_OF_BOARDS *AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES *AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES]
void CN_vector_velocity(void)
uint8_t set_bias
struct FloatVect3 Repulsionforce_Kan_old
signed char int8_t
Definition: types.h:15
float c4_oa
struct FloatVect2 target
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
struct FloatVect3 Attractforce_goal
float c3_oa
struct FloatVect3 Repulsionforce_Kan_send
void serial_start(void)
oa_method OA_method_flag
Definition: guidance_OA.c:96
float x
in meters
float K_obst
float new_heading_old
float dy_ref
void CN_vector_escape_velocity(void)
float new_heading
Definition: guidance_OA.c:114