#include "modules/decawave/uwb_positioning.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 uwb_positioning.c:

Data Structures
struct	UwbPositioning
	waypoints to use as anchors in simulation More...

Macros
#define	UWB_POSITIONING_USE_AS_LOCAL_POS TRUE
	TRUE if UWB positioning is used as local position estimate.

#define	UWB_POSITIONING_USE_AS_GPS FALSE
	TRUE if UWB positioning is used as GPS.

#define	UWB_POSITIONING_USE_EKF TRUE
	TRUE if EKF range filter is use.

#define	UWB_POSITIONING_NB_ANCHORS 3
	Number of anchors.

#define	UWB_POSITIONING_RANGE_OFFSET { 0.f, 0.f, 0.f }
	default offset, applied to individual distances

#define	UWB_POSITIONING_RANGE_SCALE { 1.f, 1.f, 1.f }
	default scale factor, applied to individual distances

#define	UWB_POSITIONING_TAG_ID 0
	My UWB tag id.

#define	UWB_POSITIONING_INITIAL_HEADING 0.f
	default initial heading correction between anchors frame and global frame

#define	UWB_POSITIONING_TIMEOUT 500
	default timeout (in ms)

#define	UWB_POSITIONING_NOISE_X 0.1f
	UWB positioning output noise.

#define	UWB_POSITIONING_NOISE_Y 0.1f

#define	UWB_POSITIONING_NOISE_Z 0.1f

#define	UWB_POSITIONING_VEL_NOISE_X 0.1f

#define	UWB_POSITIONING_VEL_NOISE_Y 0.1f

#define	UWB_POSITIONING_VEL_NOISE_Z 0.1f

Functions
static void	process_data (struct UwbPositioning *uwb)

static void	uwb_ranging_cb (uint8_t sender_id, uint32_t stamp, uint16_t src_id, uint16_t dst_id, float range)

static bool	check_anchor_timeout (struct UwbPositioning *uwb, float timeout)
	check timeout for each anchor

static bool	check_and_compute_data (struct UwbPositioning *uwb)
	check new data and compute with the proper algorithm

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

void	uwb_positioning_init (void)

void	uwb_positioning_periodic (void)

void	uwb_positioning_range_periodic (void)

void	uwb_positioning_report (void)

void	uwb_positioning_update_ekf_q (float v)
	settings handler

void	uwb_positioning_update_ekf_r_dist (float v)

void	uwb_positioning_update_ekf_r_speed (float v)

void WEAK	uwb_range (uint16_t id)
	Weak empty implementation.

Variables
static struct UwbPositioning	uwb_positioning

static abi_event	uwb_ranging_ev

bool	uwb_positioning_use_ekf
	enable EKF filtering

float	uwb_positioning_ekf_q
	process and measurements noise

float	uwb_positioning_ekf_r_dist

float	uwb_positioning_ekf_r_speed

static const uint16_t	ids [] = UWB_POSITIONING_ANCHORS_IDS
	init arrays from airframe file

static const float	pos_x [] = UWB_POSITIONING_ANCHORS_POS_X

static const float	pos_y [] = UWB_POSITIONING_ANCHORS_POS_Y

static const float	pos_z [] = UWB_POSITIONING_ANCHORS_POS_Z

static const float	offset [] = UWB_POSITIONING_RANGE_OFFSET

static const float	scale [] = UWB_POSITIONING_RANGE_SCALE

Detailed Description

Author: Gautier Hattenberger UWB positioning from anchor measurements.

Definition in file uwb_positioning.c.

Data Structure Documentation

◆ UwbPositioning

struct UwbPositioning

waypoints to use as anchors in simulation

ChibiOS SD logger UWB positioning system structure

Definition at line 164 of file uwb_positioning.c.

Collaboration diagram for UwbPositioning:

Data Fields
uint8_t	anchor_ranging_idx	Next anchor index (in the anchors array) to be ranged.
struct Anchor	anchors[UWB_POSITIONING_NB_ANCHORS]	anchors data
struct EKFRange	ekf_range	EKF filter.
bool	ekf_running	EKF logic status.
struct GpsState	gps_uwb	"fake" gps structure
float	initial_heading	initial heading correction
struct LtpDef_i	ltp_def	ltp reference
struct MedianFilterFloat	mf[UWB_POSITIONING_NB_ANCHORS]	median filter for EKF input data
struct EnuCoor_f	pos	local pos in anchors frame
float	raw_dist[UWB_POSITIONING_NB_ANCHORS]	raw distance from anchors
struct EnuCoor_f	speed	local speed in anchors frame
bool	updated	new anchor data available

Macro Definition Documentation

◆ UWB_POSITIONING_INITIAL_HEADING

#define UWB_POSITIONING_INITIAL_HEADING 0.f

default initial heading correction between anchors frame and global frame

Definition at line 84 of file uwb_positioning.c.

◆ UWB_POSITIONING_NB_ANCHORS

#define UWB_POSITIONING_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 uwb_positioning.c.