Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 "modules/core/abi.h"
34#include "modules/gps/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
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
153static 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
186
187static struct DW1000 dw1000;
188
193
196static const float pos_x[] = DW1000_ANCHORS_POS_X;
197static const float pos_y[] = DW1000_ANCHORS_POS_Y;
198static const float pos_z[] = DW1000_ANCHORS_POS_Z;
199static const float offset[] = DW1000_OFFSET;
200static const float scale[] = DW1000_SCALE;
201
202#if !SITL
204static inline float float_from_buf(uint8_t* b) {
205 float f;
206 memcpy((uint8_t*)(&f), b, sizeof(float));
207 return f;
208}
209
213 memcpy ((uint8_t*)(&u16), b, sizeof(uint16_t));
214 return u16;
215}
216
218static 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
236static 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) {
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
273static 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);
285 SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_POS_ECEF_BIT);
286
287 lla_of_ecef_i(&(dw->gps_dw1000.lla_pos), &(dw->gps_dw1000.ecef_pos));
288 SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_POS_LLA_BIT);
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);
297 SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_VEL_NED_BIT);
298
299 ecef_of_enu_vect_i(&dw->gps_dw1000.ecef_vel , &(dw->ltp_def) , &enu_speed);
300 SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_VEL_ECEF_BIT);
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;
307 SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_HMSL_BIT);
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
312 dw->gps_dw1000.course = gps_datalink.course;
313#endif
314
315 dw->gps_dw1000.num_sv = 7;
316 dw->gps_dw1000.tow = get_sys_time_msec();
317 dw->gps_dw1000.fix = GPS_FIX_3D; // set 3D fix to true
318
319 // set gps msg time
320 dw->gps_dw1000.last_msg_ticks = sys_time.nb_sec_rem;
321 dw->gps_dw1000.last_msg_time = sys_time.nb_sec;
322 dw->gps_dw1000.last_3dfix_ticks = sys_time.nb_sec_rem;
323 dw->gps_dw1000.last_3dfix_time = sys_time.nb_sec;
324
325 // publish new GPS data
327 AbiSendMsgGPS(GPS_DW1000_ID, now_ts, &(dw->gps_dw1000));
328}
329#endif
330
331#if DW1000_USE_AS_LOCAL_POS
332static void send_pos_estimate(struct DW1000 *dw)
333{
335 // send POSITION_ESTIMATE type message
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) {
342 dw->speed.x, dw->speed.y, dw->speed.z,
344 }
345}
346#endif
347
353static 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
367static 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) {
381 ekf_range_update_dist(&dw->ekf_range, dw->anchors[i].distance,
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
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
417static void process_data(struct DW1000 *dw) {
418 // process if new data
419 if (dw->updated) {
420 // send result if process returns true
422#if DW1000_USE_AS_GPS
423 // send fake GPS message for INS filters
425#endif
426#if DW1000_USE_AS_LOCAL_POS
427 // send a local postion estimate
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();
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",
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
468}
469
470#if SITL
471static const uint8_t wp_ids[] = DW1000_ANCHOR_SIM_WP;
472
473#define DW1000_SITL_SYNC FALSE
474
475static 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]),
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
545
546 struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
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;
552
553 // init trilateration algorithm
555
560 dw1000.ekf_running = false;
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
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) {
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;
615}
616
618{
619#if !SITL
620 // Look for data on serial link and send to parser
625 }
626#endif
627}
628
629
635
641
647
Main include for ABI (AirBorneInterface).
#define GPS_DW1000_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
#define WaypointX(_wp)
Definition common_nav.h:45
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition common_nav.h:48
#define WaypointY(_wp)
Definition common_nav.h:46
#define DW1000_VEL_NOISE_Z
struct EKFRange ekf_range
EKF filter.
void dw1000_arduino_init(void)
static void fill_anchor(struct DW1000 *dw)
Utility function to fill anchor from buffer.
uint8_t state
parser state
#define DW1000_NOISE_X
DW1000 Noise.
#define DW_NB_DATA
void dw1000_arduino_periodic(void)
bool dw1000_use_ekf
enable EKF filtering
float dw1000_ekf_q
process and measurements noise
uint8_t buf[DW_NB_DATA]
incoming data buffer
struct EnuCoor_f pos
local pos in anchors frame
float dw1000_ekf_r_speed
#define DW1000_TIMEOUT
default timeout (in ms)
static float float_from_buf(uint8_t *b)
Utility function to get float from buffer.
float dw1000_ekf_r_dist
uint8_t idx
buffer index
uint8_t ck
checksum
static const float pos_z[]
static bool check_and_compute_data(struct DW1000 *dw)
check new data and compute with the proper algorithm
bool updated
new anchor data available
#define DW1000_NB_ANCHORS
Number of anchors.
#define DW1000_NOISE_Y
struct Anchor anchors[DW1000_NB_ANCHORS]
anchors data
#define DW_STX
waypoints to use as anchors in simulation
#define DW1000_VEL_NOISE_X
struct GpsState gps_dw1000
"fake" gps structure
#define DW_WAIT_STX
Parsing states.
static void dw1000_arduino_parse(struct DW1000 *dw, uint8_t c)
Data parsing function.
void dw1000_arduino_update_ekf_r_speed(float v)
static const float offset[]
#define DW_GET_DATA
static uint16_t uint16_from_buf(uint8_t *b)
Utility function to get uint16_t from buffer.
static const uint16_t ids[]
init arrays from airframe file
bool ekf_running
EKF logic status.
void dw1000_arduino_update_ekf_q(float v)
settings handler
float initial_heading
initial heading correction
static const float scale[]
#define DW1000_INITIAL_HEADING
default initial heading correction between anchors frame and global frame
#define DW1000_NOISE_Z
#define DW1000_USE_EKF
TRUE if EKF range filter is use.
void dw1000_arduino_report(void)
void dw1000_reset_heading_ref(void)
Reset reference heading to current heading AHRS/INS should be aligned before calling this function.
struct MedianFilterFloat mf[DW1000_NB_ANCHORS]
median filter for EKF input data
#define DW1000_OFFSET
default offset, applied to individual distances
#define DW_GET_CK
static bool check_anchor_timeout(struct DW1000 *dw, float timeout)
check timeout for each anchor
static void process_data(struct DW1000 *dw)
void dw1000_arduino_event(void)
static const float pos_y[]
float raw_dist[DW1000_NB_ANCHORS]
raw distance from anchors
#define DW1000_SCALE
default scale factor, applied to individual distances
static const float pos_x[]
struct LtpDef_i ltp_def
ltp reference
void dw1000_arduino_update_ekf_r_dist(float v)
static struct DW1000 dw1000
struct EnuCoor_f speed
local speed in anchors frame
#define DW1000_VEL_NOISE_Y
DW1000 positionning system structure.
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
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
struct EnuCoor_f ekf_range_get_speed(struct EKFRange *ekf_range)
Get current speed.
Definition ekf_range.c:84
void ekf_range_update_dist(struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
correction step
Definition ekf_range.c:143
struct EnuCoor_f ekf_range_get_pos(struct EKFRange *ekf_range)
Get current pos.
Definition ekf_range.c:75
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
void ekf_range_predict(struct EKFRange *ekf_range)
propagate dynamic model
Definition ekf_range.c:119
EKF_range structure.
Definition ekf_range.h:43
void gps_periodic_check(struct GpsState *gps_s)
Periodic GPS check.
Definition gps.c:279
Device independent GPS code (interface)
uint32_t sacc
speed accuracy in cm/s
Definition gps.h:103
uint32_t cacc
course accuracy in rad*1e7
Definition gps.h:104
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition gps.h:99
#define GPS_VALID_VEL_ECEF_BIT
Definition gps.h:51
#define GPS_VALID_VEL_NED_BIT
Definition gps.h:52
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
uint16_t pdop
position dilution of precision scaled by 100
Definition gps.h:105
#define GPS_FIX_NONE
No GPS fix.
Definition gps.h:42
#define GPS_VALID_POS_ECEF_BIT
Definition gps.h:48
#define GPS_VALID_HMSL_BIT
Definition gps.h:53
uint32_t pacc
position accuracy in cm
Definition gps.h:100
uint8_t comp_id
id of current gps
Definition gps.h:89
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
uint8_t fix
status of fix
Definition gps.h:107
data structure for GPS information
Definition gps.h:87
uint16_t u16
Unsigned 16-bit integer.
Definition common.h:43
float q
in rad/s
float p
in rad/s
float r
in rad/s
float psi
in radians
#define FLOAT_VECT3_NORM(_v)
static float float_vect3_norm(struct FloatVect3 *v)
#define FLOAT_VECT2_NORM(_v)
rotation matrix
angular rates
#define VECT3_DIFF(_c, _a, _b)
#define MAT33_ELMT(_m, _row, _col)
int32_t lat
in degrees*1e7
int32_t y
North.
int32_t x
East.
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.
#define VECT3_NED_OF_ENU(_o, _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.
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition state.h:1300
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1306
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:848
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1367
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1058
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition uart_arch.c:357
uint8_t uart_getch(struct uart_periph *p)
Definition uart_arch.c:348
uint16_t foo
Definition main_demo5.c:58
static float update_median_filter_f(struct MedianFilterFloat *filter, float new_data)
static void init_median_filter_f(struct MedianFilterFloat *filter, uint8_t size)
bool log_started
static float timeout
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
FileDes pprzLogFile
API to get/set the generic vehicle states.
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:72
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition sys_time.h:73
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:138
int trilateration_init(struct Anchor *anchors)
Init internal trilateration structures.
int trilateration_compute(struct Anchor *anchors, struct EnuCoor_f *pos)
Compute trilateration based on the latest measurments.
float distance
last measured distance
struct EnuCoor_f pos
position of the anchor
uint16_t id
anchor ID
bool updated
new data available
float time
time of the last received data
Anchor structure.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition wedgebug.c:204
float b
Definition wedgebug.c:202