Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
formation.c
Go to the documentation of this file.
1 
5 #define FORMATION_C
6 
7 #include <math.h>
8 
10 
11 #include "multi/formation.h"
12 #include "state.h"
15 #include "autopilot.h"
16 #include "subsystems/gps.h"
17 #include "generated/flight_plan.h"
18 #include "generated/airframe.h"
19 #include "dl_protocol.h"
20 
21 #include <stdio.h>
22 
25 
27 float form_prox;
35 
37 
38 #ifndef FORM_CARROT
39 #define FORM_CARROT 2.
40 #endif
41 
42 #ifndef FORM_POS_PGAIN
43 #define FORM_POS_PGAIN 0.
44 #endif
45 
46 #ifndef FORM_SPEED_PGAIN
47 #define FORM_SPEED_PGAIN 0.
48 #endif
49 
50 #ifndef FORM_COURSE_PGAIN
51 #define FORM_COURSE_PGAIN 0.
52 #endif
53 
54 #ifndef FORM_ALTITUDE_PGAIN
55 #define FORM_ALTITUDE_PGAIN 0.
56 #endif
57 
58 #ifndef FORM_PROX
59 #define FORM_PROX 20.
60 #endif
61 
62 #ifndef FORM_MODE
63 #define FORM_MODE 0
64 #endif
65 
66 int formation_init(void) {
67  int i;
68  for (i = 0; i < NB_ACS; ++i) formation[i].status = UNSET;
69 
70  leader_id = 0;
78  old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
79  old_alt = GROUND_ALT + SECURITY_HEIGHT;
80  return FALSE;
81 }
82 
83 int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a) {
84  if (_id != AC_ID && the_acs_id[_id] == 0) return FALSE; // no info for this AC
85  DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice,&_id, &form_mode, &slot_e, &slot_n, &slot_a);
87  formation[the_acs_id[_id]].east = slot_e;
88  formation[the_acs_id[_id]].north = slot_n;
89  formation[the_acs_id[_id]].alt = slot_a;
90  return FALSE;
91 }
92 
93 int start_formation(void) {
94  uint8_t i;
95  uint8_t ac_id = AC_ID;
96  for (i = 0; i < NB_ACS; ++i) {
97  if (formation[i].status == IDLE) formation[i].status = ACTIVE;
98  }
99  enum slot_status active = ACTIVE;
100  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&active);
101  // store current cruise and alt
104  return FALSE;
105 }
106 
107 int stop_formation(void) {
108  uint8_t i;
109  uint8_t ac_id = AC_ID;
110  for (i = 0; i < NB_ACS; ++i) {
111  if (formation[i].status == ACTIVE) formation[i].status = IDLE;
112  }
113  enum slot_status idle = IDLE;
114  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&idle);
115  // restore cruise and alt
117  old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
119  old_alt = GROUND_ALT + SECURITY_HEIGHT;
120  return FALSE;
121 }
122 
123 
124 int formation_flight(void) {
125 
126  static uint8_t _1Hz = 0;
127  uint8_t nb = 0, i;
128  float ch = cosf((*stateGetHorizontalSpeedDir_f()));
129  float sh = sinf((*stateGetHorizontalSpeedDir_f()));
130  form_n = 0.;
131  form_e = 0.;
132  form_a = 0.;
136 
137  if (AC_ID == leader_id) {
140  }
141  // set info for this AC
143 
144  // broadcast info
145  uint8_t ac_id = AC_ID;
147  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&status);
148  if (++_1Hz>=4) {
149  _1Hz=0;
150  DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice,&ac_id, &form_mode,
151  &formation[the_acs_id[AC_ID]].east,
152  &formation[the_acs_id[AC_ID]].north,
153  &formation[the_acs_id[AC_ID]].alt);
154  }
155  if (formation[the_acs_id[AC_ID]].status != ACTIVE) return FALSE; // AC not ready
156 
157  // get leader info
158  struct ac_info_ * leader = get_ac_info(leader_id);
159  if (formation[the_acs_id[leader_id]].status == UNSET ||
160  formation[the_acs_id[leader_id]].status == IDLE) return FALSE; // leader not ready or not in formation
161 
162  // compute slots in the right reference frame
163  struct slot_ form[NB_ACS];
164  float cr = 0., sr = 1.;
165  if (form_mode == FORM_MODE_COURSE) {
166  cr = cosf(leader->course);
167  sr = sinf(leader->course);
168  }
169  for (i = 0; i < NB_ACS; ++i) {
170  if (formation[i].status == UNSET) continue;
171  form[i].east = formation[i].east*sr - formation[i].north*cr;
172  form[i].north = formation[i].east*cr + formation[i].north*sr;
173  form[i].alt = formation[i].alt;
174  }
175 
176  // compute control forces
177  for (i = 0; i < NB_ACS; ++i) {
178  if (the_acs[i].ac_id == AC_ID) continue;
179  struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
180  float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
181  if (delta_t > FORM_CARROT) {
182  // if AC not responding for too long
183  formation[i].status = LOST;
184  continue;
185  }
186  else formation[i].status = ACTIVE;
187  // compute control if AC is ACTIVE and around the same altitude (maybe not so usefull)
188  if (formation[i].status == ACTIVE && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox && ac->alt > 0) {
189  form_e += (ac->east + ac->gspeed*sinf(ac->course)*delta_t - stateGetPositionEnu_f()->x)
190  - (form[i].east - form[the_acs_id[AC_ID]].east);
191  form_n += (ac->north + ac->gspeed*cosf(ac->course)*delta_t - stateGetPositionEnu_f()->y)
192  - (form[i].north - form[the_acs_id[AC_ID]].north);
193  form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
194  form_speed += ac->gspeed;
195  //form_speed_e += ac->gspeed * sinf(ac->course);
196  //form_speed_n += ac->gspeed * cosf(ac->course);
197  ++nb;
198  }
199  }
200  uint8_t _nb = Max(1,nb);
201  form_n /= _nb;
202  form_e /= _nb;
203  form_a /= _nb;
205  //form_speed_e = form_speed_e / (nb+1) - (*stateGetHorizontalSpeedNorm_f()) * sh;
206  //form_speed_n = form_speed_n / (nb+1) - (*stateGetHorizontalSpeedNorm_f()) * ch;
207 
208  // set commands
210 
211  // altitude loop
212  float alt = 0.;
213  if (AC_ID == leader_id) alt = nav_altitude;
214  else alt = leader->alt - form[the_acs_id[leader_id]].alt;
215  alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a;
216  flight_altitude = Max(alt, ground_alt+SECURITY_HEIGHT);
217 
218  // carrot
219  if (AC_ID != leader_id) {
220  float dx = form[the_acs_id[AC_ID]].east - form[the_acs_id[leader_id]].east;
221  float dy = form[the_acs_id[AC_ID]].north - form[the_acs_id[leader_id]].north;
222  desired_x = leader->east + NOMINAL_AIRSPEED * form_carrot * sinf(leader->course) + dx;
223  desired_y = leader->north + NOMINAL_AIRSPEED * form_carrot * cosf(leader->course) + dy;
224  // fly to desired
226  desired_x = leader->east + dx;
227  desired_y = leader->north + dy;
228  // lateral correction
229  //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
230  //float diff_course = leader->course - (*stateGetHorizontalSpeedDir_f());
231  //NormRadAngle(diff_course);
232  //h_ctl_roll_setpoint += coef_form_course * diff_course;
233  //h_ctl_roll_setpoint += coef_form_course * diff_heading;
234  }
235  //BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
236 
237  // speed loop
238  if (nb > 0) {
239  float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
240  cruise += coef_form_pos * (form_n * ch + form_e * sh) + coef_form_speed * form_speed;
241  Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
243  }
244  return TRUE;
245 }
246 
247 void formation_pre_call(void) {
248  if (leader_id == AC_ID) {
251  }
252 }
253 
status
Definition: anemotaxis.c:10
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
float north
Definition: formation.h:26
float old_alt
Definition: formation.c:34
uint8_t leader_id
Definition: formation.c:33
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:93
float form_n
Definition: formation.c:23
float form_prox
Definition: formation.c:27
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:40
float coef_form_pos
Definition: formation.c:28
#define FORM_POS_PGAIN
Definition: formation.c:43
int formation_flight(void)
Definition: formation.c:124
#define FORM_MODE
Definition: formation.c:63
#define FORM_COURSE_PGAIN
Definition: formation.c:51
int stop_formation(void)
Definition: formation.c:107
float north
Definition: traffic_info.h:38
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
struct slot_ formation[NB_ACS]
Definition: formation.c:36
float form_speed_n
Definition: formation.c:24
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:44
float y
in meters
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
Fixed wing horizontal control.
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
float form_speed_e
Definition: formation.c:24
int form_mode
Definition: formation.c:32
float form_carrot
Definition: formation.c:26
#define Max(x, y)
Vertical control for fixed wing vehicles.
#define FORM_ALTITUDE_PGAIN
Definition: formation.c:55
float east
Definition: formation.h:25
Definition: formation.h:21
Device independent GPS code (interface)
float old_cruise
Definition: formation.c:34
float coef_form_alt
Definition: formation.c:31
Definition: formation.h:21
float alt
Definition: formation.h:27
float x
in meters
#define FORM_PROX
Definition: formation.c:59
float course
Definition: traffic_info.h:39
#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 form_speed
Definition: formation.c:24
#define FORM_SPEED_PGAIN
Definition: formation.c:47
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:106
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
#define NB_ACS
Definition: traffic_info.h:33
unsigned char uint8_t
Definition: types.h:14
Formation flight library.
API to get/set the generic vehicle states.
void formation_pre_call(void)
Definition: formation.c:247
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:840
#define FORM_MODE_COURSE
Definition: formation.h:14
float coef_form_speed
Definition: formation.c:29
float coef_form_course
Definition: formation.c:30
#define FORM_CARROT
Definition: formation.c:39
float form_a
Definition: formation.c:23
slot_status
Definition: formation.h:21
uint8_t ac_id
Definition: jsbsim_hw.c:27
int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a)
Definition: formation.c:83
float form_e
Definition: formation.c:23
float alt
in meters above WGS84 reference ellipsoid
struct GpsState gps
global GPS state
Definition: gps.c:41
float gspeed
Definition: traffic_info.h:41
int formation_init(void)
Definition: formation.c:66