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; }
|
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
|
// 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
|
#define GPS_IMCU_ID 14
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef GPS_DW1000_ID
|
||||||
|
#define GPS_DW1000_ID 15
|
||||||
|
#endif
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* IDs of IMU sensors (accel, gyro)
|
* IDs of IMU sensors (accel, gyro)
|
||||||
*/
|
*/
|
||||||
|
|||||||
Reference in New Issue
Block a user