Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
uwb_positioning.c File Reference
#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:

Go to the source code of this file.

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.

Referenced by uwb_positioning_init().

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

Referenced by check_anchor_timeout(), check_and_compute_data(), uwb_positioning_init(), uwb_positioning_range_periodic(), and uwb_ranging_cb().

◆ UWB_POSITIONING_NOISE_X

#define UWB_POSITIONING_NOISE_X   0.1f

UWB positioning output noise.

Definition at line 94 of file uwb_positioning.c.

◆ UWB_POSITIONING_NOISE_Y

#define UWB_POSITIONING_NOISE_Y   0.1f

Definition at line 98 of file uwb_positioning.c.

◆ UWB_POSITIONING_NOISE_Z

#define UWB_POSITIONING_NOISE_Z   0.1f

Definition at line 102 of file uwb_positioning.c.

◆ UWB_POSITIONING_RANGE_OFFSET

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

default offset, applied to individual distances

Definition at line 69 of file uwb_positioning.c.

◆ UWB_POSITIONING_RANGE_SCALE

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

default scale factor, applied to individual distances

Definition at line 74 of file uwb_positioning.c.

◆ UWB_POSITIONING_TAG_ID

#define UWB_POSITIONING_TAG_ID   0

My UWB tag id.

Definition at line 79 of file uwb_positioning.c.

Referenced by send_anchor_data(), and uwb_ranging_cb().

◆ UWB_POSITIONING_TIMEOUT

#define UWB_POSITIONING_TIMEOUT   500

default timeout (in ms)

Definition at line 89 of file uwb_positioning.c.

Referenced by check_and_compute_data().

◆ UWB_POSITIONING_USE_AS_GPS

#define UWB_POSITIONING_USE_AS_GPS   FALSE

TRUE if UWB positioning is used as GPS.

Definition at line 49 of file uwb_positioning.c.

◆ UWB_POSITIONING_USE_AS_LOCAL_POS

#define UWB_POSITIONING_USE_AS_LOCAL_POS   TRUE

TRUE if UWB positioning is used as local position estimate.

Definition at line 44 of file uwb_positioning.c.

◆ UWB_POSITIONING_USE_EKF

#define UWB_POSITIONING_USE_EKF   TRUE

TRUE if EKF range filter is use.

Definition at line 54 of file uwb_positioning.c.

Referenced by uwb_positioning_init().

◆ UWB_POSITIONING_VEL_NOISE_X

#define UWB_POSITIONING_VEL_NOISE_X   0.1f

Definition at line 106 of file uwb_positioning.c.

◆ UWB_POSITIONING_VEL_NOISE_Y

#define UWB_POSITIONING_VEL_NOISE_Y   0.1f

Definition at line 110 of file uwb_positioning.c.

◆ UWB_POSITIONING_VEL_NOISE_Z

#define UWB_POSITIONING_VEL_NOISE_Z   0.1f

Definition at line 114 of file uwb_positioning.c.

Function Documentation

◆ check_anchor_timeout()

bool check_anchor_timeout ( struct UwbPositioning * uwb,
float timeout )
static

check timeout for each anchor

Parameters
uwbUWB positioning state
timeouttimeout in seconds
Returns
true if one has reach timeout

Definition at line 305 of file uwb_positioning.c.

References UwbPositioning::anchors, get_sys_time_float(), Anchor::time, timeout, and UWB_POSITIONING_NB_ANCHORS.

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

bool check_and_compute_data ( struct UwbPositioning * uwb)
inlinestatic

check new data and compute with the proper algorithm

Returns
true if processing is succesful

Definition at line 319 of file uwb_positioning.c.

References UwbPositioning::anchors, check_anchor_timeout(), Anchor::distance, UwbPositioning::ekf_range, ekf_range_get_pos(), ekf_range_get_speed(), ekf_range_set_state(), ekf_range_update_dist(), UwbPositioning::ekf_running, Anchor::pos, UwbPositioning::pos, UwbPositioning::speed, timeout, trilateration_compute(), Anchor::updated, UWB_POSITIONING_NB_ANCHORS, UWB_POSITIONING_TIMEOUT, and uwb_positioning_use_ekf.

Referenced by process_data().

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

◆ process_data()

◆ uwb_positioning_init()

◆ uwb_positioning_periodic()

void uwb_positioning_periodic ( void )

◆ uwb_positioning_range_periodic()

void uwb_positioning_range_periodic ( void )

Definition at line 552 of file uwb_positioning.c.

References Anchor::id, uwb_positioning, UWB_POSITIONING_NB_ANCHORS, and uwb_range().

Here is the call graph for this function:

◆ uwb_positioning_report()

void uwb_positioning_report ( void )

Definition at line 558 of file uwb_positioning.c.

References DefaultChannel, DefaultDevice, and uwb_positioning.

◆ uwb_positioning_reset_heading_ref()

void uwb_positioning_reset_heading_ref ( void )

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

Definition at line 415 of file uwb_positioning.c.

References FloatEulers::psi, stateGetNedToBodyEulers_f(), and uwb_positioning.

Here is the call graph for this function:

◆ uwb_positioning_update_ekf_q()

void uwb_positioning_update_ekf_q ( float v)

settings handler

Definition at line 579 of file uwb_positioning.c.

References ekf_range_update_noise(), uwb_positioning, uwb_positioning_ekf_q, uwb_positioning_ekf_r_dist, and uwb_positioning_ekf_r_speed.

Here is the call graph for this function:

◆ uwb_positioning_update_ekf_r_dist()

void uwb_positioning_update_ekf_r_dist ( float v)

Definition at line 585 of file uwb_positioning.c.

References ekf_range_update_noise(), uwb_positioning, uwb_positioning_ekf_q, uwb_positioning_ekf_r_dist, and uwb_positioning_ekf_r_speed.

Here is the call graph for this function:

◆ uwb_positioning_update_ekf_r_speed()

void uwb_positioning_update_ekf_r_speed ( float v)

Definition at line 591 of file uwb_positioning.c.

References ekf_range_update_noise(), uwb_positioning, uwb_positioning_ekf_q, uwb_positioning_ekf_r_dist, and uwb_positioning_ekf_r_speed.

Here is the call graph for this function:

◆ uwb_range()

void WEAK uwb_range ( uint16_t id)

Weak empty implementation.

Definition at line 598 of file uwb_positioning.c.

Referenced by uwb_positioning_range_periodic().

Here is the caller graph for this function:

◆ uwb_ranging_cb()

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

Definition at line 201 of file uwb_positioning.c.

References get_sys_time_float(), offset, process_data(), scale, update_median_filter_f(), uwb_positioning, UWB_POSITIONING_NB_ANCHORS, and UWB_POSITIONING_TAG_ID.

Referenced by uwb_positioning_init().

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

Variable Documentation

◆ ids

const uint16_t ids[] = UWB_POSITIONING_ANCHORS_IDS
static

init arrays from airframe file

Definition at line 192 of file uwb_positioning.c.

Referenced by uwb_positioning_init().

◆ offset

◆ pos_x

const float pos_x[] = UWB_POSITIONING_ANCHORS_POS_X
static

Definition at line 193 of file uwb_positioning.c.

Referenced by uwb_positioning_init().

◆ pos_y

const float pos_y[] = UWB_POSITIONING_ANCHORS_POS_Y
static

Definition at line 194 of file uwb_positioning.c.

Referenced by uwb_positioning_init().

◆ pos_z

const float pos_z[] = UWB_POSITIONING_ANCHORS_POS_Z
static

Definition at line 195 of file uwb_positioning.c.

Referenced by send_ins_z(), and uwb_positioning_init().

◆ scale

◆ uwb_positioning

◆ uwb_positioning_ekf_q

float uwb_positioning_ekf_q

◆ uwb_positioning_ekf_r_dist

◆ uwb_positioning_ekf_r_speed

◆ uwb_positioning_use_ekf

bool uwb_positioning_use_ekf

enable EKF filtering

Definition at line 186 of file uwb_positioning.c.

Referenced by check_and_compute_data(), uwb_positioning_init(), and uwb_positioning_periodic().

◆ uwb_ranging_ev

abi_event uwb_ranging_ev
static

Definition at line 184 of file uwb_positioning.c.

Referenced by uwb_positioning_init().