Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
formation.c
Go to the documentation of this file.
1 
5 #define FORMATION_C
6 
7 #include "multi/formation.h"
8 
9 #include "std.h"
10 #include "state.h"
11 
13 
16 
17 #include "generated/flight_plan.h" // SECURITY_HEIGHT
18 
21 
23 float form_prox;
31 
32 struct slot_ formation[NB_ACS];
33 
34 #ifndef FORM_CARROT
35 #define FORM_CARROT 2.
36 #endif
37 
38 #ifndef FORM_POS_PGAIN
39 #define FORM_POS_PGAIN 0.
40 #endif
41 
42 #ifndef FORM_SPEED_PGAIN
43 #define FORM_SPEED_PGAIN 0.
44 #endif
45 
46 #ifndef FORM_COURSE_PGAIN
47 #define FORM_COURSE_PGAIN 0.
48 #endif
49 
50 #ifndef FORM_ALTITUDE_PGAIN
51 #define FORM_ALTITUDE_PGAIN 0.
52 #endif
53 
54 #ifndef FORM_PROX
55 #define FORM_PROX 20.
56 #endif
57 
58 #ifndef FORM_MODE
59 #define FORM_MODE 0
60 #endif
61 
62 int formation_init(void)
63 {
64  int i;
65  for (i = 0; i < NB_ACS; ++i) { formation[i].status = UNSET; }
66 
67  leader_id = 0;
75  old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
76  old_alt = ground_alt + SECURITY_HEIGHT;
77  return false;
78 }
79 
80 int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a)
81 {
82  if (_id != AC_ID && ti_acs_id[_id] == 0) { return false; } // no info for this AC
83  DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &_id, &form_mode, &slot_e, &slot_n, &slot_a);
85  formation[ti_acs_id[_id]].east = slot_e;
86  formation[ti_acs_id[_id]].north = slot_n;
87  formation[ti_acs_id[_id]].alt = slot_a;
88  return false;
89 }
90 
91 int start_formation(void)
92 {
93  uint8_t i;
94  uint8_t ac_id = AC_ID;
95  for (i = 0; i < NB_ACS; ++i) {
96  if (formation[i].status == IDLE) { formation[i].status = ACTIVE; }
97  }
98  uint8_t active = ACTIVE;
99  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &active);
100  // store current cruise and alt
103  return false;
104 }
105 
106 int stop_formation(void)
107 {
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  uint8_t 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 
125 {
126  static uint8_t _1Hz = 0;
127  uint8_t nb = 0, i;
128  float hspeed_dir = stateGetHorizontalSpeedDir_f();
129  float ch = cosf(hspeed_dir);
130  float sh = sinf(hspeed_dir);
131  form_n = 0.;
132  form_e = 0.;
133  form_a = 0.;
135  form_speed_n = form_speed * ch;
136  form_speed_e = form_speed * sh;
137 
138  if (AC_ID == leader_id) {
141  }
142 
143  // broadcast info
144  uint8_t ac_id = AC_ID;
146  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status);
147  if (++_1Hz >= 4) {
148  _1Hz = 0;
149  DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &ac_id, &form_mode,
150  &formation[ti_acs_id[AC_ID]].east,
151  &formation[ti_acs_id[AC_ID]].north,
152  &formation[ti_acs_id[AC_ID]].alt);
153  }
154  if (formation[ti_acs_id[AC_ID]].status != ACTIVE) { return false; } // AC not ready
155 
156  // get leader info
157  struct EnuCoor_f *leader_pos = acInfoGetPositionEnu_f(leader_id);
160  // leader not ready or not in formation
161  return false;
162  }
163 
164  // compute slots in the right reference frame
165  struct slot_ form[NB_ACS];
166  float cr = 0., sr = 1.;
167  if (form_mode == FORM_MODE_COURSE) {
168  cr = cosf(acInfoGetCourse(leader_id));
169  sr = sinf(acInfoGetCourse(leader_id));
170  }
171  for (i = 0; i < NB_ACS; ++i) {
172  if (formation[i].status == UNSET) { continue; }
173  form[i].east = formation[i].east * sr - formation[i].north * cr;
174  form[i].north = formation[i].east * cr + formation[i].north * sr;
175  form[i].alt = formation[i].alt;
176  }
177 
178  struct EnuCoor_f *my_pos = stateGetPositionEnu_f();
179  // compute control forces
180  for (i = 0; i < NB_ACS; ++i) {
181  if (ti_acs[i].ac_id == AC_ID) { continue; }
182  struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ti_acs[i].ac_id);
183  struct EnuCoor_f *ac_speed = acInfoGetVelocityEnu_f(ti_acs[i].ac_id);
184 
185  float delta_t = Max((int)(gps.tow - acInfoGetItow(ti_acs[i].ac_id)) / 1000., 0.);
186  if (delta_t > FORM_CARROT) {
187  // if AC not responding for too long
188  formation[i].status = LOST;
189  continue;
190  } else {
191  // compute control if AC is ACTIVE and around the same altitude (maybe not so useful)
192  formation[i].status = ACTIVE;
193  if (ac->z > 0 && fabs(my_pos->z - ac->z) < form_prox) {
194  form_e += (ac->x + ac_speed->x * delta_t - my_pos->x) - (form[i].east - form[ti_acs_id[AC_ID]].east);
195  form_n += (ac->y + ac_speed->y * delta_t - my_pos->y) - (form[i].north - form[ti_acs_id[AC_ID]].north);
196  form_a += (ac->z + ac_speed->z * delta_t - my_pos->z) - (form[i].alt - form[ti_acs_id[AC_ID]].alt);
197  form_speed += acInfoGetGspeed(ti_acs[i].ac_id);
198  //form_speed_e += ac->gspeed * sinf(ac->course);
199  //form_speed_n += ac->gspeed * cosf(ac->course);
200  ++nb;
201  }
202  }
203  }
204  uint8_t _nb = Max(1, nb);
205  form_n /= _nb;
206  form_e /= _nb;
207  form_a /= _nb;
209  //form_speed_e = form_speed_e / (nb+1) - stateGetHorizontalSpeedNorm_f() * sh;
210  //form_speed_n = form_speed_n / (nb+1) - stateGetHorizontalSpeedNorm_f() * ch;
211 
212  // set commands
214 
215  // altitude loop
216  float alt = 0.;
217  if (AC_ID == leader_id) {
218  alt = nav_altitude;
219  } else {
220  alt = leader_pos->z - form[ti_acs_id[leader_id]].alt;
221  }
222  alt += formation[ti_acs_id[AC_ID]].alt + coef_form_alt * form_a;
223  flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT);
224 
225  // carrot
226  if (AC_ID != leader_id) {
227  float dx = form[ti_acs_id[AC_ID]].east - form[ti_acs_id[leader_id]].east;
228  float dy = form[ti_acs_id[AC_ID]].north - form[ti_acs_id[leader_id]].north;
229  desired_x = leader_pos->x + NOMINAL_AIRSPEED * form_carrot * sinf(acInfoGetCourse(leader_id)) + dx;
230  desired_y = leader_pos->y + NOMINAL_AIRSPEED * form_carrot * cosf(acInfoGetCourse(leader_id)) + dy;
231  // fly to desired
233  desired_x = leader_pos->x + dx;
234  desired_y = leader_pos->y + dy;
235  // lateral correction
236  //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
237  //float diff_course = leader->course - hspeed_dir;
238  //NormRadAngle(diff_course);
239  //h_ctl_roll_setpoint += coef_form_course * diff_course;
240  //h_ctl_roll_setpoint += coef_form_course * diff_heading;
241  }
242  //BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
243 
244  // speed loop
245  if (nb > 0) {
246  float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
247  cruise += coef_form_pos * (form_n * ch + form_e * sh) + coef_form_speed * form_speed;
248  Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
250  }
251  return true;
252 }
253 
255 {
256  if (leader_id == AC_ID) {
259  }
260 }
261 
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
Vertical control for fixed wing vehicles.
float form_e
Definition: formation.c:19
#define FORM_MODE
Definition: formation.c:59
#define FORM_SPEED_PGAIN
Definition: formation.c:43
int formation_flight(void)
Definition: formation.c:124
#define FORM_COURSE_PGAIN
Definition: formation.c:47
float form_n
Definition: formation.c:19
float form_speed_n
Definition: formation.c:20
float coef_form_pos
Definition: formation.c:24
float coef_form_course
Definition: formation.c:26
float form_speed
Definition: formation.c:20
uint8_t leader_id
Definition: formation.c:29
int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a)
Definition: formation.c:80
#define FORM_ALTITUDE_PGAIN
Definition: formation.c:51
int start_formation(void)
Definition: formation.c:91
struct slot_ formation[NB_ACS]
Definition: formation.c:32
uint8_t form_mode
Definition: formation.c:28
#define FORM_POS_PGAIN
Definition: formation.c:39
float old_cruise
Definition: formation.c:30
float coef_form_speed
Definition: formation.c:25
float form_prox
Definition: formation.c:23
float coef_form_alt
Definition: formation.c:27
#define FORM_CARROT
Definition: formation.c:35
int formation_init(void)
Definition: formation.c:62
#define FORM_PROX
Definition: formation.c:55
float form_speed_e
Definition: formation.c:20
void formation_pre_call(void)
Definition: formation.c:254
float form_carrot
Definition: formation.c:22
int stop_formation(void)
Definition: formation.c:106
float old_alt
Definition: formation.c:30
float form_a
Definition: formation.c:19
Formation flight library.
enum slot_status status
Definition: formation.h:25
@ LOST
Definition: formation.h:22
@ ACTIVE
Definition: formation.h:22
@ UNSET
Definition: formation.h:22
@ IDLE
Definition: formation.h:22
float alt
Definition: formation.h:28
float east
Definition: formation.h:26
float north
Definition: formation.h:27
#define FORM_MODE_COURSE
Definition: formation.h:15
struct GpsState gps
global GPS state
Definition: gps.c:69
uint32_t tow
GPS time of week in ms.
Definition: gps.h:107
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
static float acInfoGetCourse(uint8_t ac_id)
Get vehicle course (float).
Definition: traffic_info.h:416
static float acInfoGetGspeed(uint8_t ac_id)
Get vehicle ground speed (float).
Definition: traffic_info.h:424
static uint32_t acInfoGetItow(uint8_t ac_id)
Get time of week from latest message (ms).
Definition: traffic_info.h:440
struct acInfo ti_acs[NB_ACS]
Definition: traffic_info.c:45
uint8_t ti_acs_id[NB_ACS_ID]
Definition: traffic_info.c:43
static struct EnuCoor_f * acInfoGetVelocityEnu_f(uint8_t ac_id)
Get position from ENU coordinates (float).
Definition: traffic_info.h:405
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition: nav.c:74
float nav_altitude
Definition: nav.c:307
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
Fixedwing Navigation library.
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:177
uint8_t status
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
#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.
Definition: vl53l1_types.h:98