Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
potential.c
Go to the documentation of this file.
1 
4 #define POTENTIAL_C
5 
6 #include <math.h>
7 
9 #include "dl_protocol.h"
10 
11 #include "potential.h"
12 #include "state.h"
15 #include "autopilot.h"
16 #include "subsystems/gps.h"
17 #include "generated/airframe.h"
18 
19 //#include <stdio.h>
20 
22 
26 
27 #ifndef FORCE_POS_GAIN
28 #define FORCE_POS_GAIN 1.
29 #endif
30 
31 #ifndef FORCE_SPEED_GAIN
32 #define FORCE_SPEED_GAIN 1.
33 #endif
34 
35 #ifndef FORCE_CLIMB_GAIN
36 #define FORCE_CLIMB_GAIN 1.
37 #endif
38 
39 #ifndef FORCE_MAX_DIST
40 #define FORCE_MAX_DIST 100.
41 #endif
42 
43 void potential_init(void)
44 {
45 
46  potential_force.east = 0.;
48  potential_force.alt = 0.;
49 
53 
54 }
55 
56 int potential_task(void)
57 {
58 
59  uint8_t i;
60 
61  float ch = cosf(stateGetHorizontalSpeedDir_f());
62  float sh = sinf(stateGetHorizontalSpeedDir_f());
63  potential_force.east = 0.;
65  potential_force.alt = 0.;
66 
67  // compute control forces
68  int8_t nb = 0;
69  for (i = 0; i < NB_ACS; ++i) {
70  if (the_acs[i].ac_id == AC_ID) { continue; }
71  struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
72  float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
73  // if AC not responding for too long, continue, else compute force
74  if (delta_t > CARROT) { continue; }
75  else {
76  float sha = sinf(ac->course);
77  float cha = cosf(ac->course);
78  float de = ac->east + sha * delta_t - stateGetPositionEnu_f()->x;
79  if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) { continue; }
80  float dn = ac->north + cha * delta_t - stateGetPositionEnu_f()->y;
81  if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) { continue; }
82  float da = ac->alt + ac->climb * delta_t - stateGetPositionUtm_f()->alt;
83  if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) { continue; }
84  float dist = sqrtf(de * de + dn * dn + da * da);
85  if (dist == 0.) { continue; }
86  float dve = stateGetHorizontalSpeedNorm_f() * sh - ac->gspeed * sha;
87  float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac->gspeed * cha;
88  float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb;
89  float scal = dve * de + dvn * dn + dva * da;
90  if (scal < 0.) { continue; } // No risk of collision
91  float d3 = dist * dist * dist;
92  potential_force.east += scal * de / d3;
93  potential_force.north += scal * dn / d3;
94  potential_force.alt += scal * da / d3;
95  ++nb;
96  }
97  }
98  if (nb == 0) { return TRUE; }
99  //potential_force.east /= nb;
100  //potential_force.north /= nb;
101  //potential_force.alt /= nb;
102 
103  // set commands
105 
106  // carrot
107  float dx = -force_pos_gain * potential_force.east;
108  float dy = -force_pos_gain * potential_force.north;
109  desired_x += dx;
110  desired_y += dy;
111  // fly to desired
113 
114  // speed loop
115  float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
116  cruise += -force_speed_gain * (potential_force.north * ch + potential_force.east * sh);
117  Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
118  potential_force.speed = cruise;
120 
121  // climb loop
125 
128 
129  return TRUE;
130 }
131 
uint8_t ac_id
Definition: sim_ap.c:47
#define FORCE_MAX_DIST
Definition: potential.c:40
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:912
float north
Definition: potential.h:15
float force_speed_gain
Definition: potential.c:24
struct ac_info_ the_acs[NB_ACS]
Definition: traffic_info.c:35
float alt
in meters above WGS84 reference ellipsoid
float gspeed
Definition: traffic_info.h:41
float climb
Definition: potential.h:18
float course
Definition: traffic_info.h:39
struct force_ potential_force
Definition: potential.c:21
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:145
int potential_task(void)
Definition: potential.c:56
float force_pos_gain
Definition: potential.c:23
float north
Definition: traffic_info.h:38
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:702
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:45
float force_climb_gain
Definition: potential.c:25
Fixed wing horizontal control.
float alt
Definition: traffic_info.h:40
float alt
Definition: potential.h:16
#define TRUE
Definition: std.h:4
#define FORCE_POS_GAIN
Definition: potential.c:28
#define Max(x, y)
float east
Definition: traffic_info.h:37
float x
in meters
Vertical control for fixed wing vehicles.
flying with potential field to avoid collision
uint32_t tow
GPS time of week in ms.
Definition: gps.h:84
uint32_t itow
Definition: traffic_info.h:43
uint16_t dn[LIGHT_NB]
Definition: light_solar.c:48
Device independent GPS code (interface)
void potential_init(void)
Definition: potential.c:43
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:921
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:675
#define NB_ACS
Definition: traffic_info.h:33
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float speed
Definition: potential.h:17
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:894
float z
in meters
float climb
Definition: traffic_info.h:42
#define FORCE_SPEED_GAIN
Definition: potential.c:32
signed char int8_t
Definition: types.h:15
float east
Definition: potential.h:14
float y
in meters
struct GpsState gps
global GPS state
Definition: gps.c:41
#define FORCE_CLIMB_GAIN
Definition: potential.c:36