From 1bd20bd296d33ecea5f237f3e4b492db70e52e16 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 26 Jun 2017 13:34:51 +0200 Subject: [PATCH] [module] add support for Decawave DW1000 modules 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. --- conf/modules/dw1000_arduino.xml | 43 +++ sw/airborne/math/pprz_algebra_float.h | 9 + sw/airborne/modules/decawave/dw1000_arduino.c | 313 ++++++++++++++++++ sw/airborne/modules/decawave/dw1000_arduino.h | 40 +++ sw/airborne/modules/decawave/trilateration.c | 115 +++++++ sw/airborne/modules/decawave/trilateration.h | 57 ++++ sw/airborne/subsystems/abi_sender_ids.h | 4 + 7 files changed, 581 insertions(+) create mode 100644 conf/modules/dw1000_arduino.xml create mode 100644 sw/airborne/modules/decawave/dw1000_arduino.c create mode 100644 sw/airborne/modules/decawave/dw1000_arduino.h create mode 100644 sw/airborne/modules/decawave/trilateration.c create mode 100644 sw/airborne/modules/decawave/trilateration.h diff --git a/conf/modules/dw1000_arduino.xml b/conf/modules/dw1000_arduino.xml new file mode 100644 index 00000000000..be04aff3438 --- /dev/null +++ b/conf/modules/dw1000_arduino.xml @@ -0,0 +1,43 @@ + + + + + + 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. + + + +
+ + + + + + + + +
+ +
+ +
+ + + + + + + + + + + + + + + + diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 76e425896a4..6b7c6fd66c7 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -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 diff --git a/sw/airborne/modules/decawave/dw1000_arduino.c b/sw/airborne/modules/decawave/dw1000_arduino.c new file mode 100644 index 00000000000..c9d51dd7550 --- /dev/null +++ b/sw/airborne/modules/decawave/dw1000_arduino.c @@ -0,0 +1,313 @@ +/* + * Copyright (C) 2017 Gautier Hattenberger + * + * 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 + * . + */ +/** + * @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 +#include +#include + +/** 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]; ///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; + } + } +} + + diff --git a/sw/airborne/modules/decawave/dw1000_arduino.h b/sw/airborne/modules/decawave/dw1000_arduino.h new file mode 100644 index 00000000000..fd2e6076e8f --- /dev/null +++ b/sw/airborne/modules/decawave/dw1000_arduino.h @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2017 Gautier Hattenberger + * + * 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 + * . + */ +/** + * @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 + diff --git a/sw/airborne/modules/decawave/trilateration.c b/sw/airborne/modules/decawave/trilateration.c new file mode 100644 index 00000000000..c2a796333ea --- /dev/null +++ b/sw/airborne/modules/decawave/trilateration.c @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2017 Gautier Hattenberger + * + * 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 + * . + */ +/** + * @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; +} + diff --git a/sw/airborne/modules/decawave/trilateration.h b/sw/airborne/modules/decawave/trilateration.h new file mode 100644 index 00000000000..c96a2861a54 --- /dev/null +++ b/sw/airborne/modules/decawave/trilateration.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2017 Gautier Hattenberger + * + * 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 + * . + */ +/** + * @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 + + diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 7d57a15fb40..3954a90435b 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -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) */