#include "modules/decawave/dw1000_arduino.h"
#include "std.h"
#include "mcu_periph/uart.h"
#include "pprzlink/messages.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/abi.h"
#include "modules/decawave/trilateration.h"
#include "subsystems/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_NB_ANCHORS 3
	Number of anchors. More...

#define	DW1000_OFFSET { 0.f, 0.f, 0.f }
	default offset, applied to final result not to individual distances More...

#define	DW1000_SCALE { 1.f, 1.f, 1.f }
	default scale factor, applied to final result not 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	DW_STX 0xFE
	frame sync byte 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 void	send_gps_dw1000_small (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...

static void	scale_position (struct DW1000 *dw)

void	dw1000_arduino_init ()

void	dw1000_arduino_periodic ()

void	dw1000_arduino_report ()

static bool	check_anchor_timeout (struct DW1000 *dw)
	check timeout for each anchor More...

void	dw1000_arduino_event ()

Variables
static struct DW1000	dw1000

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

struct DW1000

DW1000 positionning system structure.

Definition at line 82 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 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 EnuCoor_f	pos	local pos in anchors frame
struct EnuCoor_f	raw_pos	unscaled local pos in anchors frame
uint8_t	state	parser state
bool	updated	new anchor data available

Macro Definition Documentation

#define DW1000_INITIAL_HEADING 0.f

default initial heading correction between anchors frame and global frame

Definition at line 64 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init().

#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 49 of file dw1000_arduino.c.

Referenced by check_anchor_timeout(), dw1000_arduino_init(), and fill_anchor().

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

default offset, applied to final result not to individual distances

Definition at line 54 of file dw1000_arduino.c.

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

default scale factor, applied to final result not to individual distances

Definition at line 59 of file dw1000_arduino.c.

#define DW1000_TIMEOUT 500

default timeout (in ms)

Definition at line 69 of file dw1000_arduino.c.

Referenced by check_anchor_timeout().

#define DW_GET_CK 2

Definition at line 78 of file dw1000_arduino.c.

Referenced by dw1000_arduino_parse().

#define DW_GET_DATA 1

Definition at line 77 of file dw1000_arduino.c.

Referenced by dw1000_arduino_parse().

#define DW_NB_DATA 6

Definition at line 79 of file dw1000_arduino.c.

Referenced by dw1000_arduino_parse().

#define DW_STX 0xFE

frame sync byte

Definition at line 73 of file dw1000_arduino.c.

Referenced by dw1000_arduino_parse().

#define DW_WAIT_STX 0

Parsing states.

Definition at line 76 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init(), and dw1000_arduino_parse().

Function Documentation

static bool check_anchor_timeout ( struct DW1000 * dw )

static

check timeout for each anchor

Returns: true if one has reach timeout

Definition at line 280 of file dw1000_arduino.c.

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

Referenced by dw1000_arduino_event().

Here is the call graph for this function:

Here is the caller graph for this function:

void dw1000_arduino_event ( void )

Definition at line 292 of file dw1000_arduino.c.

References DW1000::anchors, check_anchor_timeout(), dw1000, dw1000_arduino_parse(), DW1000::raw_pos, scale_position(), send_gps_dw1000_small(), trilateration_compute(), uart_char_available(), uart_getch(), and DW1000::updated.

Here is the call graph for this function:

void dw1000_arduino_init ( void )

Definition at line 216 of file dw1000_arduino.c.

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

Here is the call graph for this function:

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

static

Data parsing function.

Definition at line 127 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:

void dw1000_arduino_periodic ( void )

Definition at line 256 of file dw1000_arduino.c.

References dw1000, DW1000::gps_dw1000, and gps_periodic_check().

Here is the call graph for this function:

void dw1000_arduino_report ( void )

Definition at line 262 of file dw1000_arduino.c.

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

void dw1000_reset_heading_ref ( void )

Reset reference heading to current heading AHRS/INS should be aligned before calling this function.

Definition at line 194 of file dw1000_arduino.c.

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

Here is the call graph for this function:

static void fill_anchor ( struct DW1000 * dw )

static

Utility function to fill anchor from buffer.

Definition at line 114 of file dw1000_arduino.c.

References DW1000::anchors, DW1000::buf, Anchor::distance, DW1000_NB_ANCHORS, float_from_buf(), get_sys_time_float(), Anchor::id, Anchor::time, uint16_from_buf(), and DW1000::updated.

Referenced by dw1000_arduino_parse().

Here is the call graph for this function:

Here is the caller graph for this function:

static float float_from_buf ( uint8_t * b )

inlinestatic

Utility function to get float from buffer.

Definition at line 100 of file dw1000_arduino.c.

Referenced by fill_anchor().

Here is the caller graph for this function:

static void scale_position ( struct DW1000 * dw )

static

Definition at line 209 of file dw1000_arduino.c.

References offset, DW1000::pos, DW1000::raw_pos, scale, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by dw1000_arduino_event().

Here is the caller graph for this function:

static void send_gps_dw1000_small ( struct DW1000 * dw )

static

Definition at line 159 of file dw1000_arduino.c.

References ecef_of_enu_point_i(), GpsState::ecef_pos, GpsState::fix, get_sys_time_msec(), get_sys_time_usec(), DW1000::gps_dw1000, GPS_DW1000_ID, GPS_FIX_3D, GPS_VALID_HMSL_BIT, GPS_VALID_POS_ECEF_BIT, GPS_VALID_POS_LLA_BIT, GpsState::hmsl, LtpDef_i::hmsl, DW1000::initial_heading, GpsState::last_3dfix_ticks, GpsState::last_3dfix_time, GpsState::last_msg_ticks, GpsState::last_msg_time, lla_of_ecef_i(), GpsState::lla_pos, DW1000::ltp_def, sys_time::nb_sec, sys_time::nb_sec_rem, GpsState::num_sv, DW1000::pos, GpsState::tow, GpsState::valid_fields, EnuCoor_f::x, EnuCoor_i::x, EnuCoor_f::y, EnuCoor_i::y, EnuCoor_f::z, and EnuCoor_i::z.

Referenced by dw1000_arduino_event().

Here is the call graph for this function:

Here is the caller graph for this function:

static uint16_t uint16_from_buf ( uint8_t * b )

inlinestatic

Utility function to get uint16_t from buffer.

Definition at line 107 of file dw1000_arduino.c.

Referenced by fill_anchor().

Here is the caller graph for this function:

Variable Documentation

struct DW1000 dw1000

static

Definition at line 96 of file dw1000_arduino.c.

Referenced by dw1000_arduino_event(), dw1000_arduino_init(), dw1000_arduino_periodic(), dw1000_arduino_report(), and dw1000_reset_heading_ref().

const uint16_t ids[] = DW1000_ANCHORS_IDS

static

init arrays from airframe file

Definition at line 202 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init().

const float offset[] = DW1000_OFFSET

static

Definition at line 206 of file dw1000_arduino.c.

Referenced by offset_commands(), parse_xsens700_msg(), parse_xsens_msg(), rtp_frame_send(), scale_position(), and w5100_send().

const float pos_x[] = DW1000_ANCHORS_POS_X

static

Definition at line 203 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init().

const float pos_y[] = DW1000_ANCHORS_POS_Y

static

Definition at line 204 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init().

const float pos_z[] = DW1000_ANCHORS_POS_Z

static

Definition at line 205 of file dw1000_arduino.c.

Referenced by dw1000_arduino_init().

const float scale[] = DW1000_SCALE

static

Definition at line 207 of file dw1000_arduino.c.

Referenced by dnrm2(), h(), lla_of_utm_d(), lla_of_utm_f(), scale_position(), xnrm2(), and xnrm2_f().

Data Structures

Macros

Functions

Variables

Detailed Description

Data Structure Documentation

Macro Definition Documentation

Function Documentation

Variable Documentation