Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 "modules/gps/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
57
59{
60
61 uint8_t i;
62
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; }
73 struct EnuCoor_f *ac = acInfoGetPositionEnu_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;
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;
96 ++nb;
97 }
98 }
99 if (nb == 0) { return true; }
100 potential_force.east /= nb;
102 potential_force.alt /= nb;
103
104 // set commands
106
107 // carrot
108 float dx = -force_pos_gain * potential_force.east;
110 desired_x += dx;
111 desired_y += dy;
112 // fly to desired
114
115 // speed loop
121
122 // climb loop
126
129
130 return true;
131}
Core autopilot interface common to all firmwares.
#define V_CTL_ALTITUDE_MAX_CLIMB
float v_ctl_auto_throttle_cruise_throttle
Vertical control for fixed wing vehicles.
Fixed wing horizontal control.
struct GpsState gps
global GPS state
Definition gps.c:74
Device independent GPS code (interface)
uint32_t tow
GPS time of week in ms.
Definition gps.h:109
static struct EnuCoor_f * acInfoGetVelocityEnu_f(uint8_t ac_id)
Get position from ENU coordinates (float).
static uint32_t acInfoGetItow(uint8_t ac_id)
Get time of week from latest message (ms).
struct acInfo ti_acs[NB_ACS]
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:848
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition state.h:1076
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition state.h:1085
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1058
uint16_t dn[LIGHT_NB]
Definition light_solar.c:48
uint16_t foo
Definition main_demo5.c:58
float desired_x
Definition nav.c:308
float desired_y
Definition nav.c:308
void fly_to_xy(float x, float y)
Computes desired_x, desired_y and desired_course.
Definition nav.c:356
#define NavVerticalClimbMode(_climb)
Set the vertical mode to climb control with the specified climb setpoint.
Definition nav.h:198
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition nav.h:177
int potential_task(void)
Definition potential.c:58
#define FORCE_POS_GAIN
Definition potential.c:28
float force_climb_gain
Definition potential.c:25
void potential_init(void)
Definition potential.c:43
struct force_ potential_force
Definition potential.c:21
#define FORCE_MAX_DIST
Definition potential.c:40
#define FORCE_SPEED_GAIN
Definition potential.c:32
float force_pos_gain
Definition potential.c:23
#define FORCE_CLIMB_GAIN
Definition potential.c:36
float force_speed_gain
Definition potential.c:24
flying with potential field to avoid collision
float east
Definition potential.h:14
float north
Definition potential.h:15
float climb
Definition potential.h:18
float alt
Definition potential.h:16
float speed
Definition potential.h:17
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
#define CARROT
default approaching_time for a wp
Definition navigation.h:70
#define NB_ACS
Definition rssi.c:38
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.