Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
potential.c
Go to the documentation of this file.
1 
4 #define POTENTIAL_C
5 
6 #include <math.h>
7 
8 #ifndef DOWNLINK_DEVICE
9 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
10 #endif
12 #include "dl_protocol.h"
13 
14 #include "potential.h"
15 #include "estimator.h"
18 #include "autopilot.h"
19 #include "subsystems/gps.h"
20 #include "generated/airframe.h"
21 
22 //#include <stdio.h>
23 
25 
29 
30 #ifndef FORCE_POS_GAIN
31 #define FORCE_POS_GAIN 1.
32 #endif
33 
34 #ifndef FORCE_SPEED_GAIN
35 #define FORCE_SPEED_GAIN 1.
36 #endif
37 
38 #ifndef FORCE_CLIMB_GAIN
39 #define FORCE_CLIMB_GAIN 1.
40 #endif
41 
42 #ifndef FORCE_MAX_DIST
43 #define FORCE_MAX_DIST 100.
44 #endif
45 
46 void potential_init(void) {
47 
48  potential_force.east = 0.;
50  potential_force.alt = 0.;
51 
55 
56 }
57 
58 int potential_task(void) {
59 
60  uint8_t i;
61 
62  float ch = cosf(estimator_hspeed_dir);
63  float sh = sinf(estimator_hspeed_dir);
64  potential_force.east = 0.;
66  potential_force.alt = 0.;
67 
68  // compute control forces
69  int8_t nb = 0;
70  for (i = 0; i < NB_ACS; ++i) {
71  if (the_acs[i].ac_id == AC_ID) continue;
72  struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
73  float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
74  // if AC not responding for too long, continue, else compute force
75  if (delta_t > CARROT) continue;
76  else {
77  float sha = sinf(ac->course);
78  float cha = cosf(ac->course);
79  float de = ac->east + sha*delta_t - estimator_x;
80  if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) continue;
81  float dn = ac->north + cha*delta_t - estimator_y;
82  if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) continue;
83  float da = ac->alt + ac->climb*delta_t - estimator_z;
84  if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) continue;
85  float dist = sqrtf(de*de + dn*dn + da*da);
86  if (dist == 0.) continue;
87  float dve = estimator_hspeed_mod * sh - ac->gspeed * sha;
88  float dvn = estimator_hspeed_mod * ch - ac->gspeed * cha;
89  float dva = estimator_z_dot - the_acs[i].climb;
90  float scal = dve*de + dvn*dn + dva*da;
91  if (scal < 0.) continue; // No risk of collision
92  float d3 = dist * dist * dist;
93  potential_force.east += scal * de / d3;
94  potential_force.north += scal * dn / d3;
95  potential_force.alt += scal * da / d3;
96  ++nb;
97  }
98  }
99  if (nb == 0) return TRUE;
100  //potential_force.east /= nb;
101  //potential_force.north /= nb;
102  //potential_force.alt /= nb;
103 
104  // set commands
106 
107  // carrot
108  float dx = -force_pos_gain * potential_force.east;
109  float dy = -force_pos_gain * potential_force.north;
110  desired_x += dx;
111  desired_y += dy;
112  // fly to desired
114 
115  // speed loop
116  float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
117  cruise += -force_speed_gain * (potential_force.north * ch + potential_force.east * sh);
118  Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
119  potential_force.speed = cruise;
121 
122  // climb loop
126 
128 
129  return TRUE;
130 }
131 
#define FORCE_MAX_DIST
Definition: potential.c:43
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
float force_speed_gain
Definition: potential.c:27
struct ac_info_ the_acs[NB_ACS]
Definition: traffic_info.c:35
float estimator_z_dot
Definition: estimator.c:46
float north
Definition: traffic_info.h:38
struct force_ potential_force
Definition: potential.c:24
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:140
float climb
Definition: potential.h:18
int potential_task(void)
Definition: potential.c:58
float north
Definition: potential.h:15
float force_pos_gain
Definition: potential.c:26
float estimator_y
north position in meters
Definition: estimator.c:43
float alt
Definition: potential.h:16
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:44
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
float east
Definition: traffic_info.h:37
uint32_t itow
Definition: traffic_info.h:43
float force_climb_gain
Definition: potential.c:28
float alt
Definition: traffic_info.h:40
uint32_t tow
GPS time of week in ms.
Definition: gps.h:79
#define FORCE_POS_GAIN
Definition: potential.c:31
#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:53
Device independent GPS code (interface)
float east
Definition: potential.h:14
float course
Definition: traffic_info.h:39
float estimator_x
east position in meters
Definition: estimator.c:42
void potential_init(void)
Definition: potential.c:46
#define TRUE
Definition: imu_chimu.h:144
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:106
#define NB_ACS
Definition: traffic_info.h:33
unsigned char uint8_t
Definition: types.h:14
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
float climb
Definition: traffic_info.h:42
State estimation, fusioning sensors.
#define FORCE_SPEED_GAIN
Definition: potential.c:35
uint8_t ac_id
Definition: jsbsim_hw.c:28
signed char int8_t
Definition: types.h:15
struct GpsState gps
global GPS state
Definition: gps.c:31
float gspeed
Definition: traffic_info.h:41
#define FORCE_CLIMB_GAIN
Definition: potential.c:39