Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
D
dw1000-driver
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
D7039E-E7025E
dw1000-driver
Commits
eed065ce
Commit
eed065ce
authored
8 years ago
by
ashlaban
Browse files
Options
Downloads
Patches
Plain Diff
The tag and anchor files were used after all ;)
parent
d3dfe242
No related branches found
No related tags found
No related merge requests found
Changes
2
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
apps/decawave-range/dw-range-anchor.c
+350
-0
350 additions, 0 deletions
apps/decawave-range/dw-range-anchor.c
apps/decawave-range/dw-range-tag.c
+176
-0
176 additions, 0 deletions
apps/decawave-range/dw-range-tag.c
with
526 additions
and
0 deletions
apps/decawave-range/dw-range-anchor.c
0 → 100644
+
350
−
0
View file @
eed065ce
/**
* \file Decawace uwb demo application
* \author Kim Albertsson
*
* \note If you get a HPDWARN in status reg you are probably taking to long
* time to create a delayed send. Increase MESSAGE_DELAY_MS or optimise
* critical path for creating packet.
*/
/*===========================================================================*/
/*================================ Includes =================================*/
#include
<stdio.h>
/* For printf() */
#include
<stdint.h>
#include
"string.h"
#include
<stddef.h>
#include
"udelay.h"
#include
"dw1000-base.h"
#include
"dwFrameTypes.h"
#include
"dw-range.h"
#include
"contiki.h"
// #include "contiki-net.h"
// #include "rest-engine.h"
/*===========================================================================*/
/*================================ Defines ==================================*/
#define MESSAGE_DELAY_MS 10.0f
#define DEVICE_TIME_UNITS (1.0/499.2e6/128.0)
#define ANTENNA_DELAY ((515.0 / 2.0) * 1e-9 / (1.0/499.2e6/128.0))
#define MASK_40_BIT 0x000000FFFFFFFFFFULL
/*===========================================================================*/
/*========================== Public Declarations ============================*/
/**
* \brief Keeps track of all variables needed for a distance measurement.
*/
typedef
struct
{
uint64_t
tsp
;
/**< Poll sent time */
uint64_t
trp
;
/**< Poll received time */
uint64_t
tsr
;
/**< Response sent time */
uint64_t
trr
;
/**< Response received time */
uint64_t
tsf
;
/**< Final sent time */
uint64_t
trf
;
/**< Final received time */
int64_t
ttrt
;
/**< Round trip time as seen by tag. */
int64_t
tart
;
/**< Round trip time as seen by anchor. */
int64_t
trt
;
/**< Estimated round trip time. */
int64_t
tof
;
/**< Estimated Time of Flight. [~15.65ps per tick] */
double
s
;
/**< Estimated distance */
}
distance_meas_t
;
typedef
struct
{
uint32_t
num_meas
;
uint32_t
num_errors
;
// Measurements
float
accumulated_distance
;
float
max
;
float
min
;
float
stddev
;
// Quality
float
rx_power
;
float
fp_power
;
float
noise_level
;
float
fp_ampl
;
// Sensors
float
temperature
;
float
voltage
;
}
statistics_t
;
extern
dw1000_base_driver
dw1000
;
extern
struct
process
*
interrupt_handler_callback
;
static
distance_meas_t
dist_meas
;
static
statistics_t
stats
;
// For coap service
float
lastMeasuredDistance
=
0
.
f
;
float
accumulated_distance
=
0
.
f
;
// static void res_get_handler(void* request, void* response, uint8_t *buffer, uint16_t preferred_size, int32_t *offset);
// RESOURCE(res_dw_range, "title=\"Distance: ?len=0..\";rt=\"Text\"", res_get_handler, NULL, NULL, NULL);
/*---------------------------------------------------------------------------*/
PROCESS
(
dw_main_process
,
"Main process"
);
PROCESS
(
dw_anchor_process
,
"Anchor process (dw1000)"
);
AUTOSTART_PROCESSES
(
&
dw_main_process
);
/*---------------------------------------------------------------------------*/
/*===========================================================================*/
/*============================ Public Functions =============================*/
/**
* \brief Used to convert from raw measurements to real world distances.
*
* \details
*
* \todo Present error bounds on this function.
*
* @param distance raw measurement
* @return estimated distance
*/
static
float
distance_correction
(
float
measurement
)
{
// return distance*0.8719 - 32.8600;
// return distance*0.9876 - 37.0932;
// return distance*0.9430 - 72.4511;
return
measurement
;
}
void
print_statistics
(
statistics_t
*
stats
)
{
float
avg
=
stats
->
accumulated_distance
/
stats
->
num_meas
;
printf
(
"========================================
\n
"
);
printf
(
"Measurement no. %i
\n
"
,
stats
->
num_meas
);
printf
(
"--- Data -------------------------------
\n
"
);
printf
(
"Avg: %f
\n
"
,
avg
);
printf
(
"Max: %f
\n
"
,
stats
->
max
);
printf
(
"Min: %f
\n
"
,
stats
->
min
);
printf
(
"Std dev: %f
\n
"
,
stats
->
stddev
);
printf
(
"--- Quality ----------------------------
\n
"
);
stats
->
rx_power
=
dw_get_rx_power
();
stats
->
fp_power
=
dw_get_fp_power
();
stats
->
noise_level
=
dw_get_noise_level
();
stats
->
fp_ampl
=
dw_get_fp_ampl
();
printf
(
"rx_power : %f
\n
"
,
stats
->
rx_power
);
printf
(
"fp_power : %f
\n
"
,
stats
->
fp_power
);
printf
(
"noise_level: %f
\n
"
,
stats
->
noise_level
);
printf
(
"fp_ampl : %f
\n
"
,
stats
->
fp_ampl
);
printf
(
"--- Sensors ----------------------------
\n
"
);
stats
->
temperature
=
dw_get_temperature
(
DW_ADC_SRC_LATEST
);
stats
->
voltage
=
dw_get_voltage
(
DW_ADC_SRC_LATEST
);
printf
(
"temperature: %f
\n
"
,
stats
->
temperature
);
printf
(
"voltage : %f
\n
"
,
stats
->
voltage
);
printf
(
"--- Diagnostics ------------------------
\n
"
);
printf
(
"num_packets_rx : %i
\n
"
,
packet_stats
.
num_packets_rx
);
printf
(
"num_packets_tx : %i
\n
"
,
packet_stats
.
num_packets_tx
);
printf
(
"num_packet_errors: %i
\n
"
,
packet_stats
.
num_errors
);
printf
(
"========================================
\n
"
);
}
void
reset_statistics
(
statistics_t
*
stats
)
{
stats
->
num_meas
=
0
;
stats
->
accumulated_distance
=
0
.
f
;
stats
->
max
=
-
1e9
;
stats
->
min
=
1e9
;
stats
->
stddev
=
0
.
f
;
stats
->
num_errors
=
0
;
}
/*---------------------------------------------------------------------------*/
PROCESS_THREAD
(
dw_main_process
,
ev
,
data
)
{
PROCESS_BEGIN
();
// Init coap
// rest_init_engine();
// rest_activate_resource(&res_dw_range, "test/distance");
// Init dw1000
process_start
(
&
dw_interrupt_callback_proc
,
NULL
);
dw_init
();
dw_conf_print
();
interrupt_handler_callback
=
&
dw_anchor_process
;
reset_statistics
(
&
stats
);
process_start
(
&
dw_anchor_process
,
NULL
);
// Start code proper
while
(
1
)
{
PROCESS_WAIT_EVENT
();
}
PROCESS_END
();
}
/*---------------------------------------------------------------------------*/
PROCESS_THREAD
(
dw_anchor_process
,
ev
,
data
)
{
PROCESS_BEGIN
();
printf
(
"Starting Anchor
\n
"
);
// For precise measurements this should be calibrated per device
dw_set_antenna_delay
(
ANTENNA_DELAY
);
static
ranging_state_t
ranging_state
=
IDLE
;
static
uint64_t
destAddr
=
0ULL
;
dw_frame_blink_t
frameBlink
;
dw_frame_range_t
frameRangePoll
;
dw_message_poll_t
msgPoll
;
dw_frame_range_t
frameRangeFinal
;
dw_message_final_t
msgFinal
;
while
(
1
)
{
if
(
ranging_state
==
IDLE
)
{
// This would be in the main switch as
// case IDLE:
// if contiki multithreading would have allowed it.
dw1000_rx_conf_t
rx_conf
;
rx_conf
.
is_delayed
=
0
;
rx_conf
.
timeout
=
0xFFFF
;
dw_conf_rx
(
&
rx_conf
);
dw_receive
(
DW_TRANCEIVE_ASYNC
);
PROCESS_WAIT_EVENT_UNTIL
(
ev
==
PROCESS_EVENT_CONTINUE
);
ranging_state
=
*
(
ranging_state_t
*
)
data
;
// printf("State: %i\n", ranging_state);
// printf("ranging_state: %i\n", ranging_state);
}
uint32_t
msgLen
;
uint64_t
dx_timestamp
;
switch
(
ranging_state
)
{
case
MEASUREMENT_ADDED
:
ranging_state
=
IDLE
;
if
(
stats
.
num_meas
%
100
!=
0
)
{
break
;
}
print_statistics
(
&
stats
);
if
(
stats
.
num_meas
!=
1000
)
{
break
;
}
// For easy calibration of distance_correction().
// Now you can take one sample and then reset device.
// Remove loop when calibration is done.
while
(
1
){};
// Reset statistics.
reset_statistics
(
&
stats
);
printf
(
"Reset.
\n
"
);
break
;
case
BLINK_RECEIVED
:
parseFrameBlink
(
dw1000
.
p_receive_buffer
,
dw1000
.
receive_buffer_len
,
&
frameBlink
);
destAddr
=
frameBlink
.
tag_id
;
dx_timestamp
=
dw_get_rx_timestamp
()
+
dw_ms_to_device_time
(
MESSAGE_DELAY_MS
);
sendRangeInit
(
destAddr
,
(
uint16_t
)
MESSAGE_DELAY_MS
,
dx_timestamp
);
printf
(
"Sent ranging init
\n
"
);
ranging_state
=
IDLE
;
break
;
case
POLL_RECEIVED
:
dist_meas
.
trp
=
dw_get_rx_timestamp
();
dx_timestamp
=
dist_meas
.
trp
+
dw_ms_to_device_time
(
MESSAGE_DELAY_MS
);
//printArray(dw1000.p_receive_buffer, dw1000.receive_buffer_len);
msgLen
=
dw1000
.
receive_buffer_len
-
DW_FRAME_RANGE_LEN
-
DW_FRAME_FCS_LEN
;
parseFrameRange
(
dw1000
.
p_receive_buffer
,
dw1000
.
receive_buffer_len
,
&
frameRangePoll
);
parseMessagePoll
(
frameRangePoll
.
pPayload
,
msgLen
,
&
msgPoll
);
// TODO: Keep track of paired tags
if
(
destAddr
==
0x0ULL
)
{
destAddr
=
frameRangePoll
.
src_addr
;
}
sendResponse
(
destAddr
,
&
dist_meas
.
tsr
,
dx_timestamp
);
// printArray( dw1000.p_receive_buffer, dw1000.receive_buffer_len );
ranging_state
=
IDLE
;
break
;
case
FINAL_RECEIVED
:
dist_meas
.
trf
=
dw_get_rx_timestamp
();
msgLen
=
dw1000
.
receive_buffer_len
-
DW_FRAME_RANGE_LEN
-
DW_FRAME_FCS_LEN
;
parseFrameRange
(
dw1000
.
p_receive_buffer
,
dw1000
.
receive_buffer_len
,
&
frameRangeFinal
);
parseMessageFinal
(
frameRangeFinal
.
pPayload
,
msgLen
,
&
msgFinal
);
memcpy
(
&
dist_meas
.
tsp
,
msgFinal
.
poll_msg_tx_timestamp
,
5
);
memcpy
(
&
dist_meas
.
trr
,
msgFinal
.
resp_msg_rx_timestamp
,
5
);
memcpy
(
&
dist_meas
.
tsf
,
msgFinal
.
final_msg_tx_timestamp
,
5
);
uint64_t
tsp
=
dist_meas
.
tsp
;
uint64_t
trp
=
dist_meas
.
trp
;
uint64_t
tsr
=
dist_meas
.
tsr
;
uint64_t
trr
=
dist_meas
.
trr
;
uint64_t
tsf
=
dist_meas
.
tsf
;
uint64_t
trf
=
dist_meas
.
trf
;
dist_meas
.
ttrt
=
((
trr
-
tsp
)
&
MASK_40_BIT
)
-
((
tsr
-
trp
)
&
MASK_40_BIT
);
dist_meas
.
tart
=
((
trf
-
tsr
)
&
MASK_40_BIT
)
-
((
tsf
-
trr
)
&
MASK_40_BIT
);
dist_meas
.
trt
=
((
dist_meas
.
tart
&
MASK_40_BIT
)
+
(
dist_meas
.
ttrt
&
MASK_40_BIT
))
>>
1
;
dist_meas
.
tof
=
dist_meas
.
trt
>>
1
;
dist_meas
.
s
=
3e8
*
15.65e-12
*
(
double
)
dist_meas
.
tof
;
dist_meas
.
s
=
distance_correction
(
dist_meas
.
s
);
if
(
dist_meas
.
s
>
200
.
0
f
)
{
/*
BUG: sometimes we get a really large value...
This is possibly fixed now... (has to do with delayed
send ignoring lowest 9 bits.)
*/
++
stats
.
num_errors
;
break
;
}
stats
.
accumulated_distance
+=
dist_meas
.
s
;
stats
.
max
=
(
dist_meas
.
s
>
stats
.
max
)
?
dist_meas
.
s
:
stats
.
max
;
stats
.
min
=
(
dist_meas
.
s
<
stats
.
min
)
?
dist_meas
.
s
:
stats
.
min
;
++
stats
.
num_meas
;
ranging_state
=
MEASUREMENT_ADDED
;
break
;
case
ERROR
:
default
:
++
packet_stats
.
num_errors
;
// TODO: bad encapsulation, change to software errors.
ranging_state
=
IDLE
;
break
;
}
}
PROCESS_END
();
}
/*---------------------------------------------------------------------------*/
// static void
// res_get_handler(void* request, void* response, uint8_t *buffer, uint16_t preferred_size, int32_t *offset)
// {
// /**
// * \note This should be working... But it was a long time ago I tested it
// * so it probably won't.
// */
// // Set up response payload.
// static const uint32_t length = 21;
// static char message[128];
// sprintf( message, "Distance to tag: %4f", lastMeasuredDistance );
// memcpy(buffer, message, length);
// // Set up response packet
// REST.set_header_content_type(response, REST.type.TEXT_PLAIN); /* text/plain is the default, hence this option could be omitted. */
// REST.set_header_etag(response, (uint8_t *) &length, 1);
// REST.set_response_payload(response, buffer, length);
// }
\ No newline at end of file
This diff is collapsed.
Click to expand it.
apps/decawave-range/dw-range-tag.c
0 → 100644
+
176
−
0
View file @
eed065ce
/**
* \file
* Decawace uwb demo application
* \author
* Kim Albertsson
*/
// Tested with mulle board 145
#include
"contiki.h"
#include
<stdio.h>
/* For printf() */
#include
<stdint.h>
#include
<stddef.h>
#include
"udelay.h"
#include
"dw1000-base.h"
#include
"dwFrameTypes.h"
#include
"dw-range.h"
#define DEVICE_TIME_UNITS (1.0/499.2e6/128.0)
#define ANTENNA_DELAY ((515.0 / 2.0) * 1e-9 / (1.0/499.2e6/128.0))
typedef
enum
{
UNPAIRED
=
0
,
PAIRED
}
tag_state_t
;
extern
struct
process
*
interrupt_handler_callback
;
int
error
;
/*---------------------------------------------------------------------------*/
PROCESS
(
dw_main_process
,
"Main process"
);
PROCESS
(
dw_tag_process
,
"Tag process (dw1000)"
);
AUTOSTART_PROCESSES
(
&
dw_main_process
);
/*---------------------------------------------------------------------------*/
PROCESS_THREAD
(
dw_main_process
,
ev
,
data
)
{
PROCESS_BEGIN
();
// Init dw1000
process_start
(
&
dw_interrupt_callback_proc
,
NULL
);
dw_init
();
dw_conf_print
();
interrupt_handler_callback
=
&
dw_tag_process
;
process_start
(
&
dw_tag_process
,
NULL
);
// Start code proper
while
(
1
)
{
PROCESS_WAIT_EVENT
();
}
PROCESS_END
();
}
/*---------------------------------------------------------------------------*/
PROCESS_THREAD
(
dw_tag_process
,
ev
,
data
)
{
PROCESS_BEGIN
();
static
uint64_t
dest_addr
=
0ULL
;
static
uint16_t
responseDelay
;
static
tag_state_t
tag_state
=
UNPAIRED
;
static
ranging_state_t
ranging_state
=
IDLE
;
printf
(
"Starting Tag
\n
"
);
// Setting antenna delay
dw_set_antenna_delay
(
ANTENNA_DELAY
);
// Enable rx timeout
dw_set_rx_timeout
(
0xFFFF
);
printf
(
"Timeout: %f ms.
\n
"
,
(
float
)
dw_get_rx_timeout
()
/
1000
.
0
f
);
dw_enable_rx_timeout
();
// Configure rx
static
dw1000_rx_conf_t
rx_conf
;
rx_conf
.
is_delayed
=
0
;
rx_conf
.
timeout
=
0xFFFF
;
dw_conf_rx
(
&
rx_conf
);
dw_receive
(
DW_TRANCEIVE_ASYNC
);
static
uint64_t
tsp
;
static
uint64_t
trr
;
static
uint64_t
tsf
;
static
uint64_t
dx_timestamp
;
static
uint32_t
msgLen
;
static
dw_frame_range_t
frameRange
;
static
dw_message_range_init_t
msgRangeInit
;
static
dw_message_response_t
msgResponse
;
// static struct etimer et;
// etimer_set(&et, CLOCK_SECOND);
while
(
1
)
{
PROCESS_WAIT_EVENT
();
// printf("Received Event. %i.\n", ev);
// printf("Receive state. %i.\n", ranging_state);
//else
{
ranging_state
=
*
(
ranging_state_t
*
)
data
;
switch
(
ranging_state
)
{
case
RANGE_INIT_RECEIVED
:
// printf("Range init received.\n");
msgLen
=
dw1000
.
receive_buffer_len
-
DW_FRAME_RANGE_LEN
-
DW_FRAME_FCS_LEN
;
parseFrameRange
(
dw1000
.
p_receive_buffer
,
dw1000
.
receive_buffer_len
,
&
frameRange
);
parseMessageRangeInit
(
frameRange
.
pPayload
,
msgLen
,
&
msgRangeInit
);
dest_addr
=
frameRange
.
dest_addr
;
responseDelay
=
msgRangeInit
.
response_delay_ms
;
tag_state
=
PAIRED
;
break
;
case
RESPONSE_RECEIVED
:
//printf("Response Received.\n");
trr
=
dw_get_rx_timestamp
();
dx_timestamp
=
trr
+
dw_ms_to_device_time
(
(
float
)
responseDelay
);
msgLen
=
dw1000
.
receive_buffer_len
-
DW_FRAME_RANGE_LEN
-
DW_FRAME_FCS_LEN
;
parseFrameRange
(
dw1000
.
p_receive_buffer
,
dw1000
.
receive_buffer_len
,
&
frameRange
);
parseMessageResponse
(
frameRange
.
pPayload
,
msgLen
,
&
msgResponse
);
// send final
sendFinal
(
dest_addr
,
tsp
,
trr
,
&
tsf
,
dx_timestamp
);
printf
(
"Final sent.
\n
"
);
break
;
case
ERROR
:
default:
//printf("Error.\n");
break
;
}
ranging_state
=
IDLE
;
}
if
(
ranging_state
==
IDLE
/*ev == PROCESS_EVENT_TIMER*/
)
{
// send either poll or blink
if
(
tag_state
==
UNPAIRED
&&
ranging_state
==
IDLE
)
{
// send blink
//printf("Sending Blink.\n");
sendBlink
();
// Listen for answer
ranging_state
=
RECEIVING
;
//rx_conf.is_delayed = 1;
//rx_conf.dx_timestamp = dw_get_tx_timestamp() + dw_ms_to_device_time(4.8f);
dw_conf_rx
(
&
rx_conf
);
dw_receive
(
DW_TRANCEIVE_ASYNC
);
}
else
if
(
tag_state
==
PAIRED
&&
ranging_state
==
IDLE
)
{
// send poll
printf
(
"Sending Poll.
\n
"
);
sendPoll
(
dest_addr
,
&
tsp
);
// Listen for answer
ranging_state
=
RECEIVING
;
//rx_conf.is_delayed = 1;
//rx_conf.dx_timestamp = dw_get_tx_timestamp() + dw_ms_to_device_time(responseDelay-0.2);
dw_conf_rx
(
&
rx_conf
);
dw_receive
(
DW_TRANCEIVE_ASYNC
);
}
// etimer_reset(&et);
}
}
PROCESS_END
();
}
/*---------------------------------------------------------------------------*/
\ No newline at end of file
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment