Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/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
80  discrete_ekf_new(&ekf_rl[number_filters]);
81 #endif
82  number_filters++;
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};
102  discrete_ekf_update(&ekf_rl[idx], Z);
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,
120  &id_array[pprzmsg_cnt], &range_array[pprzmsg_cnt],
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 };
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
uint8_t ac_id
Definition: sim_ap.c:45
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 uint32_t idx
float range_array[RELATIVE_LOCALIZATION_N_UAVS]
Extra datalink and telemetry using PPRZ protocol.
static void send_relative_localization_data(struct transport_tx *trans, struct link_device *dev)
float y
in meters
void discrete_ekf_no_north_predict(struct discrete_ekf_no_north *filter, float *U)
#define RELATIVE_LOCALIZATION_N_UAVS
Periodic telemetry system header (includes downlink utility and generated code).
void discrete_ekf_predict(struct discrete_ekf *filter)
Definition: discrete_ekf.c:74
float r
in rad/s
Main include for ABI (AirBorneInterface).
void relative_localization_filter_periodic(void)
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
void relative_localization_filter_init(void)
#define UWB_COMM_ID
void discrete_ekf_update(struct discrete_ekf *filter, float *Z)
Definition: discrete_ekf.c:113
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
void discrete_ekf_no_north_update(struct discrete_ekf_no_north *filter, float *Z)
uint8_t pprzmsg_cnt
Paparazzi generic algebra macros.
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
uint8_t number_filters
unsigned long uint32_t
Definition: types.h:18
void discrete_ekf_no_north_new(struct discrete_ekf_no_north *filter)
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
#define EKF_L
#define EKF_M
Definition: discrete_ekf.h:34
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
signed long int32_t
Definition: types.h:19
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define RELATIVE_LOCALIZATION_ID
int32_t id_array[RELATIVE_LOCALIZATION_N_UAVS]
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static bool int32_vect_find(const int32_t *a, const int32_t s, int *loc, const int n)
Find value s in array a.
float z
in meters
static abi_event range_communication_event
struct discrete_ekf_no_north ekf_rl[RELATIVE_LOCALIZATION_N_UAVS]
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
void discrete_ekf_new(struct discrete_ekf *filter)
Definition: discrete_ekf.c:32
static void int32_vect_set_value(int32_t *a, const int32_t v, const int n)
a = v * ones(n,1)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
Paparazzi fixed point algebra.
float x
in meters
uint32_t latest_update_time[RELATIVE_LOCALIZATION_N_UAVS]