#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:

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

dw	DW1000 struct
timeout	timeout 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()

void dw1000_arduino_init ( void )

Definition at line 513 of file dw1000_arduino.c.

References LlaCoor_i::alt, DW1000::anchors, GpsState::cacc, DW1000::ck, GpsState::comp_id, Anchor::distance, dw1000, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed, DW1000_INITIAL_HEADING, DW1000_NB_ANCHORS, DW1000_USE_EKF, dw1000_use_ekf, DW_WAIT_STX, DW1000::ekf_range, ekf_range_init(), DW1000::ekf_running, GpsState::fix, DW1000::gps_dw1000, GPS_DW1000_ID, GPS_FIX_NONE, Anchor::id, ids, DW1000::idx, init_median_filter_f(), DW1000::initial_heading, LlaCoor_i::lat, LlaCoor_i::lon, DW1000::ltp_def, ltp_def_from_lla_i(), DW1000::mf, GpsState::pacc, GpsState::pdop, DW1000::pos, Anchor::pos, pos_x, pos_y, pos_z, DW1000::raw_dist, GpsState::sacc, DW1000::speed, DW1000::state, Anchor::time, trilateration_init(), DW1000::updated, Anchor::updated, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Here is the call graph for this function:

◆ 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 )

Definition at line 568 of file dw1000_arduino.c.

References dw1000, dw1000_use_ekf, DW1000::ekf_range, ekf_range_get_pos(), ekf_range_get_speed(), ekf_range_predict(), DW1000::ekf_running, DW1000::gps_dw1000, gps_periodic_check(), log_started, DW1000::pos, pprzLogFile, process_data(), and DW1000::speed.

Here is the call graph for this function:

◆ dw1000_arduino_report()

void dw1000_arduino_report ( void )

Definition at line 599 of file dw1000_arduino.c.

References DW1000::anchors, DefaultChannel, DefaultDevice, Anchor::distance, dw1000, DW1000::pos, DW1000::raw_dist, DW1000::speed, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

◆ 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()

static void process_data ( struct DW1000 * dw )

static

Definition at line 417 of file dw1000_arduino.c.

References DW1000::anchors, check_and_compute_data(), Anchor::distance, dw1000, log_started, MAT33_ELMT, FloatRates::p, DW1000::pos, pprzLogFile, FloatRates::q, FloatRates::r, stateGetBodyRates_f(), stateGetNedToBodyRMat_f(), stateGetPositionEnu_f(), stateGetSpeedEnu_f(), Anchor::time, DW1000::updated, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by dw1000_arduino_event(), and dw1000_arduino_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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

struct DW1000 dw1000

static

Definition at line 1 of file dw1000_arduino.c.

Referenced by dw1000_arduino_event(), dw1000_arduino_init(), dw1000_arduino_periodic(), dw1000_arduino_report(), dw1000_arduino_update_ekf_q(), dw1000_arduino_update_ekf_r_dist(), dw1000_arduino_update_ekf_r_speed(), dw1000_reset_heading_ref(), and process_data().

◆ dw1000_ekf_q

float dw1000_ekf_q

process and measurements noise

Definition at line 190 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init(), dw1000_arduino_update_ekf_q(), dw1000_arduino_update_ekf_r_dist(), and dw1000_arduino_update_ekf_r_speed().

◆ dw1000_ekf_r_dist

float dw1000_ekf_r_dist

Definition at line 191 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init(), dw1000_arduino_update_ekf_q(), dw1000_arduino_update_ekf_r_dist(), and dw1000_arduino_update_ekf_r_speed().

◆ dw1000_ekf_r_speed

float dw1000_ekf_r_speed

Definition at line 192 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init(), dw1000_arduino_update_ekf_q(), dw1000_arduino_update_ekf_r_dist(), and dw1000_arduino_update_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

const float offset[] = DW1000_OFFSET

static

◆ 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

const float scale[] = DW1000_SCALE

static

Definition at line 200 of file dw1000_arduino.c.

Referenced by detection_cb(), dnrm2(), fill_anchor(), float_vect2_bound_in_2d(), float_vect2_scale_in_2d(), float_vect3_bound_in_2d(), float_vect3_bound_in_3d(), float_vect3_scale_in_2d(), h(), imu_set_defaults_accel(), imu_set_defaults_gyro(), imu_set_defaults_mag(), lis3mdl_init(), lla_of_utm_d(), lla_of_utm_f(), sobel_OCV(), vect_bound_nd(), xnrm2(), and xnrm2_f().

Data Structures

Macros

Functions

Variables

Detailed Description

Data Structure Documentation

◆ DW1000

Macro Definition Documentation

◆ DW1000_INITIAL_HEADING

◆ DW1000_NB_ANCHORS

◆ DW1000_NOISE_X

◆ DW1000_NOISE_Y

◆ DW1000_NOISE_Z

◆ DW1000_OFFSET

◆ DW1000_SCALE

◆ DW1000_TIMEOUT

◆ DW1000_USE_AS_GPS

◆ DW1000_USE_AS_LOCAL_POS

◆ DW1000_USE_EKF

◆ DW1000_VEL_NOISE_X

◆ DW1000_VEL_NOISE_Y

◆ DW1000_VEL_NOISE_Z

◆ DW_GET_CK

◆ DW_GET_DATA

◆ DW_NB_DATA

◆ DW_STX

◆ DW_WAIT_STX

Function Documentation

◆ check_anchor_timeout()

◆ check_and_compute_data()

◆ dw1000_arduino_event()

◆ dw1000_arduino_init()

◆ dw1000_arduino_parse()

◆ dw1000_arduino_periodic()

◆ dw1000_arduino_report()

◆ dw1000_arduino_update_ekf_q()

◆ dw1000_arduino_update_ekf_r_dist()

◆ dw1000_arduino_update_ekf_r_speed()

◆ dw1000_reset_heading_ref()

◆ fill_anchor()

◆ float_from_buf()

◆ process_data()

◆ uint16_from_buf()

Variable Documentation

◆ dw1000

◆ dw1000_ekf_q

◆ dw1000_ekf_r_dist

◆ dw1000_ekf_r_speed

◆ dw1000_use_ekf

◆ ids

◆ offset

◆ pos_x

◆ pos_y

◆ pos_z

◆ scale