Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
dw1000_arduino.c File Reference
#include "modules/decawave/dw1000_arduino.h"
#include "std.h"
#include "mcu_periph/uart.h"
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
#include "modules/core/abi.h"
#include "modules/decawave/trilateration.h"
#include "modules/gps/gps.h"
#include "state.h"
#include "generated/flight_plan.h"
#include "generated/airframe.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
+ Include dependency graph for dw1000_arduino.c:

Go to the source code of this file.

Data Structures

struct  DW1000
 DW1000 positionning system structure. More...
 

Macros

#define DW1000_USE_AS_LOCAL_POS   TRUE
 TRUE if dw1000 are used as local position estimate. More...
 
#define DW1000_USE_AS_GPS   FALSE
 TRUE if dw1000 are used as GPS. More...
 
#define DW1000_USE_EKF   TRUE
 TRUE if EKF range filter is use. More...
 
#define DW1000_NB_ANCHORS   3
 Number of anchors. More...
 
#define DW1000_OFFSET   { 0.f, 0.f, 0.f }
 default offset, applied to individual distances More...
 
#define DW1000_SCALE   { 1.f, 1.f, 1.f }
 default scale factor, applied to individual distances More...
 
#define DW1000_INITIAL_HEADING   0.f
 default initial heading correction between anchors frame and global frame More...
 
#define DW1000_TIMEOUT   500
 default timeout (in ms) More...
 
#define DW1000_NOISE_X   0.1f
 DW1000 Noise. More...
 
#define DW1000_NOISE_Y   0.1f
 
#define DW1000_NOISE_Z   0.1f
 
#define DW1000_VEL_NOISE_X   0.1f
 
#define DW1000_VEL_NOISE_Y   0.1f
 
#define DW1000_VEL_NOISE_Z   0.1f
 
#define DW_STX   0xFE
 waypoints to use as anchors in simulation More...
 
#define DW_WAIT_STX   0
 Parsing states. More...
 
#define DW_GET_DATA   1
 
#define DW_GET_CK   2
 
#define DW_NB_DATA   6
 

Functions

static float float_from_buf (uint8_t *b)
 Utility function to get float from buffer. More...
 
static uint16_t uint16_from_buf (uint8_t *b)
 Utility function to get uint16_t from buffer. More...
 
static void fill_anchor (struct DW1000 *dw)
 Utility function to fill anchor from buffer. More...
 
static void dw1000_arduino_parse (struct DW1000 *dw, uint8_t c)
 Data parsing function. More...
 
static bool check_anchor_timeout (struct DW1000 *dw, float timeout)
 check timeout for each anchor More...
 
static bool check_and_compute_data (struct DW1000 *dw)
 check new data and compute with the proper algorithm More...
 
static void process_data (struct DW1000 *dw)
 
void dw1000_reset_heading_ref (void)
 Reset reference heading to current heading AHRS/INS should be aligned before calling this function. More...
 
void dw1000_arduino_init (void)
 
void dw1000_arduino_periodic (void)
 
void dw1000_arduino_report (void)
 
void dw1000_arduino_event (void)
 
void dw1000_arduino_update_ekf_q (float v)
 settings handler More...
 
void dw1000_arduino_update_ekf_r_dist (float v)
 
void dw1000_arduino_update_ekf_r_speed (float v)
 

Variables

static struct DW1000 dw1000
 
bool dw1000_use_ekf
 enable EKF filtering More...
 
float dw1000_ekf_q
 process and measurements noise More...
 
float dw1000_ekf_r_dist
 
float dw1000_ekf_r_speed
 
static const uint16_t ids [] = DW1000_ANCHORS_IDS
 init arrays from airframe file More...
 
static const float pos_x [] = DW1000_ANCHORS_POS_X
 
static const float pos_y [] = DW1000_ANCHORS_POS_Y
 
static const float pos_z [] = DW1000_ANCHORS_POS_Z
 
static const float offset [] = DW1000_OFFSET
 
static const float scale [] = DW1000_SCALE
 

Detailed Description

Author
Gautier Hattenberger Driver to get ranging data from Decawave DW1000 modules connected to Arduino

Definition in file dw1000_arduino.c.


Data Structure Documentation

◆ DW1000

struct DW1000

DW1000 positionning system structure.

Definition at line 166 of file dw1000_arduino.c.

+ Collaboration diagram for DW1000:
Data Fields
struct Anchor anchors[DW1000_NB_ANCHORS] anchors data
uint8_t buf[DW_NB_DATA] incoming data buffer
uint8_t ck checksum
struct EKFRange ekf_range EKF filter.
bool ekf_running EKF logic status.
struct GpsState gps_dw1000 "fake" gps structure
uint8_t idx buffer index
float initial_heading initial heading correction
struct LtpDef_i ltp_def ltp reference
struct MedianFilterFloat mf[DW1000_NB_ANCHORS] median filter for EKF input data
struct EnuCoor_f pos local pos in anchors frame
float raw_dist[DW1000_NB_ANCHORS] raw distance from anchors
struct EnuCoor_f speed local speed in anchors frame
uint8_t state parser state
bool updated new anchor data available

Macro Definition Documentation

◆ DW1000_INITIAL_HEADING

#define DW1000_INITIAL_HEADING   0.f

default initial heading correction between anchors frame and global frame

Definition at line 79 of file dw1000_arduino.c.

◆ DW1000_NB_ANCHORS

#define DW1000_NB_ANCHORS   3

Number of anchors.

using standard trilateration algorithm, only 3 anchors are required/supported at the moment. More advanced multilateration algorithms might allow more anchors in the future

Definition at line 64 of file dw1000_arduino.c.

◆ DW1000_NOISE_X

#define DW1000_NOISE_X   0.1f

DW1000 Noise.

Definition at line 89 of file dw1000_arduino.c.

◆ DW1000_NOISE_Y

#define DW1000_NOISE_Y   0.1f

Definition at line 93 of file dw1000_arduino.c.

◆ DW1000_NOISE_Z

#define DW1000_NOISE_Z   0.1f

Definition at line 97 of file dw1000_arduino.c.

◆ DW1000_OFFSET

#define DW1000_OFFSET   { 0.f, 0.f, 0.f }

default offset, applied to individual distances

Definition at line 69 of file dw1000_arduino.c.

◆ DW1000_SCALE

#define DW1000_SCALE   { 1.f, 1.f, 1.f }

default scale factor, applied to individual distances

Definition at line 74 of file dw1000_arduino.c.

◆ DW1000_TIMEOUT

#define DW1000_TIMEOUT   500

default timeout (in ms)

Definition at line 84 of file dw1000_arduino.c.

◆ DW1000_USE_AS_GPS

#define DW1000_USE_AS_GPS   FALSE

TRUE if dw1000 are used as GPS.

Definition at line 49 of file dw1000_arduino.c.

◆ DW1000_USE_AS_LOCAL_POS

#define DW1000_USE_AS_LOCAL_POS   TRUE

TRUE if dw1000 are used as local position estimate.

Definition at line 44 of file dw1000_arduino.c.

◆ DW1000_USE_EKF

#define DW1000_USE_EKF   TRUE

TRUE if EKF range filter is use.

Definition at line 54 of file dw1000_arduino.c.

◆ DW1000_VEL_NOISE_X

#define DW1000_VEL_NOISE_X   0.1f

Definition at line 101 of file dw1000_arduino.c.

◆ DW1000_VEL_NOISE_Y

#define DW1000_VEL_NOISE_Y   0.1f

Definition at line 105 of file dw1000_arduino.c.

◆ DW1000_VEL_NOISE_Z

#define DW1000_VEL_NOISE_Z   0.1f

Definition at line 109 of file dw1000_arduino.c.

◆ DW_GET_CK

#define DW_GET_CK   2

Definition at line 162 of file dw1000_arduino.c.

◆ DW_GET_DATA

#define DW_GET_DATA   1

Definition at line 161 of file dw1000_arduino.c.

◆ DW_NB_DATA

#define DW_NB_DATA   6

Definition at line 163 of file dw1000_arduino.c.

◆ DW_STX

#define DW_STX   0xFE

waypoints to use as anchors in simulation

ChibiOS SD logger frame sync byte

Definition at line 157 of file dw1000_arduino.c.

◆ DW_WAIT_STX

#define DW_WAIT_STX   0

Parsing states.

Definition at line 160 of file dw1000_arduino.c.

Function Documentation

◆ check_anchor_timeout()

static bool check_anchor_timeout ( struct DW1000 dw,
float  timeout 
)
static

check timeout for each anchor

Parameters
dwDW1000 struct
timeouttimeout in seconds
Returns
true if one has reach timeout

Definition at line 353 of file dw1000_arduino.c.

References DW1000::anchors, DW1000_NB_ANCHORS, get_sys_time_float(), Anchor::time, and timeout.

Referenced by check_and_compute_data().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ check_and_compute_data()

static bool check_and_compute_data ( struct DW1000 dw)
inlinestatic

check new data and compute with the proper algorithm

Returns
true if processing is succesful

Definition at line 367 of file dw1000_arduino.c.

References DW1000::anchors, check_anchor_timeout(), Anchor::distance, DW1000_NB_ANCHORS, DW1000_TIMEOUT, dw1000_use_ekf, DW1000::ekf_range, ekf_range_get_pos(), ekf_range_get_speed(), ekf_range_set_state(), ekf_range_update_dist(), DW1000::ekf_running, DW1000::pos, Anchor::pos, DW1000::speed, timeout, trilateration_compute(), and Anchor::updated.

Referenced by process_data().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ dw1000_arduino_event()

void dw1000_arduino_event ( void  )

Definition at line 617 of file dw1000_arduino.c.

References dw1000, dw1000_arduino_parse(), process_data(), uart_char_available(), and uart_getch().

+ Here is the call graph for this function:

◆ dw1000_arduino_init()

◆ dw1000_arduino_parse()

static void dw1000_arduino_parse ( struct DW1000 dw,
uint8_t  c 
)
static

Data parsing function.

Definition at line 236 of file dw1000_arduino.c.

References DW1000::buf, DW1000::ck, DW_GET_CK, DW_GET_DATA, DW_NB_DATA, DW_STX, DW_WAIT_STX, fill_anchor(), DW1000::idx, and DW1000::state.

Referenced by dw1000_arduino_event().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ dw1000_arduino_periodic()

void dw1000_arduino_periodic ( void  )

◆ dw1000_arduino_report()

void dw1000_arduino_report ( void  )

◆ dw1000_arduino_update_ekf_q()

void dw1000_arduino_update_ekf_q ( float  v)

settings handler

Definition at line 630 of file dw1000_arduino.c.

References dw1000, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed, DW1000::ekf_range, and ekf_range_update_noise().

+ Here is the call graph for this function:

◆ dw1000_arduino_update_ekf_r_dist()

void dw1000_arduino_update_ekf_r_dist ( float  v)

Definition at line 636 of file dw1000_arduino.c.

References dw1000, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed, DW1000::ekf_range, and ekf_range_update_noise().

+ Here is the call graph for this function:

◆ dw1000_arduino_update_ekf_r_speed()

void dw1000_arduino_update_ekf_r_speed ( float  v)

Definition at line 642 of file dw1000_arduino.c.

References dw1000, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed, DW1000::ekf_range, and ekf_range_update_noise().

+ Here is the call graph for this function:

◆ 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 at line 463 of file dw1000_arduino.c.

References dw1000, DW1000::initial_heading, FloatEulers::psi, and stateGetNedToBodyEulers_f().

+ Here is the call graph for this function:

◆ fill_anchor()

static void fill_anchor ( struct DW1000 dw)
static

Utility function to fill anchor from buffer.

Definition at line 218 of file dw1000_arduino.c.

References DW1000::anchors, DW1000::buf, Anchor::distance, DW1000_NB_ANCHORS, float_from_buf(), get_sys_time_float(), Anchor::id, DW1000::mf, offset, DW1000::raw_dist, scale, Anchor::time, uint16_from_buf(), update_median_filter_f(), DW1000::updated, and Anchor::updated.

Referenced by dw1000_arduino_parse().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ float_from_buf()

static float float_from_buf ( uint8_t b)
inlinestatic

Utility function to get float from buffer.

Definition at line 204 of file dw1000_arduino.c.

References b, and f.

Referenced by fill_anchor().

+ Here is the caller graph for this function:

◆ process_data()

◆ uint16_from_buf()

static uint16_t uint16_from_buf ( uint8_t b)
inlinestatic

Utility function to get uint16_t from buffer.

Definition at line 211 of file dw1000_arduino.c.

References b.

Referenced by fill_anchor().

+ Here is the caller graph for this function:

Variable Documentation

◆ dw1000

◆ dw1000_ekf_q

float dw1000_ekf_q

◆ dw1000_ekf_r_dist

◆ dw1000_ekf_r_speed

◆ dw1000_use_ekf

bool dw1000_use_ekf

enable EKF filtering

Definition at line 189 of file dw1000_arduino.c.

Referenced by check_and_compute_data(), dw1000_arduino_init(), and dw1000_arduino_periodic().

◆ ids

const uint16_t ids[] = DW1000_ANCHORS_IDS
static

init arrays from airframe file

Definition at line 195 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init().

◆ offset

◆ pos_x

const float pos_x[] = DW1000_ANCHORS_POS_X
static

Definition at line 196 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init().

◆ pos_y

const float pos_y[] = DW1000_ANCHORS_POS_Y
static

Definition at line 197 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init().

◆ pos_z

const float pos_z[] = DW1000_ANCHORS_POS_Z
static

Definition at line 198 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init(), and send_ins_z().

◆ scale