Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
relative_localization_filter.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Mario Coppola
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
28 #include "state.h" // To get current states
29 
31 #include "modules/core/abi.h"
32 
33 #include "math/pprz_algebra.h"
34 #include "math/pprz_algebra_int.h"
35 
36 #include <math.h>
37 #include <stdio.h>
38 #include <stdlib.h>
39 
40 #ifndef RELATIVE_LOCALIZATION_N_UAVS
41 #define RELATIVE_LOCALIZATION_N_UAVS 4 // Maximum expected number of other UAVs
42 #endif
43 
44 /*
45  * RELATIVE_LOCALIZATION_NO_NORTH = 1 : The filter runs without a heading reference.
46  * RELATIVE_LOCALIZATION_NO_NORTH = 0 : The filter runs while using a shared reference heading.
47  */
48 #ifndef RELATIVE_LOCALIZATION_NO_NORTH
49 #define RELATIVE_LOCALIZATION_NO_NORTH 1
50 #endif
51 
52 #if RELATIVE_LOCALIZATION_NO_NORTH
53 #include "discrete_ekf_no_north.h"
55 #else
56 #include "discrete_ekf.h"
58 #endif
59 
60 int32_t id_array[RELATIVE_LOCALIZATION_N_UAVS]; // array of UWB IDs of all drones
62 uint8_t number_filters; // the number of filters running in parallel
63 float range_array[RELATIVE_LOCALIZATION_N_UAVS]; // an array to store the ranges at which the other MAVs are
64 uint8_t pprzmsg_cnt; // a counter to send paparazzi messages, which are sent in rotation
65 
67 static void range_msg_callback(uint8_t sender_id __attribute__((unused)), uint8_t ac_id,
68  float range, float trackedVx, float trackedVy, float trackedh,
69  float trackedAx, float trackedAy, float trackedYawr)
70 {
71  int idx = -1; // Initialize the index of all tracked drones (-1 for null assumption of no drone found)
72 
73  // Check if a new aircraft ID is present, if it's a new ID we start a new EKF for it.
77 #if RELATIVE_LOCALIZATION_NO_NORTH
79 #else
81 #endif
83  } else if (idx != -1) {
84  range_array[idx] = range;
85  ekf_rl[idx].dt = (get_sys_time_usec() - latest_update_time[idx]) / pow(10, 6); // Update the time between messages
86 
87  float ownVx = stateGetSpeedNed_f()->x;
88  float ownVy = stateGetSpeedNed_f()->y;
89  float ownh = stateGetPositionEnu_f()->z;
90 #if RELATIVE_LOCALIZATION_NO_NORTH
91  float ownAx = stateGetAccelNed_f()->x;
92  float ownAy = stateGetAccelNed_f()->y;
93  float ownYawr = stateGetBodyRates_f()->r;
94  float U[EKF_L] = {ownAx, ownAy, trackedAx, trackedAy, ownYawr, trackedYawr};
95  float Z[EKF_M] = {range, ownh, trackedh, ownVx, ownVy, trackedVx, trackedVy};
98 #else
99  // Measurement Vector Z = [range owvVx(NED) ownVy(NED) tracked_v_north(NED) tracked_v_east(NED) dh]
100  float Z[EKF_M] = {range, ownVx, ownVy, trackedVx, trackedVy, trackedh - ownh};
103 #endif
104 
105  }
106 
108 };
109 
110 // It is best to send the data of each tracked drone separately to avoid overloading
111 static void send_relative_localization_data(struct transport_tx *trans, struct link_device *dev)
112 {
113  pprzmsg_cnt++;
114  if (pprzmsg_cnt >= number_filters) {
115  pprzmsg_cnt = 0;
116  }
117 
118  if (number_filters > 0) {
119  pprz_msg_send_RLFILTER(trans, dev, AC_ID,
121  &ekf_rl[pprzmsg_cnt].X[0], &ekf_rl[pprzmsg_cnt].X[1], // x y (tracked wrt own)
122  &ekf_rl[pprzmsg_cnt].X[2], &ekf_rl[pprzmsg_cnt].X[3], // vx vy (own)
123  &ekf_rl[pprzmsg_cnt].X[4], &ekf_rl[pprzmsg_cnt].X[5], // vx vy (tracked)
124  &ekf_rl[pprzmsg_cnt].X[6]); // height separation
125  }
126 };
127 
129 {
131  RELATIVE_LOCALIZATION_N_UAVS); // The id_array is initialized with non-existant IDs (assuming UWB IDs are 0,1,2...)
132  number_filters = 0;
133  pprzmsg_cnt = 0;
134 
135  AbiBindMsgUWB_COMMUNICATION(UWB_COMM_ID, &range_communication_event, range_msg_callback);
137 };
138 
140 {
141  for (int i = 0; i < number_filters; i++) {
142  // send id, x, y, z, vx, vy, vz(=0)
143  AbiSendMsgRELATIVE_LOCALIZATION(RELATIVE_LOCALIZATION_ID, id_array[i], ekf_rl[i].X[0], ekf_rl[i].X[1], ekf_rl[i].X[6],
144  ekf_rl[i].X[4], ekf_rl[i].X[5], 0.f);
145  }
146 };
discrete_ekf_no_north.h
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
discrete_ekf.h
discrete_ekf_predict
void discrete_ekf_predict(struct discrete_ekf *filter)
Definition: discrete_ekf.c:74
abi.h
RELATIVE_LOCALIZATION_ID
#define RELATIVE_LOCALIZATION_ID
Definition: abi_sender_ids.h:505
stateGetAccelNed_f
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
id_array
int32_t id_array[RELATIVE_LOCALIZATION_N_UAVS]
Definition: relative_localization_filter.c:60
range_msg_callback
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)
Definition: relative_localization_filter.c:67
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
send_relative_localization_data
static void send_relative_localization_data(struct transport_tx *trans, struct link_device *dev)
Definition: relative_localization_filter.c:111
pprzmsg_cnt
uint8_t pprzmsg_cnt
Definition: relative_localization_filter.c:64
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
range_communication_event
static abi_event range_communication_event
Definition: relative_localization_filter.c:66
mesonh.mesonh_atmosphere.Z
int Z
Definition: mesonh_atmosphere.py:45
EKF_L
#define EKF_L
Definition: discrete_ekf_no_north.h:38
extra_pprz_dl.h
idx
static uint32_t idx
Definition: nps_radio_control_spektrum.c:105
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
pprz_algebra_int.h
Paparazzi fixed point algebra.
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
discrete_ekf_no_north::dt
float dt
Definition: discrete_ekf_no_north.h:59
telemetry.h
stateGetSpeedNed_f
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
discrete_ekf_update
void discrete_ekf_update(struct discrete_ekf *filter, float *Z)
Definition: discrete_ekf.c:113
relative_localization_filter_init
void relative_localization_filter_init(void)
Definition: relative_localization_filter.c:128
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
discrete_ekf_no_north
Definition: discrete_ekf_no_north.h:40
number_filters
uint8_t number_filters
Definition: relative_localization_filter.c:62
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
int32_vect_find
static bool int32_vect_find(const int32_t *a, const int32_t s, int *loc, const int n)
Find value s in array a.
Definition: pprz_algebra_int.h:683
range_array
float range_array[RELATIVE_LOCALIZATION_N_UAVS]
Definition: relative_localization_filter.c:63
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
EKF_M
#define EKF_M
Definition: discrete_ekf.h:34
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
ekf_rl
struct discrete_ekf_no_north ekf_rl[RELATIVE_LOCALIZATION_N_UAVS]
Definition: relative_localization_filter.c:54
RELATIVE_LOCALIZATION_N_UAVS
#define RELATIVE_LOCALIZATION_N_UAVS
Definition: relative_localization_filter.c:41
relative_localization_filter.h
pprz_algebra.h
Paparazzi generic algebra macros.
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
relative_localization_filter_periodic
void relative_localization_filter_periodic(void)
Definition: relative_localization_filter.c:139
discrete_ekf_no_north_predict
void discrete_ekf_no_north_predict(struct discrete_ekf_no_north *filter, float *U)
Definition: discrete_ekf_no_north.c:219
state.h
discrete_ekf
Definition: discrete_ekf.h:36
ac_id
uint8_t ac_id
Definition: sim_ap.c:47
int32_vect_set_value
static void int32_vect_set_value(int32_t *a, const int32_t v, const int n)
a = v * ones(n,1)
Definition: pprz_algebra_int.h:624
discrete_ekf_no_north_update
void discrete_ekf_no_north_update(struct discrete_ekf_no_north *filter, float *Z)
Definition: discrete_ekf_no_north.c:270
latest_update_time
uint32_t latest_update_time[RELATIVE_LOCALIZATION_N_UAVS]
Definition: relative_localization_filter.c:61
UWB_COMM_ID
#define UWB_COMM_ID
Definition: abi_sender_ids.h:454
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
discrete_ekf_new
void discrete_ekf_new(struct discrete_ekf *filter)
Definition: discrete_ekf.c:32
mesonh.mesonh_atmosphere.X
int X
Definition: mesonh_atmosphere.py:43
discrete_ekf_no_north_new
void discrete_ekf_no_north_new(struct discrete_ekf_no_north *filter)
Definition: discrete_ekf_no_north.c:192