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
potential.c
Go to the documentation of this file.
1 
4 #define POTENTIAL_C
5 
6 #include <math.h>
7 
9 #include "pprzlink/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.;
51 
55 
56 }
57 
58 int potential_task(void)
59 {
60 
61  uint8_t i;
62 
63  float ch = cosf(stateGetHorizontalSpeedDir_f());
64  float sh = sinf(stateGetHorizontalSpeedDir_f());
65  potential_force.east = 0.;
67  potential_force.alt = 0.;
68 
69  // compute control forces
70  int8_t nb = 0;
71  for (i = 0; i < NB_ACS; ++i) {
72  if (ti_acs[i].ac_id == AC_ID) { continue; }
74  struct EnuCoor_f *ac_speed = acInfoGetVelocityEnu_f(ti_acs[i].ac_id);
75  float delta_t = Max((int)(gps.tow - acInfoGetItow(ti_acs[i].ac_id)) / 1000., 0.);
76  // if AC not responding for too long, continue, else compute force
77  if (delta_t > CARROT) { continue; }
78  else {
79  float de = ac->x + ac_speed->x * delta_t - stateGetPositionEnu_f()->x;
80  if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) { continue; }
81  float dn = ac->y + ac_speed->y * delta_t - stateGetPositionEnu_f()->y;
82  if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) { continue; }
83  float da = ac->z + ac_speed->z * delta_t - stateGetPositionEnu_f()->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 = stateGetHorizontalSpeedNorm_f() * sh - ac_speed->x;
88  float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac_speed->y;
89  float dva = stateGetSpeedEnu_f()->z - ac_speed->z;
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 
129 
130  return true;
131 }
uint8_t ac_id
Definition: sim_ap.c:48
#define FORCE_MAX_DIST
Definition: potential.c:40
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
float north
Definition: potential.h:15
float force_speed_gain
Definition: potential.c:24
float climb
Definition: potential.h:18
vector in East North Up coordinates Units: meters
struct force_ potential_force
Definition: potential.c:21
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:146
int potential_task(void)
Definition: potential.c:58
float force_pos_gain
Definition: potential.c:23
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
float force_climb_gain
Definition: potential.c:25
Fixed wing horizontal control.
float alt
Definition: potential.h:16
#define FORCE_POS_GAIN
Definition: potential.c:28
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
static uint32_t acInfoGetItow(uint8_t ac_id)
Get time of week from latest message (ms).
Definition: traffic_info.h:440
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:109
#define NB_ACS
Definition: rssi.c:38
uint16_t dn[LIGHT_NB]
Definition: light_solar.c:48
Device independent GPS code (interface)
static struct EnuCoor_f * acInfoGetVelocityEnu_f(uint8_t ac_id)
Get position from ENU coordinates (float).
Definition: traffic_info.h:405
#define Max(x, y)
Definition: main_fbw.c:53
void potential_init(void)
Definition: potential.c:43
Core autopilot interface common to all firmwares.
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
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:917
float z
in meters
struct acInfo ti_acs[NB_ACS]
Definition: traffic_info.c:45
#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:69
#define FORCE_CLIMB_GAIN
Definition: potential.c:36
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40