Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
formation.c
Go to the documentation of this file.
1 
5 #define FORMATION_C
6 
7 #include <math.h>
8 
9 #ifndef DOWNLINK_DEVICE
10 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
11 #endif
13 
14 #include "multi/formation.h"
15 #include "estimator.h"
18 #include "autopilot.h"
19 #include "subsystems/gps.h"
20 #include "generated/flight_plan.h"
21 #include "generated/airframe.h"
22 #include "dl_protocol.h"
23 
24 #include <stdio.h>
25 
28 
30 float form_prox;
38 
40 
41 #ifndef FORM_CARROT
42 #define FORM_CARROT 2.
43 #endif
44 
45 #ifndef FORM_POS_PGAIN
46 #define FORM_POS_PGAIN 0.
47 #endif
48 
49 #ifndef FORM_SPEED_PGAIN
50 #define FORM_SPEED_PGAIN 0.
51 #endif
52 
53 #ifndef FORM_COURSE_PGAIN
54 #define FORM_COURSE_PGAIN 0.
55 #endif
56 
57 #ifndef FORM_ALTITUDE_PGAIN
58 #define FORM_ALTITUDE_PGAIN 0.
59 #endif
60 
61 #ifndef FORM_PROX
62 #define FORM_PROX 20.
63 #endif
64 
65 #ifndef FORM_MODE
66 #define FORM_MODE 0
67 #endif
68 
69 int formation_init(void) {
70  int i;
71  for (i = 0; i < NB_ACS; ++i) formation[i].status = UNSET;
72 
73  leader_id = 0;
81  old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
82  old_alt = GROUND_ALT + SECURITY_HEIGHT;
83  return FALSE;
84 }
85 
86 int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a) {
87  if (_id != AC_ID && the_acs_id[_id] == 0) return FALSE; // no info for this AC
88  DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice,&_id, &form_mode, &slot_e, &slot_n, &slot_a);
90  formation[the_acs_id[_id]].east = slot_e;
91  formation[the_acs_id[_id]].north = slot_n;
92  formation[the_acs_id[_id]].alt = slot_a;
93  return FALSE;
94 }
95 
96 int start_formation(void) {
97  uint8_t i;
98  uint8_t ac_id = AC_ID;
99  for (i = 0; i < NB_ACS; ++i) {
100  if (formation[i].status == IDLE) formation[i].status = ACTIVE;
101  }
102  enum slot_status active = ACTIVE;
103  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&active);
104  // store current cruise and alt
107  return FALSE;
108 }
109 
110 int stop_formation(void) {
111  uint8_t i;
112  uint8_t ac_id = AC_ID;
113  for (i = 0; i < NB_ACS; ++i) {
114  if (formation[i].status == ACTIVE) formation[i].status = IDLE;
115  }
116  enum slot_status idle = IDLE;
117  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&idle);
118  // restore cruise and alt
120  old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
122  old_alt = GROUND_ALT + SECURITY_HEIGHT;
123  return FALSE;
124 }
125 
126 
127 int formation_flight(void) {
128 
129  static uint8_t _1Hz = 0;
130  uint8_t nb = 0, i;
131  float ch = cosf(estimator_hspeed_dir);
132  float sh = sinf(estimator_hspeed_dir);
133  form_n = 0.;
134  form_e = 0.;
135  form_a = 0.;
139 
140  if (AC_ID == leader_id) {
143  }
144  // set info for this AC
146 
147  // broadcast info
148  uint8_t ac_id = AC_ID;
150  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&status);
151  if (++_1Hz>=4) {
152  _1Hz=0;
153  DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice,&ac_id, &form_mode,
154  &formation[the_acs_id[AC_ID]].east,
155  &formation[the_acs_id[AC_ID]].north,
156  &formation[the_acs_id[AC_ID]].alt);
157  }
158  if (formation[the_acs_id[AC_ID]].status != ACTIVE) return FALSE; // AC not ready
159 
160  // get leader info
161  struct ac_info_ * leader = get_ac_info(leader_id);
162  if (formation[the_acs_id[leader_id]].status == UNSET ||
163  formation[the_acs_id[leader_id]].status == IDLE) return FALSE; // leader not ready or not in formation
164 
165  // compute slots in the right reference frame
166  struct slot_ form[NB_ACS];
167  float cr = 0., sr = 1.;
168  if (form_mode == FORM_MODE_COURSE) {
169  cr = cosf(leader->course);
170  sr = sinf(leader->course);
171  }
172  for (i = 0; i < NB_ACS; ++i) {
173  if (formation[i].status == UNSET) continue;
174  form[i].east = formation[i].east*sr - formation[i].north*cr;
175  form[i].north = formation[i].east*cr + formation[i].north*sr;
176  form[i].alt = formation[i].alt;
177  }
178 
179  // compute control forces
180  for (i = 0; i < NB_ACS; ++i) {
181  if (the_acs[i].ac_id == AC_ID) continue;
182  struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
183  float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
184  if (delta_t > FORM_CARROT) {
185  // if AC not responding for too long
186  formation[i].status = LOST;
187  continue;
188  }
189  else formation[i].status = ACTIVE;
190  // compute control if AC is ACTIVE and around the same altitude (maybe not so usefull)
191  if (formation[i].status == ACTIVE && fabs(estimator_z - ac->alt) < form_prox && ac->alt > 0) {
192  form_e += (ac->east + ac->gspeed*sinf(ac->course)*delta_t - estimator_x)
193  - (form[i].east - form[the_acs_id[AC_ID]].east);
194  form_n += (ac->north + ac->gspeed*cosf(ac->course)*delta_t - estimator_y)
195  - (form[i].north - form[the_acs_id[AC_ID]].north);
196  form_a += (ac->alt - estimator_z) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
197  form_speed += ac->gspeed;
198  //form_speed_e += ac->gspeed * sinf(ac->course);
199  //form_speed_n += ac->gspeed * cosf(ac->course);
200  ++nb;
201  }
202  }
203  uint8_t _nb = Max(1,nb);
204  form_n /= _nb;
205  form_e /= _nb;
206  form_a /= _nb;
208  //form_speed_e = form_speed_e / (nb+1) - estimator_hspeed_mod * sh;
209  //form_speed_n = form_speed_n / (nb+1) - estimator_hspeed_mod * ch;
210 
211  // set commands
213 
214  // altitude loop
215  float alt = 0.;
216  if (AC_ID == leader_id) alt = nav_altitude;
217  else alt = leader->alt - form[the_acs_id[leader_id]].alt;
218  alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a;
219  flight_altitude = Max(alt, ground_alt+SECURITY_HEIGHT);
220 
221  // carrot
222  if (AC_ID != leader_id) {
223  float dx = form[the_acs_id[AC_ID]].east - form[the_acs_id[leader_id]].east;
224  float dy = form[the_acs_id[AC_ID]].north - form[the_acs_id[leader_id]].north;
225  desired_x = leader->east + NOMINAL_AIRSPEED * form_carrot * sinf(leader->course) + dx;
226  desired_y = leader->north + NOMINAL_AIRSPEED * form_carrot * cosf(leader->course) + dy;
227  // fly to desired
229  desired_x = leader->east + dx;
230  desired_y = leader->north + dy;
231  // lateral correction
232  //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
233  //float diff_course = leader->course - estimator_hspeed_dir;
234  //NormRadAngle(diff_course);
235  //h_ctl_roll_setpoint += coef_form_course * diff_course;
236  //h_ctl_roll_setpoint += coef_form_course * diff_heading;
237  }
238  //BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
239 
240  // speed loop
241  if (nb > 0) {
242  float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
243  cruise += coef_form_pos * (form_n * ch + form_e * sh) + coef_form_speed * form_speed;
244  Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
246  }
247  return TRUE;
248 }
249 
250 void formation_pre_call(void) {
251  if (leader_id == AC_ID) {
254  }
255 }
256 
status
Definition: anemotaxis.c:10
float north
Definition: formation.h:26
float old_alt
Definition: formation.c:37
uint8_t leader_id
Definition: formation.c:36
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
enum slot_status status
Definition: formation.h:24
uint8_t the_acs_id[NB_ACS_ID]
Definition: traffic_info.c:34
struct ac_info_ the_acs[NB_ACS]
Definition: traffic_info.c:35
int start_formation(void)
Definition: formation.c:96
float form_n
Definition: formation.c:26
float form_prox
Definition: formation.c:30
float coef_form_pos
Definition: formation.c:31
#define FORM_POS_PGAIN
Definition: formation.c:46
int formation_flight(void)
Definition: formation.c:127
#define FORM_MODE
Definition: formation.c:66
#define FORM_COURSE_PGAIN
Definition: formation.c:54
float estimator_z_dot
Definition: estimator.c:46
int stop_formation(void)
Definition: formation.c:110
float north
Definition: traffic_info.h:38
float estimator_y
north position in meters
Definition: estimator.c:43
struct slot_ formation[NB_ACS]
Definition: formation.c:39
float form_speed_n
Definition: formation.c:27
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
#define FALSE
Definition: imu_chimu.h:141
uint32_t itow
Definition: traffic_info.h:43
float alt
Definition: traffic_info.h:40
uint32_t tow
GPS time of week in ms.
Definition: gps.h:79
float form_speed_e
Definition: formation.c:27
int form_mode
Definition: formation.c:35
float form_carrot
Definition: formation.c:29
#define Max(x, y)
Vertical control for fixed wing vehicles.
#define FORM_ALTITUDE_PGAIN
Definition: formation.c:58
float east
Definition: formation.h:25
Device independent GPS code (interface)
float old_cruise
Definition: formation.c:37
float coef_form_alt
Definition: formation.c:34
float alt
Definition: formation.h:27
#define FORM_PROX
Definition: formation.c:62
float course
Definition: traffic_info.h:39
float estimator_x
east position in meters
Definition: estimator.c:42
#define SetAcInfo(_id, _utm_x, _utm_y, _course, _alt, _gspeed, _climb, _itow)
Definition: traffic_info.h:52
#define TRUE
Definition: imu_chimu.h:144
float v_ctl_auto_throttle_cruise_throttle
Definition: guidance_v.c:55
float form_speed
Definition: formation.c:27
#define FORM_SPEED_PGAIN
Definition: formation.c:50
#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
Formation flight library.
void formation_pre_call(void)
Definition: formation.c:250
#define FORM_MODE_COURSE
Definition: formation.h:14
float coef_form_speed
Definition: formation.c:32
State estimation, fusioning sensors.
float coef_form_course
Definition: formation.c:33
#define FORM_CARROT
Definition: formation.c:42
float form_a
Definition: formation.c:26
slot_status
Definition: formation.h:21
uint8_t ac_id
Definition: jsbsim_hw.c:28
int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a)
Definition: formation.c:86
float form_e
Definition: formation.c:26
struct GpsState gps
global GPS state
Definition: gps.c:31
float gspeed
Definition: traffic_info.h:41
int formation_init(void)
Definition: formation.c:69