mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[module] add support for Decawave DW1000 modules (#2084)
Implement trilateration algorithm for positionning using the UltraWideBand modules DW1000 from Decawave. The interface with the modules is done with an arduino using the low level library https://github.com/thotro/arduino-dw1000 with some modifications. The data extraction and trilateration are working, calibration and real flight have not been done at the moment.
This commit is contained in:
Gautier Hattenberger
committed by
Michal Podhradsky
Michal Podhradsky
parent
676358ee6e
commit
2aabe921b3
@@ -0,0 +1,43 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="dw1000_arduino" dir="decawave">
|
||||
<doc>
|
||||
<description>
|
||||
Driver to get ranging data from Decawave DW1000 modules connected to Arduino
|
||||
|
||||
Decawave DW1000 modules (http://www.decawave.com/products/dwm1000-module) are Ultra-Wide-Band devices that can be used for communication and ranging.
|
||||
Especially, using 3 modules as anchors can provide data for a localization system based on trilateration.
|
||||
The DW1000 is using a SPI connection, but an arduino-compatible board can be used with the library https://github.com/thotro/arduino-dw1000 to hyde the low level drivers and provide direct ranging informations.
|
||||
</description>
|
||||
<configure name="DW1000_ARDUINO_UART" value="UARTX" description="UART on which arduino and its DW1000 module is connected"/>
|
||||
<configure name="DW1000_ARDUINO_BAUD" value="B115200" description="UART Baudrate, default to 115200"/>
|
||||
<section name="DW1000" prefix="DW1000_">
|
||||
<define name="ANCHORS_IDS" value="1, 2, 3" type="int[]" description="Comma separated list of anchors ID"/>
|
||||
<define name="ANCHORS_POS_X" value="0., 0., 5." type="float[]" description="Comma separated list of anchors ID over X axis"/>
|
||||
<define name="ANCHORS_POS_Y" value="0., 5., 0." type="float[]" description="Comma separated list of anchors ID over Y axis"/>
|
||||
<define name="ANCHORS_POS_Z" value="0., 0., 0." type="float[]" description="Comma separated list of anchors ID over Z axis"/>
|
||||
<define name="OFFSET" value="0., 0., 0." type="float[]" description="Position offset other X, Y and Z axis"/>
|
||||
<define name="SCALE" value="1., 1., 1." type="float[]" description="Position scale factor other X, Y and Z axis"/>
|
||||
<define name="INITIAL_HEADING" value="0." description="Initial heading correction between anchors frame and global frame"/>
|
||||
<define name="NB_ANCHORS" value="3" description="Set number of anchors, only 3 are required/supported at the moment"/>
|
||||
</section>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="dw1000_arduino.h"/>
|
||||
</header>
|
||||
<init fun="dw1000_arduino_init()"/>
|
||||
<periodic fun="dw1000_arduino_periodic()" freq="10"/>
|
||||
<periodic fun="dw1000_arduino_report()" freq="10" autorun="FALSE"/>
|
||||
<periodic fun="dw1000_reset_heading_ref()" freq="1" autorun="FALSE"/>
|
||||
<event fun="dw1000_arduino_event()"/>
|
||||
<makefile>
|
||||
<configure name="DW1000_ARDUINO_UART" case="upper|lower"/>
|
||||
<configure name="DW1000_ARDUINO_BAUD" default="B115200"/>
|
||||
<file name="dw1000_arduino.c"/>
|
||||
<file name="trilateration.c"/>
|
||||
<define name="USE_$(DW1000_ARDUINO_UART_UPPER)"/>
|
||||
<define name="DW1000_ARDUINO_DEV" value="$(DW1000_ARDUINO_UART_LOWER)"/>
|
||||
<define name="$(DW1000_ARDUINO_UART_UPPER)_BAUD" value="$(DW1000_ARDUINO_BAUD)"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -583,6 +583,15 @@ static inline void float_vect_scale(float *a, const float s, const int n)
|
||||
for (i = 0; i < n; i++) { a[i] *= s; }
|
||||
}
|
||||
|
||||
/** a.b */
|
||||
static inline float float_vect_dot_product(const float *a, const float *b, const int n)
|
||||
{
|
||||
int i;
|
||||
float dot = 0.f;
|
||||
for (i = 0; i < n; i++) { dot += a[i] * b[i]; }
|
||||
return dot;
|
||||
}
|
||||
|
||||
//
|
||||
//
|
||||
// Generic matrix algebra
|
||||
|
||||
@@ -0,0 +1,313 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/decawave/dw1000_arduino.c"
|
||||
* @author Gautier Hattenberger
|
||||
* Driver to get ranging data from Decawave DW1000 modules connected to Arduino
|
||||
*/
|
||||
|
||||
#include "modules/decawave/dw1000_arduino.h"
|
||||
|
||||
#include "std.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "pprzlink/messages.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "modules/decawave/trilateration.h"
|
||||
#include "subsystems/gps.h"
|
||||
#include "state.h"
|
||||
#include "generated/flight_plan.h"
|
||||
#include "generated/airframe.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
/** 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
|
||||
*/
|
||||
#ifndef DW1000_NB_ANCHORS
|
||||
#define DW1000_NB_ANCHORS 3
|
||||
#endif
|
||||
|
||||
/** default offset, applied to final result not to individual distances */
|
||||
#ifndef DW1000_OFFSET
|
||||
#define DW1000_OFFSET { 0.f, 0.f, 0.f }
|
||||
#endif
|
||||
|
||||
/** default scale factor, applied to final result not to individual distances */
|
||||
#ifndef DW1000_SCALE
|
||||
#define DW1000_SCALE { 1.f, 1.f, 1.f }
|
||||
#endif
|
||||
|
||||
/** default initial heading correction between anchors frame and global frame */
|
||||
#ifndef DW1000_INITIAL_HEADING
|
||||
#define DW1000_INITIAL_HEADING 0.f
|
||||
#endif
|
||||
|
||||
/** default timeout (in ms) */
|
||||
#ifndef DW1000_TIMEOUT
|
||||
#define DW1000_TIMEOUT 500
|
||||
#endif
|
||||
|
||||
/** frame sync byte */
|
||||
#define DW_STX 0xFE
|
||||
|
||||
/** Parsing states */
|
||||
#define DW_WAIT_STX 0
|
||||
#define DW_GET_DATA 1
|
||||
#define DW_GET_CK 2
|
||||
#define DW_NB_DATA 6
|
||||
|
||||
/** DW1000 positionning system structure */
|
||||
struct DW1000 {
|
||||
uint8_t buf[DW_NB_DATA]; ///< incoming data buffer
|
||||
uint8_t idx; ///< buffer index
|
||||
uint8_t ck; ///< checksum
|
||||
uint8_t state; ///< parser state
|
||||
float initial_heading; ///< initial heading correction
|
||||
struct Anchor anchors[DW1000_NB_ANCHORS]; ///<anchors data
|
||||
struct EnuCoor_f pos; ///< local pos in anchors frame
|
||||
struct EnuCoor_f raw_pos; ///< unscaled local pos in anchors frame
|
||||
struct GpsState gps_dw1000; ///< "fake" gps structure
|
||||
struct LtpDef_i ltp_def; ///< ltp reference
|
||||
bool updated; ///< new anchor data available
|
||||
};
|
||||
|
||||
static struct DW1000 dw1000;
|
||||
|
||||
|
||||
/** Utility function to get float from buffer */
|
||||
static inline float float_from_buf(uint8_t* b) {
|
||||
float f;
|
||||
memcpy((uint8_t*)(&f), b, sizeof(float));
|
||||
return f;
|
||||
}
|
||||
|
||||
/** Utility function to get uint16_t from buffer */
|
||||
static inline uint16_t uint16_from_buf(uint8_t* b) {
|
||||
uint16_t u16;
|
||||
memcpy ((uint8_t*)(&u16), b, sizeof(uint16_t));
|
||||
return u16;
|
||||
}
|
||||
|
||||
/** Utility function to fill anchor from buffer */
|
||||
static void fill_anchor(struct DW1000 *dw) {
|
||||
for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
|
||||
uint16_t id = uint16_from_buf(dw->buf);
|
||||
if (dw->anchors[i].id == id) {
|
||||
dw->anchors[i].distance = float_from_buf(dw->buf + 2);
|
||||
dw->anchors[i].time = get_sys_time_float();
|
||||
dw->updated = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** Data parsing function */
|
||||
static void dw1000_arduino_parse(struct DW1000 *dw, uint8_t c)
|
||||
{
|
||||
switch (dw->state) {
|
||||
|
||||
case DW_WAIT_STX:
|
||||
/* Waiting Synchro */
|
||||
if (c == DW_STX) {
|
||||
dw->idx = 0;
|
||||
dw->ck = 0;
|
||||
dw->state = DW_GET_DATA;
|
||||
}
|
||||
break;
|
||||
|
||||
case DW_GET_DATA:
|
||||
/* Read Bytes */
|
||||
dw->buf[dw->idx++] = c;
|
||||
dw->ck += c;
|
||||
if (dw->idx == DW_NB_DATA) {
|
||||
dw->state = DW_GET_CK;
|
||||
}
|
||||
break;
|
||||
|
||||
case DW_GET_CK:
|
||||
/* Checksum */
|
||||
if (dw->ck == c) {
|
||||
fill_anchor(dw);
|
||||
}
|
||||
dw->state = DW_WAIT_STX;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void send_gps_dw1000_small(struct DW1000 *dw)
|
||||
{
|
||||
// rotate and convert to cm integer
|
||||
float x = dw->pos.x * cosf(dw->initial_heading) - dw->pos.y * sinf(dw->initial_heading);
|
||||
float y = dw->pos.x * sinf(dw->initial_heading) + dw->pos.y * cosf(dw->initial_heading);
|
||||
struct EnuCoor_i enu_pos;
|
||||
enu_pos.x = (int32_t) (x * 100);
|
||||
enu_pos.y = (int32_t) (y * 100);
|
||||
enu_pos.z = (int32_t) (dw->pos.z * 100);
|
||||
|
||||
// Convert the ENU coordinates to ECEF
|
||||
ecef_of_enu_point_i(&(dw->gps_dw1000.ecef_pos), &(dw->ltp_def), &enu_pos);
|
||||
SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_POS_ECEF_BIT);
|
||||
|
||||
lla_of_ecef_i(&(dw->gps_dw1000.lla_pos), &(dw->gps_dw1000.ecef_pos));
|
||||
SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_POS_LLA_BIT);
|
||||
|
||||
dw->gps_dw1000.hmsl = dw->ltp_def.hmsl + enu_pos.z * 10;
|
||||
SetBit(dw->gps_dw1000.valid_fields, GPS_VALID_HMSL_BIT);
|
||||
|
||||
dw->gps_dw1000.num_sv = 7;
|
||||
dw->gps_dw1000.tow = get_sys_time_msec();
|
||||
dw->gps_dw1000.fix = GPS_FIX_3D; // set 3D fix to true
|
||||
|
||||
// set gps msg time
|
||||
dw->gps_dw1000.last_msg_ticks = sys_time.nb_sec_rem;
|
||||
dw->gps_dw1000.last_msg_time = sys_time.nb_sec;
|
||||
dw->gps_dw1000.last_3dfix_ticks = sys_time.nb_sec_rem;
|
||||
dw->gps_dw1000.last_3dfix_time = sys_time.nb_sec;
|
||||
|
||||
// publish new GPS data
|
||||
uint32_t now_ts = get_sys_time_usec();
|
||||
AbiSendMsgGPS(GPS_DW1000_ID, now_ts, &(dw->gps_dw1000));
|
||||
}
|
||||
|
||||
void dw1000_reset_heading_ref(void)
|
||||
{
|
||||
// store current heading as ref and stop periodic call
|
||||
dw1000.initial_heading = stateGetNedToBodyEulers_f()->psi;
|
||||
dw1000_arduino_dw1000_reset_heading_ref_status = MODULES_STOP;
|
||||
}
|
||||
|
||||
/// init arrays from airframe file
|
||||
static const uint16_t ids[] = DW1000_ANCHORS_IDS;
|
||||
static const float pos_x[] = DW1000_ANCHORS_POS_X;
|
||||
static const float pos_y[] = DW1000_ANCHORS_POS_Y;
|
||||
static const float pos_z[] = DW1000_ANCHORS_POS_Z;
|
||||
static const float offset[] = DW1000_OFFSET;
|
||||
static const float scale[] = DW1000_SCALE;
|
||||
|
||||
static void scale_position(struct DW1000 *dw)
|
||||
{
|
||||
dw->pos.x = scale[0] * (dw->raw_pos.x - offset[0]);
|
||||
dw->pos.y = scale[1] * (dw->raw_pos.y - offset[1]);
|
||||
dw->pos.z = scale[2] * (dw->raw_pos.z - offset[2]);
|
||||
}
|
||||
|
||||
void dw1000_arduino_init()
|
||||
{
|
||||
// init DW1000 structure
|
||||
dw1000.idx = 0;
|
||||
dw1000.ck = 0;
|
||||
dw1000.state = DW_WAIT_STX;
|
||||
dw1000.initial_heading = DW1000_INITIAL_HEADING;
|
||||
dw1000.pos.x = 0.f;
|
||||
dw1000.pos.y = 0.f;
|
||||
dw1000.pos.z = 0.f;
|
||||
dw1000.updated = false;
|
||||
for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
|
||||
dw1000.anchors[i].distance = 0.f;
|
||||
dw1000.anchors[i].time = 0.f;
|
||||
dw1000.anchors[i].id = ids[i];
|
||||
dw1000.anchors[i].pos.x = pos_x[i];
|
||||
dw1000.anchors[i].pos.y = pos_y[i];
|
||||
dw1000.anchors[i].pos.z = pos_z[i];
|
||||
}
|
||||
|
||||
// gps structure init
|
||||
dw1000.gps_dw1000.fix = GPS_FIX_NONE;
|
||||
dw1000.gps_dw1000.pdop = 0;
|
||||
dw1000.gps_dw1000.sacc = 0;
|
||||
dw1000.gps_dw1000.pacc = 0;
|
||||
dw1000.gps_dw1000.cacc = 0;
|
||||
dw1000.gps_dw1000.comp_id = GPS_DW1000_ID;
|
||||
|
||||
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
|
||||
llh_nav0.lat = NAV_LAT0;
|
||||
llh_nav0.lon = NAV_LON0;
|
||||
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
|
||||
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
|
||||
ltp_def_from_lla_i(&dw1000.ltp_def, &llh_nav0);
|
||||
|
||||
// init trilateration algorithm
|
||||
trilateration_init(dw1000.anchors);
|
||||
|
||||
}
|
||||
|
||||
void dw1000_arduino_periodic()
|
||||
{
|
||||
// Check for timeout
|
||||
gps_periodic_check(&(dw1000.gps_dw1000));
|
||||
}
|
||||
|
||||
void dw1000_arduino_report()
|
||||
{
|
||||
float buf[9];
|
||||
buf[0] = dw1000.anchors[0].distance;
|
||||
buf[1] = dw1000.anchors[1].distance;
|
||||
buf[2] = dw1000.anchors[2].distance;
|
||||
buf[3] = dw1000.raw_pos.x;
|
||||
buf[4] = dw1000.raw_pos.y;
|
||||
buf[5] = dw1000.raw_pos.z;
|
||||
buf[6] = dw1000.pos.x;
|
||||
buf[7] = dw1000.pos.y;
|
||||
buf[8] = dw1000.pos.z;
|
||||
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 9, buf);
|
||||
}
|
||||
|
||||
/** check timeout for each anchor
|
||||
* @return true if one has reach timeout
|
||||
*/
|
||||
static bool check_anchor_timeout(struct DW1000 *dw)
|
||||
{
|
||||
const float now = get_sys_time_float();
|
||||
const float timeout = (float)DW1000_TIMEOUT / 1000.;
|
||||
for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
|
||||
if (now - dw->anchors[i].time > timeout) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void dw1000_arduino_event()
|
||||
{
|
||||
// Look for data on serial link and send to parser
|
||||
while (uart_char_available(&DW1000_ARDUINO_DEV)) {
|
||||
uint8_t ch = uart_getch(&DW1000_ARDUINO_DEV);
|
||||
dw1000_arduino_parse(&dw1000, ch);
|
||||
// process if new data
|
||||
if (dw1000.updated) {
|
||||
// if no timeout on anchors, run trilateration algorithm
|
||||
if (check_anchor_timeout(&dw1000) == false &&
|
||||
trilateration_compute(dw1000.anchors, &dw1000.raw_pos) == 0) {
|
||||
// apply scale and neutral corrections
|
||||
scale_position(&dw1000);
|
||||
// send fake GPS message for INS filters
|
||||
send_gps_dw1000_small(&dw1000);
|
||||
}
|
||||
dw1000.updated = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,40 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/decawave/dw1000_arduino.h"
|
||||
* @author Gautier Hattenberger
|
||||
* Driver to get ranging data from Decawave DW1000 modules connected to Arduino
|
||||
*/
|
||||
|
||||
#ifndef DW1000_ARDUINO_H
|
||||
#define DW1000_ARDUINO_H
|
||||
|
||||
extern void dw1000_arduino_init(void);
|
||||
extern void dw1000_arduino_periodic(void);
|
||||
extern void dw1000_arduino_report(void);
|
||||
extern void dw1000_arduino_event(void);
|
||||
|
||||
/** Reset reference heading to current heading
|
||||
* AHRS/INS should be aligned before calling this function
|
||||
*/
|
||||
extern void dw1000_reset_heading_ref(void);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,115 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/decawave/trilateration.c"
|
||||
* @author Gautier Hattenberger
|
||||
* Trilateration algorithm
|
||||
* https://en.wikipedia.org/wiki/Trilateration
|
||||
*/
|
||||
|
||||
#include "trilateration.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
// base original locations
|
||||
static float P[3][3];
|
||||
// base unit vector on x, y and z axis
|
||||
static float Ex[3], Ey[3], Ez[3];
|
||||
// base inter distances
|
||||
static float D, I, J;
|
||||
|
||||
bool init_failed;
|
||||
|
||||
int trilateration_init(struct Anchor *anchors)
|
||||
{
|
||||
float tmp[3], n;
|
||||
init_failed = false;
|
||||
|
||||
// store original points
|
||||
for (int i = 0; i < 3; i++) {
|
||||
P[i][0] = anchors[i].pos.x;
|
||||
P[i][1] = anchors[i].pos.y;
|
||||
P[i][2] = anchors[i].pos.z;
|
||||
}
|
||||
// Ex = (P1 - P0) / ||P1 - P0||
|
||||
float_vect_diff(tmp, P[1], P[0], 3);
|
||||
n = float_vect_norm(tmp, 3);
|
||||
if (n > 0.f) {
|
||||
float_vect_sdiv(Ex, tmp, n, 3);
|
||||
} else {
|
||||
init_failed = true;
|
||||
return -1;
|
||||
}
|
||||
// I = Ex . (P2 - P0)
|
||||
float_vect_diff(tmp, P[2], P[0], 3);
|
||||
I = float_vect_dot_product(Ex, tmp, 3);
|
||||
// Ey = (P2 - P0 - I.Ex) / ||P2 - P0 - I.Ex||
|
||||
float_vect_smul(tmp, Ex, -I, 3);
|
||||
float_vect_diff(tmp, tmp, P[0], 3);
|
||||
float_vect_add(tmp, P[2], 3);
|
||||
n = float_vect_norm(tmp, 3);
|
||||
if (n > 0.f) {
|
||||
float_vect_sdiv(Ey, tmp, n, 3);
|
||||
} else {
|
||||
init_failed = true;
|
||||
return -1;
|
||||
}
|
||||
// Ez = Ex x Ey
|
||||
Ez[0] = Ex[1]*Ey[2] - Ex[2]*Ey[1];
|
||||
Ez[1] = Ex[2]*Ey[0] - Ex[0]*Ey[2];
|
||||
Ez[2] = Ex[0]*Ey[1] - Ex[1]*Ey[0];
|
||||
// D = ||P1 - P0||
|
||||
float_vect_diff(tmp, P[1], P[0], 3);
|
||||
D = float_vect_norm(tmp, 3);
|
||||
// J = Ey . (P2 - P0)
|
||||
float_vect_diff(tmp, P[2], P[0], 3);
|
||||
J = float_vect_dot_product(Ey, tmp, 3);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int trilateration_compute(struct Anchor *anchors, struct EnuCoor_f *pos)
|
||||
{
|
||||
if (init_failed) {
|
||||
return -1;
|
||||
}
|
||||
const float r02 = anchors[0].distance * anchors[0].distance;
|
||||
const float r12 = anchors[1].distance * anchors[1].distance;
|
||||
const float r22 = anchors[2].distance * anchors[2].distance;
|
||||
const float d2 = D * D;
|
||||
const float i2 = I * I;
|
||||
const float j2 = J * J;
|
||||
float tmp[3];
|
||||
tmp[0] = (r02 - r12 + d2) / (2.f * D);
|
||||
tmp[1] = (r02 - r22 + i2 + j2) / (2.f * J) - ((I * tmp[0]) / J);
|
||||
const float d0 = r02 - (tmp[0] * tmp[0]) - (tmp[1] * tmp[1]);
|
||||
if (d0 < 0.f) {
|
||||
// impossible solution
|
||||
// might happen if position of the anchors are not correct
|
||||
// or if reported distances are completely wrong
|
||||
return -1;
|
||||
}
|
||||
tmp[2] = sqrtf(d0); // only keep positive value
|
||||
// restore original frame
|
||||
pos->x = P[0][0] + tmp[0] * Ex[0] + tmp[1] * Ey[0] + tmp[2] * Ez[0];
|
||||
pos->y = P[0][1] + tmp[0] * Ex[1] + tmp[1] * Ey[1] + tmp[2] * Ez[1];
|
||||
pos->z = P[0][2] + tmp[0] * Ex[2] + tmp[1] * Ey[2] + tmp[2] * Ez[2];
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,57 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/decawave/trilateration.h"
|
||||
* @author Gautier Hattenberger
|
||||
* Trilateration algorithm
|
||||
* https://en.wikipedia.org/wiki/Trilateration
|
||||
*/
|
||||
|
||||
#ifndef TRILATERATION_H
|
||||
#define TRILATERATION_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
|
||||
/** Anchor structure */
|
||||
struct Anchor {
|
||||
float distance; ///< last measured distance
|
||||
float time; ///< time of the last received data
|
||||
struct EnuCoor_f pos; ///< position of the anchor
|
||||
uint16_t id; ///< anchor ID
|
||||
};
|
||||
|
||||
/** Init internal trilateration structures
|
||||
*
|
||||
* @param[in] anchors array of anchors with their location
|
||||
*/
|
||||
extern int trilateration_init(struct Anchor *anchors);
|
||||
|
||||
/** Compute trilateration based on the latest measurments
|
||||
*
|
||||
* @param[in] anchors array of anchors with updated distance measurements
|
||||
* @param[out] pos computed position
|
||||
* @return error status (0 for valid position)
|
||||
*/
|
||||
extern int trilateration_compute(struct Anchor *anchors, struct EnuCoor_f *pos);
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -194,6 +194,10 @@
|
||||
#define GPS_IMCU_ID 14
|
||||
#endif
|
||||
|
||||
#ifndef GPS_DW1000_ID
|
||||
#define GPS_DW1000_ID 15
|
||||
#endif
|
||||
|
||||
/*
|
||||
* IDs of IMU sensors (accel, gyro)
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user