relative_localization_filter.c File Reference

#include "relative_localization_filter.h"
#include "subsystems/datalink/telemetry.h"
#include "state.h"
#include "modules/datalink/extra_pprz_dl.h"
#include "subsystems/abi.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_int.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "discrete_ekf_no_north.h"

Include dependency graph for relative_localization_filter.c:

Go to the source code of this file.

Macros
#define	RELATIVE_LOCALIZATION_N_UAVS   4
#define	RELATIVE_LOCALIZATION_NO_NORTH   1

Functions
static void	range_msg_callback (uint8_t sender_id, uint8_t ac_id, float range, float trackedVx, float trackedVy, float trackedh, float trackedAx, float trackedAy, float trackedYawr)
static void	send_relative_localization_data (struct transport_tx *trans, struct link_device *dev)
void	relative_localization_filter_init (void)
void	relative_localization_filter_periodic (void)

Variables
struct discrete_ekf_no_north	ekf_rl [RELATIVE_LOCALIZATION_N_UAVS]
int32_t	id_array [RELATIVE_LOCALIZATION_N_UAVS]
uint32_t	latest_update_time [RELATIVE_LOCALIZATION_N_UAVS]
uint8_t	number_filters
float	range_array [RELATIVE_LOCALIZATION_N_UAVS]
uint8_t	pprzmsg_cnt
static abi_event	range_communication_event

Detailed Description

Author

Mario Coppola Relative Localization Filter for collision avoidance between drones

Definition in file relative_localization_filter.c.

Macro Definition Documentation

#define RELATIVE_LOCALIZATION_N_UAVS   4

Definition at line 41 of file relative_localization_filter.c.

Referenced by range_msg_callback(), and relative_localization_filter_init().

#define RELATIVE_LOCALIZATION_NO_NORTH   1

Definition at line 49 of file relative_localization_filter.c.

Function Documentation

static void range_msg_callback ( uint8_t  sender_id, uint8_t  ac_id, float  range, float  trackedVx, float  trackedVy, float  trackedh, float  trackedAx, float  trackedAy, float  trackedYawr  )

static

Definition at line 67 of file relative_localization_filter.c.

References ac_id, discrete_ekf_new(), discrete_ekf_no_north_new(), discrete_ekf_no_north_predict(), discrete_ekf_no_north_update(), discrete_ekf_predict(), discrete_ekf_update(), discrete_ekf_no_north::dt, EKF_L, EKF_M, ekf_rl, get_sys_time_usec(), id_array, idx, int32_vect_find(), latest_update_time, number_filters, FloatRates::r, range_array, RELATIVE_LOCALIZATION_N_UAVS, stateGetAccelNed_f(), stateGetBodyRates_f(), stateGetPositionEnu_f(), stateGetSpeedNed_f(), NedCoor_f::x, NedCoor_f::y, mesonh.mesonh_atmosphere::Z, and EnuCoor_f::z.

Referenced by relative_localization_filter_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void relative_localization_filter_init

(

void

)

Definition at line 128 of file relative_localization_filter.c.

References DefaultPeriodic, id_array, int32_vect_set_value(), number_filters, pprzmsg_cnt, range_msg_callback(), register_periodic_telemetry(), RELATIVE_LOCALIZATION_N_UAVS, send_relative_localization_data(), and UWB_COMM_ID.

Here is the call graph for this function:

void relative_localization_filter_periodic

(

void

)

Definition at line 139 of file relative_localization_filter.c.

References ekf_rl, id_array, number_filters, RELATIVE_LOCALIZATION_ID, and mesonh.mesonh_atmosphere::X.

static void send_relative_localization_data ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 111 of file relative_localization_filter.c.

References ekf_rl, id_array, number_filters, pprzmsg_cnt, range_array, and mesonh.mesonh_atmosphere::X.

Referenced by relative_localization_filter_init().

Here is the caller graph for this function:

Variable Documentation

struct discrete_ekf_no_north ekf_rl[RELATIVE_LOCALIZATION_N_UAVS]

Definition at line 54 of file relative_localization_filter.c.

Referenced by range_msg_callback(), relative_localization_filter_periodic(), and send_relative_localization_data().

int32_t id_array[RELATIVE_LOCALIZATION_N_UAVS]

Definition at line 60 of file relative_localization_filter.c.

Referenced by range_msg_callback(), relative_localization_filter_init(), relative_localization_filter_periodic(), and send_relative_localization_data().

uint32_t latest_update_time[RELATIVE_LOCALIZATION_N_UAVS]

Definition at line 61 of file relative_localization_filter.c.

Referenced by range_msg_callback().

uint8_t number_filters

Definition at line 62 of file relative_localization_filter.c.

Referenced by range_msg_callback(), relative_localization_filter_init(), relative_localization_filter_periodic(), and send_relative_localization_data().

uint8_t pprzmsg_cnt

Definition at line 64 of file relative_localization_filter.c.

Referenced by relative_localization_filter_init(), and send_relative_localization_data().

float range_array[RELATIVE_LOCALIZATION_N_UAVS]

Definition at line 63 of file relative_localization_filter.c.

Referenced by range_msg_callback(), and send_relative_localization_data().

abi_event range_communication_event

static

Definition at line 66 of file relative_localization_filter.c.