Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-08-17 17:12:33 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Decawave positioning refactor (#3660)
Doxygen / build (push) Has been cancelled
Details

Browse Source 
* [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
2026-06-17 23:37:23 +02:00
committed by GitHub
co-authored by Fabien-B Gautier Hattenberger
parent fa97bcc52d
commit 134bc165d2
13 changed files with 1525 additions and 756 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+15
View File
@@ -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>
conf/modules/dw1000_arduino.xml
-87
View File
@@ -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>
conf/modules/dw1000_range_arduino.xml
+37
View File
@@ -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>
conf/modules/pdec.xml
+33
View File
@@ -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>
conf/modules/uwb_positioning.xml
+80
View File
@@ -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>
sw/airborne/modules/core/abi_sender_ids.h
+11 -3
View File
@@ -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
*/
sw/airborne/modules/decawave/dw1000_arduino.c
-647
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/decawave/dw1000_range_arduino.c
+121
View File
@@ -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);
}
}
sw/airborne/modules/decawave/dw1000_range_arduino.h
+21
View File
@@ -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
sw/airborne/modules/decawave/pdec.c
+435
View File
@@ -0,0 +1,435 @@
/*
* 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.
*/
#include "modules/decawave/pdec.h"
#include "mcu_periph/uart.h"
#include "modules/datalink/downlink.h"
#include "modules/core/abi.h"
#include "modules/core/abi_sender_ids.h"
#include <stdio.h>
#include <string.h>
#define PDEC_SYNC_BYTE 0xff
#define PDEC_FRAME_OVERHEAD 4
#define PDEC_PACKED __attribute__((packed))
pdec_t pdec;
static void _pdec_init(pdec_t *pdec);
static void _pdec_periodic_report(pdec_t *pdec);
static void _pdec_event(pdec_t *pdec);
struct pdec_cmd_payload {
uint8_t id;
} PDEC_PACKED;
struct pdec_target_cmd_payload {
uint8_t id;
uint16_t dst_id;
} PDEC_PACKED;
struct pdec_error_payload {
uint8_t id;
uint8_t command;
uint8_t status;
} PDEC_PACKED;
struct pdec_distance_payload {
uint8_t id;
uint16_t dst_id;
uint8_t status;
float distance;
} PDEC_PACKED;
struct pdec_device_list_payload {
uint8_t id;
uint8_t count;
uint16_t device_ids[];
} PDEC_PACKED;
struct pdec_send_data_result_payload {
uint8_t id;
uint16_t dst_id;
uint8_t status;
uint8_t sent_len;
} PDEC_PACKED;
struct pdec_ranging_event_payload {
uint8_t id;
uint8_t kind;
uint16_t src_id;
uint16_t dst_id;
float distance;
} PDEC_PACKED;
struct pdec_tdoa_report_payload {
uint16_t reporter_id;
float distance;
} PDEC_PACKED;
struct pdec_tdoa_report_event_payload {
uint8_t id;
uint32_t blink_id;
uint8_t expected_count;
uint8_t count;
uint8_t timed_out;
struct pdec_tdoa_report_payload reports[];
} PDEC_PACKED;
static bool pdec_send_frame(pdec_t *pdec, const uint8_t *payload, uint8_t payload_len)
{
uint8_t frame[PDEC_FRAME_OVERHEAD + PDEC_MAX_PAYLOAD_LEN];
uint8_t checksum = payload_len;
uint16_t frame_len = (uint16_t)payload_len + PDEC_FRAME_OVERHEAD;
long fd = 0;
if (payload_len == 0) {
pdec->counters.tx_drops++;
return false;
}
frame[0] = PDEC_SYNC_BYTE;
frame[1] = PDEC_SYNC_BYTE;
frame[2] = payload_len;
for (uint8_t i = 0; i < payload_len; i++) {
frame[3 + i] = payload[i];
checksum ^= payload[i];
}
frame[3 + payload_len] = checksum;
uart_put_buffer(pdec->dev, fd, frame, frame_len);
pdec->counters.tx_frames++;
return true;
}
static void pdec_parse_distance_result(struct pdec_distance_result *result,
const struct pdec_distance_payload *payload)
{
result->dst_id = payload->dst_id;
result->status = (enum pdec_status)payload->status;
result->distance = payload->distance;
result->updated = true;
}
static void pdec_dispatch_frame(pdec_t *pdec)
{
const uint8_t *payload = pdec->rx_payload;
uint8_t len = pdec->rx_len;
uint8_t msg_id;
if (len == 0) {
pdec->counters.rx_length_errors++;
return;
}
msg_id = payload[0];
switch (msg_id) {
case PDEC_MSG_ERROR: {
if (len != sizeof(struct pdec_error_payload)) {
pdec->counters.rx_length_errors++;
return;
}
const struct pdec_error_payload *msg = (const struct pdec_error_payload *)payload;
pdec->last_error.command = msg->command;
pdec->last_error.status = (enum pdec_status)msg->status;
pdec->last_error.updated = true;
break;
}
case PDEC_MSG_RANGE_RESULT:
if (len != sizeof(struct pdec_distance_payload)) {
pdec->counters.rx_length_errors++;
return;
}
pdec_parse_distance_result(&pdec->last_range, (const struct pdec_distance_payload *)payload);
break;
case PDEC_MSG_DISTANCE:
if (len != sizeof(struct pdec_distance_payload)) {
pdec->counters.rx_length_errors++;
return;
}
pdec_parse_distance_result(&pdec->last_distance, (const struct pdec_distance_payload *)payload);
break;
case PDEC_MSG_DEVICE_LIST: {
if (len < sizeof(struct pdec_device_list_payload)) {
pdec->counters.rx_length_errors++;
return;
}
const struct pdec_device_list_payload *msg = (const struct pdec_device_list_payload *)payload;
uint8_t total_count = msg->count;
if (total_count > ((PDEC_MAX_PAYLOAD_LEN - sizeof(struct pdec_device_list_payload)) / sizeof(msg->device_ids[0])) ||
len != (uint8_t)(sizeof(struct pdec_device_list_payload) + (total_count * sizeof(msg->device_ids[0])))) {
pdec->counters.rx_length_errors++;
return;
}
pdec->devices.total_count = total_count;
pdec->devices.count = total_count > PDEC_MAX_DEVICES ? PDEC_MAX_DEVICES : total_count;
pdec->devices.truncated = pdec->devices.total_count > pdec->devices.count;
for (uint8_t i = 0; i < pdec->devices.count; i++) {
pdec->devices.device_ids[i] = msg->device_ids[i];
}
pdec->devices.updated = true;
break;
}
case PDEC_MSG_SEND_DATA_RESULT: {
if (len != sizeof(struct pdec_send_data_result_payload)) {
pdec->counters.rx_length_errors++;
return;
}
const struct pdec_send_data_result_payload *msg = (const struct pdec_send_data_result_payload *)payload;
pdec->last_send_data.dst_id = msg->dst_id;
pdec->last_send_data.status = (enum pdec_status)msg->status;
pdec->last_send_data.sent_len = msg->sent_len;
pdec->last_send_data.updated = true;
break;
}
case PDEC_MSG_RANGING_EVENT: {
if (len != sizeof(struct pdec_ranging_event_payload)) {
pdec->counters.rx_length_errors++;
return;
}
const struct pdec_ranging_event_payload *msg = (const struct pdec_ranging_event_payload *)payload;
pdec->last_ranging_event.kind = (enum pdec_event_kind)msg->kind;
pdec->last_ranging_event.src_id = msg->src_id;
pdec->last_ranging_event.dst_id = msg->dst_id;
pdec->last_ranging_event.distance = msg->distance;
pdec->last_ranging_event.updated = true;
AbiSendMsgUWB_RANGING(UWB_PDEC_ID,
get_sys_time_usec(),
msg->src_id,
msg->dst_id,
msg->distance
);
break;
}
case PDEC_MSG_TDOA_REPORT_EVENT: {
if (len < sizeof(struct pdec_tdoa_report_event_payload)) {
pdec->counters.rx_length_errors++;
return;
}
const struct pdec_tdoa_report_event_payload *msg = (const struct pdec_tdoa_report_event_payload *)payload;
uint8_t total_count = msg->count;
if (total_count > ((PDEC_MAX_PAYLOAD_LEN - sizeof(struct pdec_tdoa_report_event_payload)) /
sizeof(struct pdec_tdoa_report_payload)) ||
len != (uint8_t)(sizeof(struct pdec_tdoa_report_event_payload) +
(total_count * sizeof(struct pdec_tdoa_report_payload)))) {
pdec->counters.rx_length_errors++;
return;
}
pdec->last_tdoa_report_event.blink_id = msg->blink_id;
pdec->last_tdoa_report_event.expected_count = msg->expected_count;
pdec->last_tdoa_report_event.total_count = total_count;
pdec->last_tdoa_report_event.count = total_count > PDEC_MAX_TDOA_REPORTS ? PDEC_MAX_TDOA_REPORTS : total_count;
pdec->last_tdoa_report_event.timed_out = msg->timed_out != 0;
pdec->last_tdoa_report_event.truncated = pdec->last_tdoa_report_event.total_count > pdec->last_tdoa_report_event.count;
uint16_t src_id[PDEC_MAX_TDOA_REPORTS];
float range_diff[PDEC_MAX_TDOA_REPORTS];
for (uint8_t i = 0; i < pdec->last_tdoa_report_event.count; i++) {
src_id[i] = msg->reports[i].reporter_id;
range_diff[i] = msg->reports[i].distance;
pdec->last_tdoa_report_event.reports[i].reporter_id = src_id[i];
pdec->last_tdoa_report_event.reports[i].distance = range_diff[i];
}
pdec->last_tdoa_report_event.updated = true;
AbiSendMsgUWB_TDOA(UWB_PDEC_ID,
get_sys_time_usec(),
(uint16_t)msg->blink_id,
pdec->last_tdoa_report_event.count,
src_id,
range_diff
);
break;
}
default:
pdec->counters.rx_unknown_messages++;
return;
}
pdec->counters.rx_frames++;
}
static void pdec_parse_byte(pdec_t *pdec, uint8_t byte)
{
switch (pdec->rx_state) {
case PDEC_RX_SYNC_1:
if (byte == PDEC_SYNC_BYTE) {
pdec->rx_state = PDEC_RX_SYNC_2;
}
break;
case PDEC_RX_SYNC_2:
if (byte == PDEC_SYNC_BYTE) {
pdec->rx_state = PDEC_RX_LEN;
} else {
pdec->rx_state = PDEC_RX_SYNC_1;
}
break;
case PDEC_RX_LEN:
pdec->rx_len = byte;
pdec->rx_idx = 0;
pdec->rx_checksum = byte;
pdec->rx_state = byte == 0 ? PDEC_RX_CHECKSUM : PDEC_RX_PAYLOAD;
break;
case PDEC_RX_PAYLOAD:
pdec->rx_payload[pdec->rx_idx++] = byte;
pdec->rx_checksum ^= byte;
if (pdec->rx_idx >= pdec->rx_len) {
pdec->rx_state = PDEC_RX_CHECKSUM;
}
break;
case PDEC_RX_CHECKSUM:
if (pdec->rx_checksum == byte) {
pdec_dispatch_frame(pdec);
} else {
pdec->counters.rx_checksum_errors++;
}
pdec->rx_state = PDEC_RX_SYNC_1;
break;
default:
pdec->rx_state = PDEC_RX_SYNC_1;
break;
}
}
static void _pdec_init(pdec_t *pdec)
{
memset(pdec, 0, sizeof(*pdec));
pdec->dev = &PDEC_UART_DEV;
pdec->rx_state = PDEC_RX_SYNC_1;
}
static void _pdec_periodic_report(pdec_t *pdec __attribute__((unused)))
{
char buf[100];
if(pdec->last_error.updated) {
}
if(pdec->last_range.updated) {
}
if(pdec->last_distance.updated) {
}
if(pdec->devices.updated) {
}
if(pdec->last_send_data.updated) {
}
if(pdec->last_ranging_event.updated) {
pdec->last_ranging_event.updated = false;
int len = snprintf(buf, sizeof(buf), "[RNG]%x->%x:%0.2f",
pdec->last_ranging_event.src_id,
pdec->last_ranging_event.dst_id,
pdec->last_ranging_event.distance);
DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, len, buf);
}
if(pdec->last_tdoa_report_event.updated) {
pdec->last_tdoa_report_event.updated = false;
int offset = snprintf(buf, sizeof(buf), "[TDOA]");
for(int i=0; i<pdec->last_tdoa_report_event.count; i++) {
offset += snprintf(buf+offset, sizeof(buf)-offset, "%x:%0.2f,",
pdec->last_tdoa_report_event.reports[i].reporter_id,
pdec->last_tdoa_report_event.reports[i].distance);
}
DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, offset, buf);
}
}
static void _pdec_event(pdec_t *pdec)
{
while (uart_char_available(pdec->dev)) {
pdec_parse_byte(pdec, uart_getch(pdec->dev));
}
}
bool pdec_range(uint16_t dst_id)
{
const struct pdec_target_cmd_payload payload = {
.id = PDEC_CMD_RANGE,
.dst_id = dst_id,
};
return pdec_send_frame(&pdec, (const uint8_t *)&payload, sizeof(payload));
}
bool pdec_get_distance(uint16_t dst_id)
{
const struct pdec_target_cmd_payload payload = {
.id = PDEC_CMD_GET_DISTANCE,
.dst_id = dst_id,
};
return pdec_send_frame(&pdec, (const uint8_t *)&payload, sizeof(payload));
}
bool pdec_list_devices(void)
{
const struct pdec_cmd_payload payload = {
.id = PDEC_CMD_LIST_DEVICES,
};
return pdec_send_frame(&pdec, (const uint8_t *)&payload, sizeof(payload));
}
bool pdec_send_data(uint16_t dst_id, const uint8_t *data, uint8_t len)
{
uint8_t payload[sizeof(struct pdec_target_cmd_payload) + PDEC_MAX_DATA_LEN];
struct pdec_target_cmd_payload header = {
.id = PDEC_CMD_SEND_DATA,
.dst_id = dst_id,
};
if (len > PDEC_MAX_DATA_LEN || (len > 0 && data == NULL)) {
pdec.counters.tx_drops++;
return false;
}
memcpy(payload, &header, sizeof(header));
if (len > 0) {
memcpy(&payload[sizeof(header)], data, len);
}
return pdec_send_frame(&pdec, payload, (uint8_t)(sizeof(header) + len));
}
void pdec_init(void)
{
_pdec_init(&pdec);
}
void pdec_periodic_report(void)
{
_pdec_periodic_report(&pdec);
}
void pdec_event(void)
{
_pdec_event(&pdec);
}
void uwb_range(uint16_t dst_id) {
pdec_range(dst_id);
}
sw/airborne/modules/decawave/pdec.h
+157
View File
@@ -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);
sw/airborne/modules/decawave/uwb_positioning.c
+598
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/decawave/dw1000_arduino.h → sw/airborne/modules/decawave/uwb_positioning.h
+17 -19
View File
@@ -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);