Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
dcf.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Hector Garcia de Marina
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
22 #include <math.h>
23 #include <std.h>
24 
25 #include "modules/muti/dcf/dcf.h"
26 //#include "modules/datalink/datalink.h" // dl_buffer
28 #include "modules/nav/common_nav.h"
29 #include "autopilot.h"
30 #include "std.h"
31 
32 #if PERIODIC_TELEMETRY
33 static void send_dcf(struct transport_tx *trans, struct link_device *dev)
34 {
35  pprz_msg_send_DCF(trans, dev, AC_ID, 4 * DCF_MAX_NEIGHBORS, &(dcf_tables.tableNei[0][0]), DCF_MAX_NEIGHBORS,
37 }
38 #endif // PERIODIC TELEMETRY
39 
40 // Control
42 #ifndef DCF_GAIN_K
43 #define DCF_GAIN_K 10
44 #endif
46 #ifndef DCF_RADIUS
47 #define DCF_RADIUS 80
48 #endif
50 #ifndef DCF_TIMEOUT
51 #define DCF_TIMEOUT 1500
52 #endif
54 #ifndef DCF_BROADTIME
55 #define DCF_BROADTIME 200
56 #endif
57 
59 struct dcf_tab dcf_tables;
60 
62 
63 void dcf_init(void)
64 {
65  for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) {
66  dcf_tables.tableNei[i][0] = -1;
67  dcf_tables.error_sigma[i] = 0;
68  }
69 
70 #if PERIODIC_TELEMETRY
72 #endif
73 }
74 
76 {
77  float xc = waypoints[wp].x;
78  float yc = waypoints[wp].y;
79  struct EnuCoor_f *p = stateGetPositionEnu_f();
80  float x = p->x;
81  float y = p->y;
82  float u = 0;
83 
84  dcf_control.theta = atan2f(y - yc, x - xc);
85 
87 
88  for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) {
89  if (dcf_tables.tableNei[i][0] != -1) {
91  if (timeout > dcf_control.timeout) {
93  } else {
95 
96  float t1 = dcf_control.theta;
97  float t2 = dcf_tables.tableNei[i][1] * M_PI / 1800.0;
98  float td = dcf_tables.tableNei[i][2] * M_PI / 1800.0;
99 
100  float c1 = cosf(t1);
101  float s1 = sinf(t1);
102  float c2 = cosf(t2);
103  float s2 = sinf(t2);
104 
105  float e = atan2f(c2 * s1 - s2 * c1, c1 * c2 + s1 * s2) - gvf_control.s * td;
106 
107  u += e;
108  dcf_tables.error_sigma[i] = (uint16_t)(e * 1800.0 / M_PI);
109  }
110  }
111  }
112 
113  u *= -gvf_control.s * dcf_control.k;
114 
115  gvf_ellipse_XY(xc, yc, dcf_control.radius + u, dcf_control.radius + u, 0);
116 
119  last_transmision = now;
120  }
121 
122  return true;
123 }
124 
126 {
127  struct pprzlink_msg msg;
128 
129  for (int i = 0; i < DCF_MAX_NEIGHBORS; i++)
130  if (dcf_tables.tableNei[i][0] != -1) {
131  msg.trans = &(DefaultChannel).trans_tx;
132  msg.dev = &(DefaultDevice).device;
133  msg.sender_id = AC_ID;
134  msg.receiver_id = dcf_tables.tableNei[i][0];
135  msg.component_id = 0;
136  pprzlink_msg_send_DCF_THETA(&msg, &(dcf_control.theta));
137  }
138 }
139 
141 {
142  uint8_t ac_id = DL_DCF_REG_TABLE_ac_id(buf);
143  if (ac_id == AC_ID) {
144  uint8_t nei_id = DL_DCF_REG_TABLE_nei_id(buf);
145  int16_t desired_sigma = DL_DCF_REG_TABLE_desired_sigma(buf);
146 
147  if (nei_id == 0) {
148  for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) {
149  dcf_tables.tableNei[i][0] = -1;
150  }
151  } else {
152  for (int i = 0; i < DCF_MAX_NEIGHBORS; i++)
153  if (dcf_tables.tableNei[i][0] == (int16_t)nei_id) {
154  dcf_tables.tableNei[i][0] = (int16_t)nei_id;
155  dcf_tables.tableNei[i][2] = desired_sigma;
156  return;
157  }
158 
159  for (int i = 0; i < DCF_MAX_NEIGHBORS; i++)
160  if (dcf_tables.tableNei[i][0] == -1) {
161  dcf_tables.tableNei[i][0] = (int16_t)nei_id;
162  dcf_tables.tableNei[i][2] = desired_sigma;
163  return;
164  }
165  }
166  }
167 }
168 
170 {
171  int16_t sender_id = (int16_t)(SenderIdOfPprzMsg(buf));
172  for (int i = 0; i < DCF_MAX_NEIGHBORS; i++)
173  if (dcf_tables.tableNei[i][0] == sender_id) {
175  dcf_tables.tableNei[i][1] = (int16_t)((DL_DCF_THETA_theta(buf)) * 1800 / M_PI);
176  break;
177  }
178 }
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:222
Core autopilot interface common to all firmwares.
static uint16_t c1
Definition: baro_MS5534A.c:203
static uint16_t c2
Definition: baro_MS5534A.c:203
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:98
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
void send_theta_to_nei(void)
Definition: dcf.c:125
#define DCF_BROADTIME
Definition: dcf.c:55
uint32_t last_transmision
Definition: dcf.c:61
#define DCF_RADIUS
Definition: dcf.c:47
void dcf_init(void)
Definition: dcf.c:63
static void send_dcf(struct transport_tx *trans, struct link_device *dev)
Definition: dcf.c:33
#define DCF_TIMEOUT
Definition: dcf.c:51
void parseThetaTable(uint8_t *buf)
Definition: dcf.c:169
struct dcf_tab dcf_tables
Definition: dcf.c:59
void parseRegTable(uint8_t *buf)
Definition: dcf.c:140
struct dcf_con dcf_control
Definition: dcf.c:58
#define DCF_GAIN_K
Definition: dcf.c:43
bool distributed_circular(uint8_t wp)
Definition: dcf.c:75
uint32_t last_theta[DCF_MAX_NEIGHBORS]
Definition: dcf.h:50
float theta
Definition: dcf.h:41
int16_t error_sigma[DCF_MAX_NEIGHBORS]
Definition: dcf.h:49
#define DCF_MAX_NEIGHBORS
Definition: dcf.h:34
float radius
Definition: dcf.h:39
uint16_t broadtime
Definition: dcf.h:42
uint16_t timeout
Definition: dcf.h:40
int16_t tableNei[DCF_MAX_NEIGHBORS][4]
Definition: dcf.h:48
float k
Definition: dcf.h:38
Definition: dcf.h:37
Definition: dcf.h:47
#define AP_MODE_AUTO2
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
gvf_con gvf_control
Definition: gvf.c:36
bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
Definition: gvf.c:433
int8_t s
Definition: gvf.h:56
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
static float p[2][2]
float t1
Definition: ins_ext_pose.c:345
static float timeout
float y
in meters
float x
in meters
vector in East North Up coordinates Units: meters
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98