Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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  potential_force.east = 0.;
47  potential_force.alt = 0.;
48 
52 
53 }
54 
55 int potential_task(void) {
56 
57  uint8_t i;
58 
59  float ch = cosf((*stateGetHorizontalSpeedDir_f()));
60  float sh = sinf((*stateGetHorizontalSpeedDir_f()));
61  potential_force.east = 0.;
63  potential_force.alt = 0.;
64 
65  // compute control forces
66  int8_t nb = 0;
67  for (i = 0; i < NB_ACS; ++i) {
68  if (the_acs[i].ac_id == AC_ID) continue;
69  struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
70  float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
71  // if AC not responding for too long, continue, else compute force
72  if (delta_t > CARROT) continue;
73  else {
74  float sha = sinf(ac->course);
75  float cha = cosf(ac->course);
76  float de = ac->east + sha*delta_t - stateGetPositionEnu_f()->x;
77  if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) continue;
78  float dn = ac->north + cha*delta_t - stateGetPositionEnu_f()->y;
79  if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) continue;
80  float da = ac->alt + ac->climb*delta_t - stateGetPositionUtm_f()->alt;
81  if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) continue;
82  float dist = sqrtf(de*de + dn*dn + da*da);
83  if (dist == 0.) continue;
84  float dve = (*stateGetHorizontalSpeedNorm_f()) * sh - ac->gspeed * sha;
85  float dvn = (*stateGetHorizontalSpeedNorm_f()) * ch - ac->gspeed * cha;
86  float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb;
87  float scal = dve*de + dvn*dn + dva*da;
88  if (scal < 0.) continue; // No risk of collision
89  float d3 = dist * dist * dist;
90  potential_force.east += scal * de / d3;
91  potential_force.north += scal * dn / d3;
92  potential_force.alt += scal * da / d3;
93  ++nb;
94  }
95  }
96  if (nb == 0) return TRUE;
97  //potential_force.east /= nb;
98  //potential_force.north /= nb;
99  //potential_force.alt /= nb;
100 
101  // set commands
103 
104  // carrot
105  float dx = -force_pos_gain * potential_force.east;
106  float dy = -force_pos_gain * potential_force.north;
107  desired_x += dx;
108  desired_y += dy;
109  // fly to desired
111 
112  // speed loop
113  float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
114  cruise += -force_speed_gain * (potential_force.north * ch + potential_force.east * sh);
115  Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
116  potential_force.speed = cruise;
118 
119  // climb loop
123 
125 
126  return TRUE;
127 }
128 
#define FORCE_MAX_DIST
Definition: potential.c:40
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
float force_speed_gain
Definition: potential.c:24
struct ac_info_ the_acs[NB_ACS]
Definition: traffic_info.c:35
float north
Definition: traffic_info.h:38
struct force_ potential_force
Definition: potential.c:21
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:141
float climb
Definition: potential.h:18
float z
in meters
int potential_task(void)
Definition: potential.c:55
float north
Definition: potential.h:15
float force_pos_gain
Definition: potential.c:23
float alt
Definition: potential.h:16
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:44
float y
in meters
float east
Definition: traffic_info.h:37
uint32_t itow
Definition: traffic_info.h:43
float force_climb_gain
Definition: potential.c:25
float alt
Definition: traffic_info.h:40
Fixed wing horizontal control.
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
#define FORCE_POS_GAIN
Definition: potential.c:28
#define Max(x, y)
float speed
Definition: potential.h:17
Vertical control for fixed wing vehicles.
flying with potential field to avoid collision
uint16_t dn[LIGHT_NB]
Definition: light_solar.c:48
Device independent GPS code (interface)
float east
Definition: potential.h:14
float x
in meters
float course
Definition: traffic_info.h:39
void potential_init(void)
Definition: potential.c:43
#define TRUE
Definition: imu_chimu.h:144
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:106
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
#define NB_ACS
Definition: traffic_info.h:33
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:840
float climb
Definition: traffic_info.h:42
#define FORCE_SPEED_GAIN
Definition: potential.c:32
uint8_t ac_id
Definition: jsbsim_hw.c:27
signed char int8_t
Definition: types.h:15
float alt
in meters above WGS84 reference ellipsoid
struct GpsState gps
global GPS state
Definition: gps.c:41
float gspeed
Definition: traffic_info.h:41
#define FORCE_CLIMB_GAIN
Definition: potential.c:36