Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
dw1000_arduino.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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  */
27 
28 #include "std.h"
29 #include "mcu_periph/uart.h"
30 #include "pprzlink/messages.h"
32 #include "subsystems/abi.h"
34 #include "subsystems/gps.h"
35 #include "state.h"
36 #include "generated/flight_plan.h"
37 #include "generated/airframe.h"
38 #include <stdio.h>
39 #include <stdlib.h>
40 #include <string.h>
41 
43 #ifndef DW1000_USE_AS_LOCAL_POS
44 #define DW1000_USE_AS_LOCAL_POS TRUE
45 #endif
46 
48 #ifndef DW1000_USE_AS_GPS
49 #define DW1000_USE_AS_GPS FALSE
50 #endif
51 
53 #ifndef DW1000_USE_EKF
54 #define DW1000_USE_EKF TRUE
55 #endif
56 
63 #ifndef DW1000_NB_ANCHORS
64 #define DW1000_NB_ANCHORS 3
65 #endif
66 
68 #ifndef DW1000_OFFSET
69 #define DW1000_OFFSET { 0.f, 0.f, 0.f }
70 #endif
71 
73 #ifndef DW1000_SCALE
74 #define DW1000_SCALE { 1.f, 1.f, 1.f }
75 #endif
76 
78 #ifndef DW1000_INITIAL_HEADING
79 #define DW1000_INITIAL_HEADING 0.f
80 #endif
81 
83 #ifndef DW1000_TIMEOUT
84 #define DW1000_TIMEOUT 500
85 #endif
86 
88 #ifndef DW1000_NOISE_X
89 #define DW1000_NOISE_X 0.1f
90 #endif
91 
92 #ifndef DW1000_NOISE_Y
93 #define DW1000_NOISE_Y 0.1f
94 #endif
95 
96 #ifndef DW1000_NOISE_Z
97 #define DW1000_NOISE_Z 0.1f
98 #endif
99 
100 #ifndef DW1000_VEL_NOISE_X
101 #define DW1000_VEL_NOISE_X 0.1f
102 #endif
103 
104 #ifndef DW1000_VEL_NOISE_Y
105 #define DW1000_VEL_NOISE_Y 0.1f
106 #endif
107 
108 #ifndef DW1000_VEL_NOISE_Z
109 #define DW1000_VEL_NOISE_Z 0.1f
110 #endif
111 
112 #if DW1000_USE_EKF
114 #include "filters/median_filter.h"
115 
116 #define DW1000_EKF_UNINIT 0
117 #define DW1000_EKF_POS_INIT 1
118 #define DW1000_EKF_RUNNING 2
119 
120 #ifndef DW1000_EKF_P0_POS
121 #define DW1000_EKF_P0_POS 1.0f
122 #endif
123 
124 #ifndef DW1000_EKF_P0_SPEED
125 #define DW1000_EKF_P0_SPEED 1.0f
126 #endif
127 
128 #ifndef DW1000_EKF_Q
129 #define DW1000_EKF_Q 4.0f
130 #endif
131 
132 #ifndef DW1000_EKF_R_DIST
133 #define DW1000_EKF_R_DIST 0.1f
134 #endif
135 
136 #ifndef DW1000_EKF_R_SPEED
137 #define DW1000_EKF_R_SPEED 0.1f
138 #endif
139 
140 #endif // USE_EKF
141 
144 #if SITL
145 #ifndef DW1000_ANCHOR_SIM_WP
146 #define DW1000_ANCHOR_SIM_WP { WP_ANCHOR_1, WP_ANCHOR_2, WP_ANCHOR_3 }
147 #endif
148 #endif
149 
151 #if DW1000_LOG
153 static bool log_started;
154 #endif
155 
157 #define DW_STX 0xFE
158 
160 #define DW_WAIT_STX 0
161 #define DW_GET_DATA 1
162 #define DW_GET_CK 2
163 #define DW_NB_DATA 6
164 
166 struct DW1000 {
174  struct EnuCoor_f pos;
175  struct EnuCoor_f speed;
177  struct LtpDef_i ltp_def;
178  bool updated;
179  bool ekf_running;
182 #if SITL
183  uint8_t anchor_sim_wp[DW1000_NB_ANCHORS];
184 #endif
185 };
186 
187 static struct DW1000 dw1000;
188 
193 
195 static const uint16_t ids[] = DW1000_ANCHORS_IDS;
196 static const float pos_x[] = DW1000_ANCHORS_POS_X;
197 static const float pos_y[] = DW1000_ANCHORS_POS_Y;
198 static const float pos_z[] = DW1000_ANCHORS_POS_Z;
199 static const float offset[] = DW1000_OFFSET;
200 static const float scale[] = DW1000_SCALE;
201 
202 #if !SITL
203 
204 static inline float float_from_buf(uint8_t* b) {
205  float f;
206  memcpy((uint8_t*)(&f), b, sizeof(float));
207  return f;
208 }
209 
212  uint16_t u16;
213  memcpy ((uint8_t*)(&u16), b, sizeof(uint16_t));
214  return u16;
215 }
216 
218 static void fill_anchor(struct DW1000 *dw) {
219  for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
220  uint16_t id = uint16_from_buf(dw->buf);
221  if (dw->anchors[i].id == id) {
222  dw->raw_dist[i] = float_from_buf(dw->buf + 2);
223  // median filter for EKF
224  const float dist = scale[i] * (dw->raw_dist[i] - offset[i]);
225  // store scaled distance
226  dw->anchors[i].distance = update_median_filter_f(&dw->mf[i], dist);
227  dw->anchors[i].time = get_sys_time_float();
228  dw->anchors[i].updated = true;
229  dw->updated = true; // at least one of the anchor is updated
230  break;
231  }
232  }
233 }
234 
236 static void dw1000_arduino_parse(struct DW1000 *dw, uint8_t c)
237 {
238  switch (dw->state) {
239 
240  case DW_WAIT_STX:
241  /* Waiting Synchro */
242  if (c == DW_STX) {
243  dw->idx = 0;
244  dw->ck = 0;
245  dw->state = DW_GET_DATA;
246  }
247  break;
248 
249  case DW_GET_DATA:
250  /* Read Bytes */
251  dw->buf[dw->idx++] = c;
252  dw->ck += c;
253  if (dw->idx == DW_NB_DATA) {
254  dw->state = DW_GET_CK;
255  }
256  break;
257 
258  case DW_GET_CK:
259  /* Checksum */
260  if (dw->ck == c) {
261  fill_anchor(dw);
262  }
263  dw->state = DW_WAIT_STX;
264  break;
265 
266  default:
267  dw->state = DW_WAIT_STX;
268  }
269 }
270 #endif // !SITL
271 
272 #if DW1000_USE_AS_GPS
273 static void send_gps_dw1000_small(struct DW1000 *dw)
274 {
275  // rotate and convert to cm integer
276  float x = dw->pos.x * cosf(dw->initial_heading) - dw->pos.y * sinf(dw->initial_heading);
277  float y = dw->pos.x * sinf(dw->initial_heading) + dw->pos.y * cosf(dw->initial_heading);
278  struct EnuCoor_i enu_pos;
279  enu_pos.x = (int32_t) (x * 100.f);
280  enu_pos.y = (int32_t) (y * 100.f);
281  enu_pos.z = (int32_t) (dw->pos.z * 100.f);
282 
283  // Convert the ENU coordinates to ECEF
284  ecef_of_enu_point_i(&(dw->gps_dw1000.ecef_pos), &(dw->ltp_def), &enu_pos);
286 
289 
290  // Convert ENU speed to ECEF
291  struct EnuCoor_i enu_speed;
292  enu_speed.x = (int32_t) (dw->speed.x * 100.f);
293  enu_speed.y = (int32_t) (dw->speed.y * 100.f);
294  enu_speed.z = (int32_t) (dw->speed.z * 100.f);
295 
296  VECT3_NED_OF_ENU(dw->gps_dw1000.ned_vel, enu_speed);
298 
299  ecef_of_enu_vect_i(&dw->gps_dw1000.ecef_vel , &(dw->ltp_def) , &enu_speed);
301 
302  dw->gps_dw1000.gspeed = (int16_t)FLOAT_VECT2_NORM(enu_speed);
303  dw->gps_dw1000.speed_3d = (int16_t)FLOAT_VECT3_NORM(enu_speed);
304 
305  // HMSL
306  dw->gps_dw1000.hmsl = dw->ltp_def.hmsl + enu_pos.z * 10;
308 
309 #if defined(SECONDARY_GPS) && AHRS_USE_GPS_HEADING
310  // a second GPS is used to get heading
311  // ugly hack: it is a datalink GPS
313 #endif
314 
315  dw->gps_dw1000.num_sv = 7;
317  dw->gps_dw1000.fix = GPS_FIX_3D; // set 3D fix to true
318 
319  // set gps msg time
324 
325  // publish new GPS data
326  uint32_t now_ts = get_sys_time_usec();
327  AbiSendMsgGPS(GPS_DW1000_ID, now_ts, &(dw->gps_dw1000));
328 }
329 #endif
330 
331 #if DW1000_USE_AS_LOCAL_POS
332 static void send_pos_estimate(struct DW1000 *dw)
333 {
334  uint32_t now_ts = get_sys_time_usec();
335  // send POSITION_ESTIMATE type message
336  AbiSendMsgPOSITION_ESTIMATE(GPS_DW1000_ID, now_ts,
337  dw->pos.x, dw->pos.y, dw->pos.z,
339  // send VELOCITY_ESTIMATE type message if EKF is running
340  if (dw1000_use_ekf && dw->ekf_running) {
341  AbiSendMsgVELOCITY_ESTIMATE(GPS_DW1000_ID, now_ts,
342  dw->speed.x, dw->speed.y, dw->speed.z,
344  }
345 }
346 #endif
347 
353 static bool check_anchor_timeout(struct DW1000 *dw, float timeout)
354 {
355  const float now = get_sys_time_float();
356  for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
357  if (now - dw->anchors[i].time > timeout) {
358  return true;
359  }
360  }
361  return false;
362 }
363 
367 static inline bool check_and_compute_data(struct DW1000 *dw)
368 {
369  const float timeout = (float)DW1000_TIMEOUT / 1000.;
370  if (dw1000_use_ekf) {
371  if (dw->ekf_running) {
372  // filter is running
373  if (check_anchor_timeout(dw, 5.f*timeout)) {
374  // no more valid data for a long time
375  dw->ekf_running = false;
376  return false;
377  } else {
378  // run filter on each updated anchor
379  for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
380  if (dw->anchors[i].updated) {
382  dw->anchors[i].pos);
383  dw->anchors[i].updated = false;
384  }
385  }
386  dw->pos = ekf_range_get_pos(&dw->ekf_range);
387  dw->speed = ekf_range_get_speed(&dw->ekf_range);
388  return true;
389  }
390  } else {
391  // filter is currently not running,
392  // waiting for a new valid initial position
393  if (check_anchor_timeout(dw, timeout)) {
394  // no valid data
395  return false;
396  } else {
397  if (trilateration_compute(dw->anchors, &(dw->pos)) == 0) {
398  // got valid initial pos
399  struct EnuCoor_f speed = { 0.f, 0.f, 0.f };
400  ekf_range_set_state(&dw->ekf_range, dw->pos, speed);
401  dw->ekf_running = true;
402  return true;
403  } else {
404  // trilateration failed
405  return false;
406  }
407  }
408  }
409  } else {
410  // Direct trilateration only
411  // if no timeout on anchors, run trilateration algorithm
412  return (check_anchor_timeout(dw, timeout) == false &&
413  trilateration_compute(dw->anchors, &(dw->pos)) == 0);
414  }
415 }
416 
417 static void process_data(struct DW1000 *dw) {
418  // process if new data
419  if (dw->updated) {
420  // send result if process returns true
421  if (check_and_compute_data(dw)) {
422 #if DW1000_USE_AS_GPS
423  // send fake GPS message for INS filters
424  send_gps_dw1000_small(dw);
425 #endif
426 #if DW1000_USE_AS_LOCAL_POS
427  // send a local postion estimate
428  send_pos_estimate(dw);
429 #endif
430  }
431 #if DW1000_LOG
432  if (log_started) {
433  struct EnuCoor_f pos = *stateGetPositionEnu_f();
434  struct EnuCoor_f speed = *stateGetSpeedEnu_f();
435  struct FloatRates *rates = stateGetBodyRates_f();
436  struct FloatRMat *ned_to_body = stateGetNedToBodyRMat_f();
437  float omega_z = -rates->p * MAT33_ELMT(*ned_to_body, 2, 0)
438  + rates->q * MAT33_ELMT(*ned_to_body, 2, 1)
439  + rates->r * MAT33_ELMT(*ned_to_body, 2, 2);
440  sdLogWriteLog(pprzLogFile, "%.3f %.3f %.3f %3.f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
442  dw1000.anchors[0].time,
444  dw1000.anchors[1].time,
446  dw1000.anchors[2].time,
447  dw1000.pos.x,
448  dw1000.pos.y,
449  dw1000.pos.z,
450  pos.x,
451  pos.y,
452  pos.z,
453  speed.x,
454  speed.y,
455  speed.z,
456  omega_z);
457  }
458 #endif
459  dw->updated = false;
460  }
461 }
462 
464 {
465  // store current heading as ref and stop periodic call
467  dw1000_arduino_dw1000_reset_heading_ref_status = MODULES_STOP;
468 }
469 
470 #if SITL
471 static const uint8_t wp_ids[] = DW1000_ANCHOR_SIM_WP;
472 
473 #define DW1000_SITL_SYNC FALSE
474 
475 static void compute_anchors_dist_from_wp(struct DW1000 *dw)
476 {
477 #if !DW1000_SITL_SYNC
478  static int i = 0; // async data update (more realistic)
479 #endif
480  // position of the aircraft
481  struct FloatVect3 *pos = (struct FloatVect3 *) (stateGetPositionEnu_f());
482  float time = get_sys_time_float();
483  // compute distance to each WP/anchor
484 #if DW1000_SITL_SYNC
485  for (uint8_t i = 0; i < DW1000_NB_ANCHORS; i++) {
486 #endif
487  struct FloatVect3 a_pos = {
488  WaypointX(wp_ids[i]),
489  WaypointY(wp_ids[i]),
490  WaypointAlt(wp_ids[i])
491  };
492  struct FloatVect3 diff;
493  VECT3_DIFF(diff, (*pos), a_pos);
494  dw->anchors[i].distance = float_vect3_norm(&diff);
495  dw->anchors[i].time = time;
496  dw->anchors[i].updated = true;
497  dw->updated = true;
498 
499  i = (i+1)%DW1000_NB_ANCHORS;
500 #if DW1000_SITL_SYNC
501  }
502 #endif
503  struct EnuCoor_f t_pos;
504  // direct trilat for debug
505  if (trilateration_compute(dw->anchors, &t_pos) == 0) {
506  dw->raw_dist[0] = t_pos.x;
507  dw->raw_dist[1] = t_pos.y;
508  dw->raw_dist[2] = t_pos.z;
509  }
510 }
511 #endif // SITL
512 
514 {
515  // init DW1000 structure
516  dw1000.idx = 0;
517  dw1000.ck = 0;
520  dw1000.pos.x = 0.f;
521  dw1000.pos.y = 0.f;
522  dw1000.pos.z = 0.f;
523  dw1000.speed.x = 0.f;
524  dw1000.speed.y = 0.f;
525  dw1000.speed.z = 0.f;
526  dw1000.updated = false;
527  for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
528  dw1000.raw_dist[i] = 0.f;
529  dw1000.anchors[i].distance = 0.f;
530  dw1000.anchors[i].time = 0.f;
531  dw1000.anchors[i].id = ids[i];
532  dw1000.anchors[i].updated = false;
533  dw1000.anchors[i].pos.x = pos_x[i];
534  dw1000.anchors[i].pos.y = pos_y[i];
535  dw1000.anchors[i].pos.z = pos_z[i];
536  }
537 
538  // gps structure init
540  dw1000.gps_dw1000.pdop = 0;
541  dw1000.gps_dw1000.sacc = 0;
542  dw1000.gps_dw1000.pacc = 0;
543  dw1000.gps_dw1000.cacc = 0;
545 
546  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
547  llh_nav0.lat = NAV_LAT0;
548  llh_nav0.lon = NAV_LON0;
549  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
550  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
551  ltp_def_from_lla_i(&dw1000.ltp_def, &llh_nav0);
552 
553  // init trilateration algorithm
555 
557  dw1000_ekf_q = DW1000_EKF_Q;
558  dw1000_ekf_r_dist = DW1000_EKF_R_DIST;
559  dw1000_ekf_r_speed = DW1000_EKF_R_SPEED;
560  dw1000.ekf_running = false;
561  ekf_range_init(&dw1000.ekf_range, DW1000_EKF_P0_POS, DW1000_EKF_P0_SPEED,
562  DW1000_EKF_Q, DW1000_EKF_R_DIST, DW1000_EKF_R_SPEED, 0.1f);
563  for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
565  }
566 }
567 
569 {
570  if (dw1000_use_ekf) {
571  if (dw1000.ekf_running) {
575  }
576  } else {
577  dw1000.ekf_running = false; // init sequence will be required at next run
578  }
579 #if SITL
580  // compute position from WP for simulation
581  compute_anchors_dist_from_wp(&dw1000);
583 #endif
584 #if DW1000_USE_AS_GPS
585  // Check for GPS timeout
587 #endif
588 #if DW1000_LOG
589  if (pprzLogFile != -1) {
590  if (!log_started) {
591  sdLogWriteLog(pprzLogFile,
592  "d1 t1 d2 t2 d3 t3 x y z gps_x gps_y gps_z vx vy vz omega\n");
593  log_started = true;
594  }
595  }
596 #endif
597 }
598 
600 {
601  float buf[12];
602  buf[0] = dw1000.anchors[0].distance;
603  buf[1] = dw1000.anchors[1].distance;
604  buf[2] = dw1000.anchors[2].distance;
605  buf[3] = dw1000.raw_dist[0];
606  buf[4] = dw1000.raw_dist[1];
607  buf[5] = dw1000.raw_dist[2];
608  buf[6] = dw1000.pos.x;
609  buf[7] = dw1000.pos.y;
610  buf[8] = dw1000.pos.z;
611  buf[9] = dw1000.speed.x;
612  buf[10] = dw1000.speed.y;
613  buf[11] = dw1000.speed.z;
614  DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 12, buf);
615 }
616 
618 {
619 #if !SITL
620  // Look for data on serial link and send to parser
621  while (uart_char_available(&DW1000_ARDUINO_DEV)) {
622  uint8_t ch = uart_getch(&DW1000_ARDUINO_DEV);
625  }
626 #endif
627 }
628 
629 
631 {
632  dw1000_ekf_q = v;
634 }
635 
637 {
638  dw1000_ekf_r_dist = v;
640 }
641 
643 {
644  dw1000_ekf_r_speed = v;
646 }
647 
c
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
uint16_t
unsigned short uint16_t
Definition: types.h:16
GpsState::valid_fields
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
dw1000_arduino_periodic
void dw1000_arduino_periodic(void)
Definition: dw1000_arduino.c:568
pprzLogFile
FileDes pprzLogFile
Definition: sdlog_chibios.c:86
VECT3_NED_OF_ENU
#define VECT3_NED_OF_ENU(_o, _i)
Definition: pprz_geodetic_int.h:150
DW1000::ck
uint8_t ck
checksum
Definition: dw1000_arduino.c:169
DW_STX
#define DW_STX
waypoints to use as anchors in simulation
Definition: dw1000_arduino.c:157
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
check_anchor_timeout
static bool check_anchor_timeout(struct DW1000 *dw, float timeout)
check timeout for each anchor
Definition: dw1000_arduino.c:353
EKFRange
EKF_range structure.
Definition: ekf_range.h:43
trilateration.h
uart_getch
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:968
b
float b
Definition: wedgebug.c:202
abi.h
EnuCoor_i::y
int32_t y
North.
Definition: pprz_geodetic_int.h:79
lla_of_ecef_i
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
Definition: pprz_geodetic_int.c:363
scale
static const float scale[]
Definition: dw1000_arduino.c:200
EnuCoor_i::x
int32_t x
East.
Definition: pprz_geodetic_int.h:78
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
GPS_DW1000_ID
#define GPS_DW1000_ID
Definition: abi_sender_ids.h:268
DW1000_VEL_NOISE_Z
#define DW1000_VEL_NOISE_Z
Definition: dw1000_arduino.c:109
LtpDef_i
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_int.h:98
get_sys_time_float
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
WaypointY
#define WaypointY(_wp)
Definition: common_nav.h:46
DW1000_NOISE_Y
#define DW1000_NOISE_Y
Definition: dw1000_arduino.c:93
ids
static const uint16_t ids[]
init arrays from airframe file
Definition: dw1000_arduino.c:195
GpsState
data structure for GPS information
Definition: gps.h:87
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
dw1000_arduino_update_ekf_q
void dw1000_arduino_update_ekf_q(float v)
settings handler
Definition: dw1000_arduino.c:630
DW_GET_CK
#define DW_GET_CK
Definition: dw1000_arduino.c:162
DW1000::mf
struct MedianFilterFloat mf[DW1000_NB_ANCHORS]
median filter for EKF input data
Definition: dw1000_arduino.c:181
GpsState::pacc
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
WaypointX
#define WaypointX(_wp)
Definition: common_nav.h:45
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
uart_char_available
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
dw1000_arduino_parse
static void dw1000_arduino_parse(struct DW1000 *dw, uint8_t c)
Data parsing function.
Definition: dw1000_arduino.c:236
dw1000
static struct DW1000 dw1000
Definition: dw1000_arduino.c:187
Anchor::updated
bool updated
new data available
Definition: trilateration.h:39
GpsState::sacc
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
uint32_t
unsigned long uint32_t
Definition: types.h:18
VECT3_DIFF
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
dw1000_reset_heading_ref
void dw1000_reset_heading_ref(void)
Reset reference heading to current heading AHRS/INS should be aligned before calling this function.
Definition: dw1000_arduino.c:463
ecef_of_enu_vect_i
void ecef_of_enu_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Rotate a vector from ENU to ECEF.
Definition: pprz_geodetic_int.c:199
DW_NB_DATA
#define DW_NB_DATA
Definition: dw1000_arduino.c:163
FLOAT_VECT3_NORM
#define FLOAT_VECT3_NORM(_v)
Definition: pprz_algebra_float.h:164
update_median_filter_f
static float update_median_filter_f(struct MedianFilterFloat *filter, float new_data)
Definition: median_filter.h:175
ekf_range_get_speed
struct EnuCoor_f ekf_range_get_speed(struct EKFRange *ekf_range)
Get current speed.
Definition: ekf_range.c:84
MAT33_ELMT
#define MAT33_ELMT(_m, _row, _col)
Definition: pprz_algebra.h:436
ekf_range_get_pos
struct EnuCoor_f ekf_range_get_pos(struct EKFRange *ekf_range)
Get current pos.
Definition: ekf_range.c:75
timeout
static float timeout
Definition: object_tracking.c:70
DW1000::ltp_def
struct LtpDef_i ltp_def
ltp reference
Definition: dw1000_arduino.c:177
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
DW_GET_DATA
#define DW_GET_DATA
Definition: dw1000_arduino.c:161
GpsState::ned_vel
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
DW1000::ekf_range
struct EKFRange ekf_range
EKF filter.
Definition: dw1000_arduino.c:180
u16
uint16_t u16
Unsigned 16-bit integer.
Definition: common.h:43
dw1000_arduino_report
void dw1000_arduino_report(void)
Definition: dw1000_arduino.c:599
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
DW1000_SCALE
#define DW1000_SCALE
default scale factor, applied to individual distances
Definition: dw1000_arduino.c:74
DW1000::idx
uint8_t idx
buffer index
Definition: dw1000_arduino.c:168
GPS_VALID_POS_LLA_BIT
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:49
sdlog_chibios.h
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
FloatVect3
Definition: pprz_algebra_float.h:54
float_from_buf
static float float_from_buf(uint8_t *b)
Utility function to get float from buffer.
Definition: dw1000_arduino.c:204
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
Anchor::id
uint16_t id
anchor ID
Definition: trilateration.h:38
dw1000_arduino_event
void dw1000_arduino_event(void)
Definition: dw1000_arduino.c:617
DW1000_INITIAL_HEADING
#define DW1000_INITIAL_HEADING
default initial heading correction between anchors frame and global frame
Definition: dw1000_arduino.c:79
GpsState::last_msg_ticks
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:116
DW_WAIT_STX
#define DW_WAIT_STX
Parsing states.
Definition: dw1000_arduino.c:160
DW1000::initial_heading
float initial_heading
initial heading correction
Definition: dw1000_arduino.c:171
std.h
DW1000_NOISE_Z
#define DW1000_NOISE_Z
Definition: dw1000_arduino.c:97
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
DW1000
DW1000 positionning system structure.
Definition: dw1000_arduino.c:166
float_vect3_norm
static float float_vect3_norm(struct FloatVect3 *v)
Definition: pprz_algebra_float.h:171
DW1000::speed
struct EnuCoor_f speed
local speed in anchors frame
Definition: dw1000_arduino.c:175
DW1000::pos
struct EnuCoor_f pos
local pos in anchors frame
Definition: dw1000_arduino.c:174
gps.h
Device independent GPS code (interface)
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
pos_z
static const float pos_z[]
Definition: dw1000_arduino.c:198
DW1000::raw_dist
float raw_dist[DW1000_NB_ANCHORS]
raw distance from anchors
Definition: dw1000_arduino.c:173
dw1000_ekf_q
float dw1000_ekf_q
process and measurements noise
Definition: dw1000_arduino.c:190
dw1000_arduino_init
void dw1000_arduino_init(void)
Definition: dw1000_arduino.c:513
dw1000_use_ekf
bool dw1000_use_ekf
enable EKF filtering
Definition: dw1000_arduino.c:189
dw1000_arduino_update_ekf_r_dist
void dw1000_arduino_update_ekf_r_dist(float v)
Definition: dw1000_arduino.c:636
GpsState::speed_3d
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:98
ekf_range_update_noise
void ekf_range_update_noise(struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed)
Update process and measurement noises.
Definition: ekf_range.c:93
DW1000_VEL_NOISE_X
#define DW1000_VEL_NOISE_X
Definition: dw1000_arduino.c:101
DW1000::state
uint8_t state
parser state
Definition: dw1000_arduino.c:170
FLOAT_VECT2_NORM
#define FLOAT_VECT2_NORM(_v)
Definition: pprz_algebra_float.h:132
log_started
bool log_started
Definition: meteo_france_DAQ.c:47
DW1000_VEL_NOISE_Y
#define DW1000_VEL_NOISE_Y
Definition: dw1000_arduino.c:105
trilateration_init
int trilateration_init(struct Anchor *anchors)
Init internal trilateration structures.
Definition: trilateration.c:39
DW1000::gps_dw1000
struct GpsState gps_dw1000
"fake" gps structure
Definition: dw1000_arduino.c:176
int16_t
signed short int16_t
Definition: types.h:17
get_sys_time_msec
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
uint8_t
unsigned char uint8_t
Definition: types.h:14
DW1000::buf
uint8_t buf[DW_NB_DATA]
incoming data buffer
Definition: dw1000_arduino.c:167
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
DW1000_USE_EKF
#define DW1000_USE_EKF
TRUE if EKF range filter is use.
Definition: dw1000_arduino.c:54
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
GpsState::last_3dfix_time
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
gps_periodic_check
void gps_periodic_check(struct GpsState *gps_s)
Periodic GPS check.
Definition: gps.c:264
GpsState::last_3dfix_ticks
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:114
uint16_from_buf
static uint16_t uint16_from_buf(uint8_t *b)
Utility function to get uint16_t from buffer.
Definition: dw1000_arduino.c:211
GpsState::comp_id
uint8_t comp_id
id of current gps
Definition: gps.h:89
ltp_def_from_lla_i
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
Definition: pprz_geodetic_int.c:72
median_filter.h
dw1000_arduino.h
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
process_data
static void process_data(struct DW1000 *dw)
Definition: dw1000_arduino.c:417
stateGetNedToBodyRMat_f
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
Anchor::pos
struct EnuCoor_f pos
position of the anchor
Definition: trilateration.h:37
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
ekf_range_set_state
void ekf_range_set_state(struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed)
Set initial state vector.
Definition: ekf_range.c:55
DW1000_NB_ANCHORS
#define DW1000_NB_ANCHORS
Number of anchors.
Definition: dw1000_arduino.c:64
MedianFilterFloat
Definition: median_filter.h:142
DW1000_TIMEOUT
#define DW1000_TIMEOUT
default timeout (in ms)
Definition: dw1000_arduino.c:84
DW1000_OFFSET
#define DW1000_OFFSET
default offset, applied to individual distances
Definition: dw1000_arduino.c:69
GpsState::last_msg_time
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
DW1000_NOISE_X
#define DW1000_NOISE_X
DW1000 Noise.
Definition: dw1000_arduino.c:89
dw1000_arduino_update_ekf_r_speed
void dw1000_arduino_update_ekf_r_speed(float v)
Definition: dw1000_arduino.c:642
WaypointAlt
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
offset
static const float offset[]
Definition: dw1000_arduino.c:199
ekf_range_update_dist
void ekf_range_update_dist(struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
correction step
Definition: ekf_range.c:143
pos_x
static const float pos_x[]
Definition: dw1000_arduino.c:196
pos_y
static const float pos_y[]
Definition: dw1000_arduino.c:197
DW1000::ekf_running
bool ekf_running
EKF logic status.
Definition: dw1000_arduino.c:179
GPS_VALID_VEL_NED_BIT
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
dw1000_ekf_r_dist
float dw1000_ekf_r_dist
Definition: dw1000_arduino.c:191
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
ekf_range.h
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
int32_t
signed long int32_t
Definition: types.h:19
stateGetSpeedEnu_f
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
GPS_VALID_VEL_ECEF_BIT
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:51
sys_time
Definition: sys_time.h:71
GpsState::num_sv
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
sys_time::nb_sec_rem
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
dw1000_ekf_r_speed
float dw1000_ekf_r_speed
Definition: dw1000_arduino.c:192
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
trilateration_compute
int trilateration_compute(struct Anchor *anchors, struct EnuCoor_f *pos)
Compute trilateration based on the latest measurments.
Definition: trilateration.c:87
GpsState::pdop
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:105
ecef_of_enu_point_i
void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Convert a point in local ENU to ECEF.
Definition: pprz_geodetic_int.c:238
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
Anchor::time
float time
time of the last received data
Definition: trilateration.h:36
init_median_filter_f
static void init_median_filter_f(struct MedianFilterFloat *filter, uint8_t size)
Definition: median_filter.h:148
DW1000::anchors
struct Anchor anchors[DW1000_NB_ANCHORS]
anchors data
Definition: dw1000_arduino.c:172
GPS_VALID_POS_ECEF_BIT
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
GpsState::cacc
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:104
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
state.h
Anchor::distance
float distance
last measured distance
Definition: trilateration.h:35
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
sys_time::nb_sec
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
GpsState::ecef_vel
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
fill_anchor
static void fill_anchor(struct DW1000 *dw)
Utility function to fill anchor from buffer.
Definition: dw1000_arduino.c:218
GPS_FIX_NONE
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
ekf_range_init
void ekf_range_init(struct EKFRange *ekf_range, float P0_pos, float P0_speed, float Q_sigma2, float R_dist, float R_speed, float dt)
Init EKF_range internal struct.
Definition: ekf_range.c:29
ekf_range_predict
void ekf_range_predict(struct EKFRange *ekf_range)
propagate dynamic model
Definition: ekf_range.c:119
EnuCoor_i
vector in East North Up coordinates
Definition: pprz_geodetic_int.h:77
GPS_VALID_HMSL_BIT
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
Anchor
Anchor structure.
Definition: trilateration.h:34
check_and_compute_data
static bool check_and_compute_data(struct DW1000 *dw)
check new data and compute with the proper algorithm
Definition: dw1000_arduino.c:367
DW1000::updated
bool updated
new anchor data available
Definition: dw1000_arduino.c:178