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) */