Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 }
force_::east
float east
Definition: potential.h:14
potential_init
void potential_init(void)
Definition: potential.c:43
stateGetHorizontalSpeedDir_f
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
force_::climb
float climb
Definition: potential.h:18
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
force_
Definition: potential.h:13
stabilization_attitude.h
force_::speed
float speed
Definition: potential.h:17
force_speed_gain
float force_speed_gain
Definition: potential.c:24
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
potential.h
flying with potential field to avoid collision
v_ctl_auto_throttle_cruise_throttle
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:171
FORCE_MAX_DIST
#define FORCE_MAX_DIST
Definition: potential.c:40
acInfoGetPositionEnu_f
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
CARROT
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40
FORCE_CLIMB_GAIN
#define FORCE_CLIMB_GAIN
Definition: potential.c:36
acInfoGetItow
static uint32_t acInfoGetItow(uint8_t ac_id)
Get time of week from latest message (ms).
Definition: traffic_info.h:440
gps.h
Device independent GPS code (interface)
desired_x
float desired_x
Definition: nav.c:306
NavVerticalClimbMode
#define NavVerticalClimbMode(_climb)
Set the vertical mode to climb control with the specified climb setpoint.
Definition: nav.h:192
desired_y
float desired_y
Definition: nav.c:306
FORCE_POS_GAIN
#define FORCE_POS_GAIN
Definition: potential.c:28
acInfoGetVelocityEnu_f
static struct EnuCoor_f * acInfoGetVelocityEnu_f(uint8_t ac_id)
Get position from ENU coordinates (float).
Definition: traffic_info.h:405
uint8_t
unsigned char uint8_t
Definition: types.h:14
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
autopilot.h
force_pos_gain
float force_pos_gain
Definition: potential.c:23
potential_force
struct force_ potential_force
Definition: potential.c:21
int8_t
signed char int8_t
Definition: types.h:15
ti_acs
struct acInfo ti_acs[NB_ACS]
Definition: traffic_info.c:45
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
stateGetSpeedEnu_f
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
NB_ACS
#define NB_ACS
Definition: rssi.c:38
force_climb_gain
float force_climb_gain
Definition: potential.c:25
force_::north
float north
Definition: potential.h:15
FORCE_SPEED_GAIN
#define FORCE_SPEED_GAIN
Definition: potential.c:32
V_CTL_ALTITUDE_MAX_CLIMB
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:146
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
Max
#define Max(x, y)
Definition: main_fbw.c:53
force_::alt
float alt
Definition: potential.h:16
state.h
ac_id
uint8_t ac_id
Definition: sim_ap.c:48
dn
uint16_t dn[LIGHT_NB]
Definition: light_solar.c:48
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
potential_task
int potential_task(void)
Definition: potential.c:58
fly_to_xy
void fly_to_xy(float x, float y)
Computes desired_x, desired_y and desired_course.
Definition: nav.c:355
guidance_v.h