Paparazzi UAS  v5.15_devel-47-g0391b4d
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
dw1000_arduino.c File Reference
#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:

Go to the source code of this file.

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(), Anchor::time, and timeout.

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

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