mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-10-02 12:23:17 +08:00
Decawave positioning refactor (#3660)
Doxygen / build (push) Has been cancelled
Doxygen / build (push) Has been cancelled
* [decawave] split driver from filter * [decawave] rename dw1000_arduino to uwb_positioning * [decawave] rename dw1000_arduino_periph to dw1000_range_arduino * [decawave] Add pdec driver. --------- Co-authored-by: Fabien-B <Fabien-B@github.com> Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>
This commit is contained in:
Fabien-B
co-authored by
Fabien-B
Gautier Hattenberger
Fabien-B
Gautier Hattenberger
parent
fa97bcc52d
commit
134bc165d2
@@ -281,6 +281,21 @@
|
||||
<field name="values" type="int32_t *">Pointer to force sensor values (driver raw units)</field>
|
||||
</message>
|
||||
|
||||
<message name="UWB_RANGING" id="43">
|
||||
<field name="stamp" type="uint32_t" unit="us">Timestamp in micro-seconds</field>
|
||||
<field name="src_id" type="uint16_t">Ranging initiator</field>
|
||||
<field name="dst_id" type="uint16_t">Ranging responder</field>
|
||||
<field name="range" type="float" unit="m">Distance between the devices</field>
|
||||
</message>
|
||||
|
||||
<message name="UWB_TDOA" id="44">
|
||||
<field name="stamp" type="uint32_t" unit="us">Timestamp in micro-seconds</field>
|
||||
<field name="blink_id" type="uint16_t"/>
|
||||
<field name="count" type="uint8_t">Number of beacons in the array</field>
|
||||
<field name="src_id" type="uint16_t*">Beacons ids</field>
|
||||
<field name="range_diff" type="float*" unit="m">Range difference with the closest beacon</field>
|
||||
</message>
|
||||
|
||||
</msg_class>
|
||||
|
||||
</protocol>
|
||||
|
||||
@@ -1,87 +0,0 @@
|
||||
<!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.
|
||||
|
||||
See https://hal-enac.archives-ouvertes.fr/hal-01936955 for more information on the EKF filtering.
|
||||
</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"/>
|
||||
<configure name="DW1000_USE_AS_LOCAL_POS" value="FALSE|TRUE" description="use as a local positioning system (default: TRUE)"/>
|
||||
<configure name="DW1000_USE_AS_GPS" value="FALSE|TRUE" description="use as a fake GPS positioning system (default: FALSE)"/>
|
||||
<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"/>
|
||||
<define name="USE_EKF" value="FALSE|TRUE" description="Enable EKF filtering, required to estimate speed"/>
|
||||
<define name="EKF_Q" value="1.0" description="EKF process noise"/>
|
||||
<define name="EKF_R_DIST" value="0.1" description="EKF noise on distance measurements"/>
|
||||
<define name="EKF_R_SPEED" value="0.1" description="EKF noise on speed measurements (if available)"/>
|
||||
<define name="NOISE_X|Y|Z" value="0.1" description="Noise level reported by the POSITION_ESTIMATE message when USE_AS_LOCAL_POS is activated"/>
|
||||
<define name="VEL_NOISE_X|Y|Z" value="0.1" description="Noise level reported by the VELOCITY_ESTIMATE message when USE_AS_LOCAL_POS is activated"/>
|
||||
</section>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="dw1000">
|
||||
<dl_setting max="1" min="0" step="1" module="decawave/dw1000_arduino" var="dw1000_use_ekf" shortname="use_ekf" values="FALSE|TRUE"/>
|
||||
<dl_setting max="5.0" min="0.01" step="0.01" module="decawave/dw1000_arduino" var="dw1000_ekf_q" shortname="q_noise" handler="update_ekf_q"/>
|
||||
<dl_setting max="1.0" min="0.01" step="0.01" module="decawave/dw1000_arduino" var="dw1000_ekf_r_dist" shortname="r_dist" handler="update_ekf_r_dist"/>
|
||||
<dl_setting max="1.0" min="0.01" step="0.01" module="decawave/dw1000_arduino" var="dw1000_ekf_r_speed" shortname="r_speed" handler="update_ekf_r_speed"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<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"/>
|
||||
<configure name="DW1000_USE_AS_LOCAL_POS" default="TRUE"/>
|
||||
<configure name="DW1000_USE_AS_GPS" default="FALSE"/>
|
||||
<file name="dw1000_arduino.c"/>
|
||||
<file name="trilateration.c"/>
|
||||
<file name="ekf_range.c"/>
|
||||
<define name="DW1000_USE_AS_LOCAL_POS" value="$(DW1000_USE_AS_LOCAL_POS)"/>
|
||||
<define name="DW1000_USE_AS_GPS" value="$(DW1000_USE_AS_GPS)"/>
|
||||
<raw>
|
||||
ifeq (,$(findstring $(DW1000_USE_AS_GPS),0 FALSE))
|
||||
ifdef SECONDARY_GPS
|
||||
ifneq (,$(findstring $(SECONDARY_GPS), dw1000))
|
||||
# this is the secondary GPS
|
||||
$(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"modules/decawave/dw1000_arduino.h\"
|
||||
$(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_DW1000
|
||||
else
|
||||
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/decawave/dw1000_arduino.h\"
|
||||
$(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_DW1000
|
||||
endif
|
||||
else
|
||||
# plain old single GPS usage
|
||||
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/decawave/dw1000_arduino.h\"
|
||||
endif
|
||||
endif
|
||||
</raw>
|
||||
</makefile>
|
||||
<makefile target="ap">
|
||||
<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>
|
||||
|
||||
@@ -0,0 +1,37 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
<module name="dw1000_range_arduino" dir="decawave" task="sensors">
|
||||
<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_RANGE_ARDUINO_UART" value="UARTX" description="UART on which arduino and its DW1000 module is connected"/>
|
||||
<configure name="DW1000_RANGE_ARDUINO_BAUD" value="B115200" description="UART Baudrate, default to 115200"/>
|
||||
</doc>
|
||||
<dep>
|
||||
<depends>uart</depends>
|
||||
<provides>ranging</provides>
|
||||
<suggests>uwb_positioning</suggests>
|
||||
</dep>
|
||||
<header>
|
||||
<file name="dw1000_range_arduino.h"/>
|
||||
</header>
|
||||
<init fun="dw1000_range_arduino_init()"/>
|
||||
<event fun="dw1000_range_arduino_event()"/>
|
||||
<makefile target="ap">
|
||||
<configure name="DW1000_RANGE_ARDUINO_UART" case="upper|lower"/>
|
||||
<configure name="DW1000_RANGE_ARDUINO_BAUD" default="B115200"/>
|
||||
<define name="USE_$(DW1000_RANGE_ARDUINO_UART_UPPER)"/>
|
||||
<define name="DW1000_RANGE_ARDUINO_DEV" value="$(DW1000_RANGE_ARDUINO_UART_LOWER)"/>
|
||||
<define name="$(DW1000_RANGE_ARDUINO_UART_UPPER)_BAUD" value="$(DW1000_RANGE_ARDUINO_BAUD)"/>
|
||||
<define name="UWB_POSITIONING_TAG_ID" value="0" />
|
||||
<file name="dw1000_range_arduino.c"/>
|
||||
<test>
|
||||
<define name="DW1000_RANGE_ARDUINO_DEV" value="uart3"/>
|
||||
<define name="USE_UART3"/>
|
||||
<define name="UWB_POSITIONING_TAG_ID" value="0" />
|
||||
</test>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,33 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
<module name="pdec" dir="decawave" task="sensors">
|
||||
<doc>
|
||||
<description>
|
||||
Driver for the Pprz Decawave module, based on the DWM3001C. Allows ranging, as well as TDOA.
|
||||
</description>
|
||||
</doc>
|
||||
<dep>
|
||||
<depends>uart</depends>
|
||||
<provides>ranging</provides>
|
||||
<suggests>uwb_positioning</suggests>
|
||||
</dep>
|
||||
<header>
|
||||
<file name="pdec.h"/>
|
||||
</header>
|
||||
<init fun="pdec_init()"/>
|
||||
<periodic fun="pdec_periodic_report()" freq="10" autorun="FALSE"/>
|
||||
<event fun="pdec_event()"/>
|
||||
<makefile>
|
||||
<configure name="PDEC_UART" default="UART3" case="upper|lower"/>
|
||||
<define name="$(PDEC_UART_UPPER)_BAUD" value="B115200"/>
|
||||
<define name="PDEC_UART_DEV" value="$(PDEC_UART_LOWER)"/>
|
||||
<define name="USE_$(PDEC_UART_UPPER)"/>
|
||||
<file name="pdec.c"/>
|
||||
<test>
|
||||
<define name="PDEC_UART_DEV" value="uart3"/>
|
||||
<define name="USE_UART3"/>
|
||||
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
|
||||
<define name="DOWNLINK_DEVICE" value="uart0"/>
|
||||
<define name="USE_UART0"/>
|
||||
</test>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,80 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="uwb_positioning" dir="decawave">
|
||||
<doc>
|
||||
<description>
|
||||
Estimate local position from UWB anchor measurements.
|
||||
|
||||
UWB devices can be used for communication, ranging, and positioning.
|
||||
This module consumes anchor measurements and estimates a local position.
|
||||
With range measurements, 3 anchors are required/supported by the current trilateration path.
|
||||
|
||||
See https://hal-enac.archives-ouvertes.fr/hal-01936955 for more information on the EKF filtering.
|
||||
</description>
|
||||
<configure name="UWB_POSITIONING_USE_AS_LOCAL_POS" value="FALSE|TRUE" description="use as a local positioning system (default: TRUE)"/>
|
||||
<configure name="UWB_POSITIONING_USE_AS_GPS" value="FALSE|TRUE" description="use as a fake GPS positioning system (default: FALSE)"/>
|
||||
<section name="UWB_POSITIONING" prefix="UWB_POSITIONING_">
|
||||
<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="RANGE_OFFSET" value="0., 0., 0." type="float[]" description="Offset applied to individual range measurements"/>
|
||||
<define name="RANGE_SCALE" value="1., 1., 1." type="float[]" description="Scale factor applied to individual range measurements"/>
|
||||
<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"/>
|
||||
<define name="USE_EKF" value="FALSE|TRUE" description="Enable EKF filtering, required to estimate speed"/>
|
||||
<define name="EKF_Q" value="1.0" description="EKF process noise"/>
|
||||
<define name="EKF_R_DIST" value="0.1" description="EKF noise on distance measurements"/>
|
||||
<define name="EKF_R_SPEED" value="0.1" description="EKF noise on speed measurements (if available)"/>
|
||||
<define name="NOISE_X|Y|Z" value="0.1" description="Noise level reported by the POSITION_ESTIMATE message when USE_AS_LOCAL_POS is activated"/>
|
||||
<define name="VEL_NOISE_X|Y|Z" value="0.1" description="Noise level reported by the VELOCITY_ESTIMATE message when USE_AS_LOCAL_POS is activated"/>
|
||||
</section>
|
||||
</doc>
|
||||
<dep>
|
||||
<depends>@ranging</depends>
|
||||
</dep>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="uwb_positioning">
|
||||
<dl_setting max="1" min="0" step="1" module="decawave/uwb_positioning" var="uwb_positioning_use_ekf" shortname="use_ekf" values="FALSE|TRUE"/>
|
||||
<dl_setting max="5.0" min="0.01" step="0.01" module="decawave/uwb_positioning" var="uwb_positioning_ekf_q" shortname="q_noise" handler="update_ekf_q"/>
|
||||
<dl_setting max="1.0" min="0.01" step="0.01" module="decawave/uwb_positioning" var="uwb_positioning_ekf_r_dist" shortname="r_dist" handler="update_ekf_r_dist"/>
|
||||
<dl_setting max="1.0" min="0.01" step="0.01" module="decawave/uwb_positioning" var="uwb_positioning_ekf_r_speed" shortname="r_speed" handler="update_ekf_r_speed"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="uwb_positioning.h"/>
|
||||
</header>
|
||||
<init fun="uwb_positioning_init()"/>
|
||||
<periodic fun="uwb_positioning_periodic()" freq="10"/>
|
||||
<periodic fun="uwb_positioning_range_periodic()" freq="10"/>
|
||||
<periodic fun="uwb_positioning_report()" freq="10" autorun="FALSE"/>
|
||||
<periodic fun="uwb_positioning_reset_heading_ref()" freq="1" autorun="FALSE"/>
|
||||
<makefile>
|
||||
<configure name="UWB_POSITIONING_USE_AS_LOCAL_POS" default="TRUE"/>
|
||||
<configure name="UWB_POSITIONING_USE_AS_GPS" default="FALSE"/>
|
||||
<file name="uwb_positioning.c"/>
|
||||
<file name="trilateration.c"/>
|
||||
<file name="ekf_range.c"/>
|
||||
<define name="UWB_POSITIONING_USE_AS_LOCAL_POS" value="$(UWB_POSITIONING_USE_AS_LOCAL_POS)"/>
|
||||
<define name="UWB_POSITIONING_USE_AS_GPS" value="$(UWB_POSITIONING_USE_AS_GPS)"/>
|
||||
<raw>
|
||||
ifeq (,$(findstring $(UWB_POSITIONING_USE_AS_GPS),0 FALSE))
|
||||
ifdef SECONDARY_GPS
|
||||
ifneq (,$(findstring $(SECONDARY_GPS), uwb_positioning))
|
||||
# this is the secondary GPS
|
||||
$(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"modules/decawave/uwb_positioning.h\"
|
||||
$(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_UWB
|
||||
else
|
||||
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/decawave/uwb_positioning.h\"
|
||||
$(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_UWB
|
||||
endif
|
||||
else
|
||||
# plain old single GPS usage
|
||||
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/decawave/uwb_positioning.h\"
|
||||
endif
|
||||
endif
|
||||
</raw>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -304,8 +304,8 @@
|
||||
#define GPS_IMCU_ID 14
|
||||
#endif
|
||||
|
||||
#ifndef GPS_DW1000_ID
|
||||
#define GPS_DW1000_ID 15
|
||||
#ifndef GPS_UWB_ID
|
||||
#define GPS_UWB_ID 15
|
||||
#endif
|
||||
|
||||
#ifndef GPS_UBX2_ID
|
||||
@@ -528,12 +528,20 @@
|
||||
#endif
|
||||
|
||||
/*
|
||||
* UWB communication (message 19)
|
||||
* UWB communication and ranging (messages 19, 43, 44)
|
||||
*/
|
||||
#ifndef UWB_COMM_ID
|
||||
#define UWB_COMM_ID 1
|
||||
#endif
|
||||
|
||||
#ifndef UWB_DW1000_ARDUINO_ID
|
||||
#define UWB_DW1000_ARDUINO_ID 2
|
||||
#endif
|
||||
|
||||
|
||||
#ifndef UWB_PDEC_ID
|
||||
#define UWB_PDEC_ID 3
|
||||
#endif
|
||||
/*
|
||||
* IDs of Obstacle detection systems
|
||||
*/
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,121 @@
|
||||
/**
|
||||
* Copyright (C) 2026 Fabien-B <fabien-b@github.com>
|
||||
* This file is part of paparazzi. See LICENCE file.
|
||||
*
|
||||
* @file "modules/decawave/dw1000_range_arduino.c"
|
||||
* @author Fabien-B <fabien-b@github.com>
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "modules/decawave/dw1000_range_arduino.h"
|
||||
|
||||
#include "std.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "modules/core/abi.h"
|
||||
#include "modules/core/abi_sender_ids.h"
|
||||
#include "modules/decawave/trilateration.h"
|
||||
#include "generated/airframe.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
/** 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 Arduino range parser state */
|
||||
struct DW1000RangeArduino {
|
||||
struct uart_periph *dev;
|
||||
uint8_t buf[DW_NB_DATA]; ///< incoming data buffer
|
||||
uint8_t idx; ///< buffer index
|
||||
uint8_t ck; ///< checksum
|
||||
uint8_t state; ///< parser state
|
||||
};
|
||||
|
||||
struct DW1000RangeArduino dw1000_range_arduino;
|
||||
|
||||
|
||||
/** 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;
|
||||
}
|
||||
|
||||
/** Send range data decoded from the serial frame */
|
||||
static void send_anchor_data(struct DW1000RangeArduino *dw) {
|
||||
uint16_t dst_id = uint16_from_buf(dw->buf);
|
||||
uint16_t src_id = UWB_POSITIONING_TAG_ID;
|
||||
float raw_dist = float_from_buf(dw->buf + 2);
|
||||
uint32_t now_ts = get_sys_time_usec();
|
||||
|
||||
AbiSendMsgUWB_RANGING(UWB_DW1000_ARDUINO_ID, now_ts, src_id, dst_id, raw_dist);
|
||||
}
|
||||
|
||||
/** Data parsing function */
|
||||
static void dw1000_range_arduino_parse(struct DW1000RangeArduino *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) {
|
||||
send_anchor_data(dw);
|
||||
}
|
||||
dw->state = DW_WAIT_STX;
|
||||
break;
|
||||
|
||||
default:
|
||||
dw->state = DW_WAIT_STX;
|
||||
}
|
||||
}
|
||||
|
||||
void dw1000_range_arduino_init(void)
|
||||
{
|
||||
dw1000_range_arduino.dev = &DW1000_RANGE_ARDUINO_DEV;
|
||||
}
|
||||
|
||||
void dw1000_range_arduino_event(void)
|
||||
{
|
||||
// Look for data on serial link and send to parser
|
||||
while (uart_char_available(dw1000_range_arduino.dev)) {
|
||||
uint8_t ch = uart_getch(dw1000_range_arduino.dev);
|
||||
dw1000_range_arduino_parse(&dw1000_range_arduino, ch);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
/**
|
||||
* Copyright (C) 2026 Fabien-B <fabien-b@github.com>
|
||||
* This file is part of paparazzi. See LICENCE file.
|
||||
*
|
||||
* @file "modules/decawave/dw1000_range_arduino.h"
|
||||
* @author Fabien-B <fabien-b@github.com>
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef DW1000_RANGE_ARDUINO_H
|
||||
#define DW1000_RANGE_ARDUINO_H
|
||||
|
||||
extern void dw1000_range_arduino_init(void);
|
||||
extern void dw1000_range_arduino_event(void);
|
||||
|
||||
#endif // DW1000_RANGE_ARDUINO_H
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,157 @@
|
||||
/*
|
||||
* Copyright (C) 2026 Fabien-B <fabien-b@github.com>
|
||||
*
|
||||
* This file is part of paparazzi. See LICENCE file.
|
||||
*
|
||||
* Pprz Decawave driver. Allows ranging, as well as TDOA.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
struct uart_periph;
|
||||
|
||||
#ifndef PDEC_MAX_DEVICES
|
||||
#define PDEC_MAX_DEVICES 32
|
||||
#endif
|
||||
|
||||
#ifndef PDEC_MAX_TDOA_REPORTS
|
||||
#define PDEC_MAX_TDOA_REPORTS 32
|
||||
#endif
|
||||
|
||||
#define PDEC_MAX_PAYLOAD_LEN 255
|
||||
#define PDEC_MAX_DATA_LEN 252
|
||||
|
||||
enum pdec_command {
|
||||
PDEC_CMD_RANGE = 0x01,
|
||||
PDEC_CMD_GET_DISTANCE = 0x02,
|
||||
PDEC_CMD_LIST_DEVICES = 0x03,
|
||||
PDEC_CMD_SEND_DATA = 0x04,
|
||||
};
|
||||
|
||||
enum pdec_msg {
|
||||
PDEC_MSG_ERROR = 0x7F,
|
||||
PDEC_MSG_RANGE_RESULT = 0x81,
|
||||
PDEC_MSG_DISTANCE = 0x82,
|
||||
PDEC_MSG_DEVICE_LIST = 0x83,
|
||||
PDEC_MSG_SEND_DATA_RESULT = 0x84,
|
||||
PDEC_MSG_RANGING_EVENT = 0x90,
|
||||
PDEC_MSG_TDOA_REPORT_EVENT = 0x92,
|
||||
};
|
||||
|
||||
enum pdec_status {
|
||||
PDEC_STATUS_OK = 0x00,
|
||||
PDEC_STATUS_ERROR = 0x01,
|
||||
PDEC_STATUS_INVALID_LENGTH = 0x02,
|
||||
PDEC_STATUS_NOT_FOUND = 0x03,
|
||||
PDEC_STATUS_RANGE_FAILED = 0x04,
|
||||
PDEC_STATUS_MESSAGE_TOO_LONG = 0x05,
|
||||
};
|
||||
|
||||
enum pdec_event_kind {
|
||||
PDEC_EVENT_SINGLE_SIDED = 0x00,
|
||||
PDEC_EVENT_DOUBLE_SIDED = 0x01,
|
||||
PDEC_EVENT_INDIRECT = 0x02,
|
||||
};
|
||||
|
||||
enum pdec_rx_state {
|
||||
PDEC_RX_SYNC_1,
|
||||
PDEC_RX_SYNC_2,
|
||||
PDEC_RX_LEN,
|
||||
PDEC_RX_PAYLOAD,
|
||||
PDEC_RX_CHECKSUM,
|
||||
};
|
||||
|
||||
struct pdec_error_response {
|
||||
bool updated;
|
||||
uint8_t command;
|
||||
enum pdec_status status;
|
||||
};
|
||||
|
||||
struct pdec_distance_result {
|
||||
bool updated;
|
||||
uint16_t dst_id;
|
||||
enum pdec_status status;
|
||||
float distance;
|
||||
};
|
||||
|
||||
struct pdec_device_list {
|
||||
bool updated;
|
||||
uint8_t count;
|
||||
uint8_t total_count;
|
||||
bool truncated;
|
||||
uint16_t device_ids[PDEC_MAX_DEVICES];
|
||||
};
|
||||
|
||||
struct pdec_send_data_result {
|
||||
bool updated;
|
||||
uint16_t dst_id;
|
||||
enum pdec_status status;
|
||||
uint8_t sent_len;
|
||||
};
|
||||
|
||||
struct pdec_ranging_event {
|
||||
bool updated;
|
||||
enum pdec_event_kind kind;
|
||||
uint16_t src_id;
|
||||
uint16_t dst_id;
|
||||
float distance;
|
||||
};
|
||||
|
||||
struct pdec_tdoa_report {
|
||||
uint16_t reporter_id;
|
||||
float distance;
|
||||
};
|
||||
|
||||
struct pdec_tdoa_report_event {
|
||||
bool updated;
|
||||
uint32_t blink_id;
|
||||
uint8_t expected_count;
|
||||
uint8_t count;
|
||||
uint8_t total_count;
|
||||
bool timed_out;
|
||||
bool truncated;
|
||||
struct pdec_tdoa_report reports[PDEC_MAX_TDOA_REPORTS];
|
||||
};
|
||||
|
||||
struct pdec_counters {
|
||||
uint32_t rx_frames;
|
||||
uint32_t rx_checksum_errors;
|
||||
uint32_t rx_length_errors;
|
||||
uint32_t rx_unknown_messages;
|
||||
uint32_t tx_frames;
|
||||
uint32_t tx_drops;
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
struct uart_periph *dev;
|
||||
|
||||
struct pdec_error_response last_error;
|
||||
struct pdec_distance_result last_range;
|
||||
struct pdec_distance_result last_distance;
|
||||
struct pdec_device_list devices;
|
||||
struct pdec_send_data_result last_send_data;
|
||||
struct pdec_ranging_event last_ranging_event;
|
||||
struct pdec_tdoa_report_event last_tdoa_report_event;
|
||||
struct pdec_counters counters;
|
||||
|
||||
/* Driver-owned parser state. */
|
||||
enum pdec_rx_state rx_state;
|
||||
uint8_t rx_len;
|
||||
uint8_t rx_idx;
|
||||
uint8_t rx_checksum;
|
||||
uint8_t rx_payload[PDEC_MAX_PAYLOAD_LEN];
|
||||
} pdec_t;
|
||||
|
||||
extern pdec_t pdec;
|
||||
|
||||
void pdec_init(void);
|
||||
void pdec_periodic_report(void);
|
||||
void pdec_event(void);
|
||||
|
||||
bool pdec_range(uint16_t dst_id);
|
||||
bool pdec_get_distance(uint16_t dst_id);
|
||||
bool pdec_list_devices(void);
|
||||
bool pdec_send_data(uint16_t dst_id, const uint8_t *data, uint8_t len);
|
||||
File diff suppressed because it is too large
Load Diff
+17
-19
@@ -18,42 +18,40 @@
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/decawave/dw1000_arduino.h"
|
||||
* @file "modules/decawave/uwb_positioning.h"
|
||||
* @author Gautier Hattenberger
|
||||
* Driver to get ranging data from Decawave DW1000 modules connected to Arduino
|
||||
* UWB positioning from anchor measurements.
|
||||
*/
|
||||
|
||||
#ifndef DW1000_ARDUINO_H
|
||||
#define DW1000_ARDUINO_H
|
||||
#pragma once
|
||||
|
||||
#include "std.h"
|
||||
|
||||
/** enable EKF filtering */
|
||||
extern bool dw1000_use_ekf;
|
||||
extern bool uwb_positioning_use_ekf;
|
||||
|
||||
/** process and measurements noise */
|
||||
extern float dw1000_ekf_q;
|
||||
extern float dw1000_ekf_r_dist;
|
||||
extern float dw1000_ekf_r_speed;
|
||||
extern float uwb_positioning_ekf_q;
|
||||
extern float uwb_positioning_ekf_r_dist;
|
||||
extern float uwb_positioning_ekf_r_speed;
|
||||
/** settings handler */
|
||||
extern void dw1000_arduino_update_ekf_q(float v);
|
||||
extern void dw1000_arduino_update_ekf_r_dist(float v);
|
||||
extern void dw1000_arduino_update_ekf_r_speed(float v);
|
||||
extern void uwb_positioning_update_ekf_q(float v);
|
||||
extern void uwb_positioning_update_ekf_r_dist(float v);
|
||||
extern void uwb_positioning_update_ekf_r_speed(float v);
|
||||
|
||||
extern void dw1000_arduino_init(void);
|
||||
extern void dw1000_arduino_periodic(void);
|
||||
extern void dw1000_arduino_report(void);
|
||||
extern void dw1000_arduino_event(void);
|
||||
extern void uwb_positioning_init(void);
|
||||
extern void uwb_positioning_periodic(void);
|
||||
extern void uwb_positioning_range_periodic(void);
|
||||
extern void uwb_positioning_report(void);
|
||||
|
||||
/** Reset reference heading to current heading
|
||||
* AHRS/INS should be aligned before calling this function
|
||||
*/
|
||||
extern void dw1000_reset_heading_ref(void);
|
||||
extern void uwb_positioning_reset_heading_ref(void);
|
||||
|
||||
// when used as a GPS
|
||||
#ifndef PRIMARY_GPS
|
||||
#define PRIMARY_GPS GPS_DW1000
|
||||
#endif
|
||||
|
||||
#define PRIMARY_GPS GPS_UWB
|
||||
#endif
|
||||
|
||||
void uwb_range(uint16_t id);
|
||||
Reference in New Issue
Block a user