From eb4cda43e1c8aa55ed89847de322ba3830e1baa5 Mon Sep 17 00:00:00 2001
From: David <stierint@hotmail.com>
Date: Wed, 14 Apr 2021 23:24:02 -0400
Subject: [PATCH] update handling of range bias
---
README.MD | 10 +-
src/application/application_definitions.h | 64 ++-
src/application/dw_main.c | 93 +++-
src/application/instance.c | 37 +-
src/application/instance.h | 22 +-
src/application/instance_common.c | 520 ++++++++++++++++++++--
6 files changed, 637 insertions(+), 109 deletions(-)
diff --git a/README.MD b/README.MD
index 3b6bde2..25b719e 100644
--- a/README.MD
+++ b/README.MD
@@ -77,10 +77,16 @@ The TX and RX delay values displayed are the calibration values set for the curr
```
ADDR STATUS
-NXX FLXXX XX.XXm
+NXX HXX XX.XXm
```
-This is the final screen that will be displayed by the UWB. The address (ADDR) and status will be displayed on the top line. Status will be either SEARCHING or CONNECTED to show network connection status. The second line will display the most recent range measurement in meters along with some information related to network coordination. NXX is the number of neighboring UWBs (within communication distance of the UWB). NXX with alternate with HXX which is the number of hidden UWBs (not within communication distance of the UWB, but within communication distance of a neighboring UWB). FLXXX is the TDMA framelength.
+```
+ADDR STATUS
+XX.XXdB XX.XXm
+```
+
+These are final UWB screens and they displayed in an alternatin pattern. The address (ADDR) and status will be displayed on the top line. The status will be either SEARCHING or CONNECTED to show network connection status. The second line will display the most recent range measurement in meters along with some information related to network coordination. NXX is the number of neighboring UWBs (within communication distance of the UWB) and HXX is the number of hidden UWBs (not within communication distance of the UWB, but within communication distance of a neighboring UWB). XX.XXdB represents the extimated received signal level (RSL). The RSL displayed is the average of the last 50 estimations its primary use is in determining calibration distances.
+
### Flashing the Binary
diff --git a/src/application/application_definitions.h b/src/application/application_definitions.h
index 327c383..8dd0f04 100644
--- a/src/application/application_definitions.h
+++ b/src/application/application_definitions.h
@@ -15,40 +15,25 @@
//do not use a number larger than 80 if USING_64BIT_ADDR==1
//do not use a number larger than 80 if USING_64BIT_ADDR==0
//these are the largest UWB network sizes that the firmware will support.
- //USING_64BIT_ADDR==1: Limiting factor is the max length of the INF messages
- //USING_64BIT_ADDR==0: Limiting factor is the memory required to store TDMAInfo objects for each UWB
+ //when USING_64BIT_ADDR==1, limiting factor is the max length of the INF messages
+ //when USING_64BIT_ADDR==0, limiting factor is the memory required to store TDMAInfo objects for each UWB
//TODO set all below to zero as default values
-#define SET_TXRX_DELAY 1 //when set to 1 - the DW1000 RX and TX delays are set to the TX_DELAY and RX_DELAY defines
-#define TX_ANT_DELAY 16466
-#define RX_ANT_DELAY 16466 //TODO put reasonable initial value? add explanation
-//TODO have antenna delay per S1 channel config!
-//I still need to make it apply the settings below based on the S1 settings
-//S1 5-6-7 OFF-OFF-OFF
-#define TX_ANT_DELAY_000 0
-#define RX_ANT_DELAY_000 0
-//S1 5-6-7 OFF-OFF-ON
-#define TX_ANT_DELAY_001 0
-#define RX_ANT_DELAY_001 0
-//S1 5-6-7 OFF-ON-OFF
-#define TX_ANT_DELAY_010 0
-#define RX_ANT_DELAY_010 0
-//S1 5-6-7 OFF-ON-ON
-#define TX_ANT_DELAY_011 0
-#define RX_ANT_DELAY_011 0
-//S1 5-6-7 ON-OFF-OFF
-#define TX_ANT_DELAY_100 0
-#define RX_ANT_DELAY_100 0
-//S1 5-6-7 ON-OFF-ON
-#define TX_ANT_DELAY_101 0
-#define RX_ANT_DELAY_101 0
-//S1 5-6-7 ON-ON-OFF
-#define TX_ANT_DELAY_110 0
-#define RX_ANT_DELAY_110 0
-//S1 5-6-7 ON-ON-ON
-#define TX_ANT_DELAY_111 0
-#define RX_ANT_DELAY_111 0
-
+#define SET_TXRX_DELAY 1 //when set to 1 - the DW1000 RX and TX delays are set to the TX_ANT_DELAY and RX_ANT_DELAY defines
+
+//Antenna delay per S1 channel config
+//S1 6-7 : OFF-OFF
+#define TX_ANT_DELAY_00 16454
+#define RX_ANT_DELAY_00 16454
+//S1 6-7 : OFF-ON
+#define TX_ANT_DELAY_01 16457
+#define RX_ANT_DELAY_01 16457
+//S1 6-7 : ON-OFF
+#define TX_ANT_DELAY_10 16462
+#define RX_ANT_DELAY_10 16462
+//S1 6-7 : ON-ON
+#define TX_ANT_DELAY_11 16456
+#define RX_ANT_DELAY_11 16456
#define USING_64BIT_ADDR 0 //when set to 0 - the DecaRanging application will use 16-bit addresses
@@ -56,10 +41,12 @@
*******************************************************************************************************************
*******************************************************************************************************************/
-#define SPEED_OF_LIGHT 299702547.0 //299704644.539f //(299702547.0) // (299,792.458 km/s in vacuum) / (1.000293) in m/s in air //TODO clean up
+#define SPEED_OF_LIGHT 299702547.0 //in m/s in air
#define CORRECT_RANGE_BIAS (1) // Compensate for small bias due to uneven accumulator growth at close up high power
+#define NUM_RSL_AVG 50 // Number of Received Signal Level values to average for front LCD display
+
#define NUM_INST 1
#define MASK_40BIT (0x00FFFFFFFFFF) // DW1000 counter is 40 bits
#define MASK_42BIT (0x000003FFFFFFFFFF) //stm32 microsecond timestamps are 42 bits
@@ -116,8 +103,8 @@ enum
#define ANCH_RESPONSE_MSG_LEN 1 // FunctionCode(1)
#define TAG_FINAL_MSG_LEN 16 // FunctionCode(1), Poll_TxTime(5), Resp_RxTime(5), Final_TxTime(5)
#define RNG_INIT_MSG_LEN 1 // FunctionCode(1)
-#define RNG_REPORT_MSG_LEN_SHORT 9 // FunctionCode(1), time of flight (6), short address (2)
-#define RNG_REPORT_MSG_LEN_LONG 15 // FunctionCode(1), time of flight (6), long address (8)
+#define RNG_REPORT_MSG_LEN_SHORT 17 // FunctionCode(1), time of flight (6), received signal level (8), short address (2)
+#define RNG_REPORT_MSG_LEN_LONG 23 // FunctionCode(1), time of flight (6), received signal level (8), long address (8)
#define SYNC_MSG_LEN 8 // FunctionCode(1), framelength (1), time since frame start (6)
#define MAX_MAC_MSG_DATA_LEN (TAG_FINAL_MSG_LEN) //max message len of the above
@@ -212,7 +199,9 @@ enum
// Range report byte offsets.
#define REPORT_TOF 1 // Offset to put ToF values in the report message. (6 bytes)
-#define REPORT_ADDR 7 // Offset to put address of other UWB involved in the range report (2 [short address] or 8 [long address] bytes)
+#define REPORT_RSL 7 // Offset to put RSL values in the report message
+#define REPORT_ADDR 15 // Offset to put address of other UWB involved in the range report (2 [short address] or 8 [long address] bytes)
+
//Sync
#define SYNC_FRAMELENGTH 1 // offset to put the framelength in the sync message
@@ -240,11 +229,10 @@ enum
#define US_TO_SY_INT(x) (((x) * 10000) / 10256)
-#define RX_TO_CB_DLY_US 50 //TODO is this right? should this be dependent on data???
+#define RX_TO_CB_DLY_US 50
#define RX_CB_TO_TX_CMD_DLY_US 220
#define TX_CMD_TO_TX_CB_DLY_US 90 //doesn't include preamble, sfd, phr, and data. saw 81 to 87
-//TODO seems to not be long enough, failing on delayed TX... (could this value be dependant on UWB settings??? or does the note below accoutn for it. should I be including the preamble and sfd?)
#define MIN_DELAYED_TX_DLY_US 90 //doesn't include preamble and sfd. //80 is the minimum delay for delayed tx. anything shorter will cause failure
diff --git a/src/application/dw_main.c b/src/application/dw_main.c
index 9023245..719e940 100644
--- a/src/application/dw_main.c
+++ b/src/application/dw_main.c
@@ -28,6 +28,8 @@ extern int usb_init(void);
extern void usb_printconfig(int, uint8*, int);
extern void send_usbmessage(uint8*, int);
+extern double dwt_getrangebias(uint8 chan, float range, uint8 prf);
+
#define SOFTWARE_VER_STRING "TDMA Version 1.0" //
int instance_mode = DISCOVERY;
@@ -443,9 +445,12 @@ int dw_main(void)
int n;
updateLCD = TRUE;
//send the new range information to LCD and/or USB
- double idist = instance_get_idist(inst->newRangeUWBIndex);
- int rng = (int)(idist*1000);
+// double idist_rng = instance_get_idist(inst->newRangeUWBIndex);
+// double idist_rsl = instance_get_idistrsl(inst->newRangeUWBIndex);
+ int rng_rng = (int)(instance_get_idist(inst->newRangeUWBIndex)*1000);
+ int rng_rsl = (int)(instance_get_idistrsl(inst->newRangeUWBIndex)*1000);
int rng_raw = (int)(instance_get_idistraw(inst->newRangeUWBIndex)*1000);
+ int rsl = (int)(instance_get_irsl(inst->newRangeUWBIndex)*1000);
uint64 saddr = instance_get_addr();
uint64 aaddr = instancenewrangeancadd();
@@ -455,21 +460,74 @@ int dw_main(void)
{
//only update range on display if this UWB is one of the UWBs involved in the range measurement
if(memcmp(&saddr, &aaddr, sizeof(uint64)) == 0 || memcmp(&saddr, &taddr, sizeof(uint64)) == 0){
- range_result = idist;
+// range_result = instance_get_idist(inst->newRangeUWBIndex);
+ range_result = instance_get_idistrsl(inst->newRangeUWBIndex);
+// range_result = rng_raw; TODO
+ }
+
+ inst->RSL[inst->idxRSL] = instance_get_irsl(inst->newRangeUWBIndex);
+ inst->idxRSL = (inst->idxRSL + 1)%NUM_RSL_AVG;
+
+
+ if(inst->rslCnt >= NUM_RSL_AVG)
+ {
+ inst->avgRSL = 0;
+ for(int j = 0; j < NUM_RSL_AVG; j++)
+ {
+ inst->avgRSL += inst->RSL[j];
+ }
+ inst->avgRSL /= NUM_RSL_AVG;
}
+ else
+ {
+ inst->avgRSL = 0;
+ inst->rslCnt++;
+ }
+
}
+
// //self address, ranging anchor address, ranging tag address, range TODO
// n = sprintf((char*)&dataseq[0], "%016llX %016llX %016llX %08X %08X", saddr, aaddr, taddr, rng); //TODO make this 04 or something! (and update the interface nodes as well...)
- n = sprintf((char*)&dataseq[0], "%016llX %016llX %016llX %08X", saddr, aaddr, taddr, rng);//TODO keep only this nominal one!
-// n = sprintf((char*)&dataseq[0], "%08i", rng);
+// n = sprintf((char*)&dataseq[0], "%016llX %016llX %016llX %08X", saddr, aaddr, taddr, rng);//TODO keep only this nominal one!
+// n = sprintf((char*)&dataseq[0], "%016llX %016llX %016llX %08X", saddr, aaddr, taddr, rng_raw);
+// n = sprintf((char*)&dataseq[0], "%08i, %08i, %08i", rng, rng_raw, rng-rng_raw);
+// n = sprintf((char*)&dataseq[0], "%08i", rng_raw);
+// n = sprintf((char*)&dataseq[0], "%08i, %08i, %08f", rng, rng_raw, inst->rxPWR);
+// n = sprintf((char*)&dataseq[0], "%08i, %08f", rng_raw, inst->rxPWR);
// n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%04llX,%04llX", taddr, aaddr);
-//
-//
+
+// saddr, aaddr, taddr, rng_rsl, rng_rng, rng_raw, rsl //TODO
+ n = sprintf((char*)&dataseq[0], "%016llX %016llX %016llX %08X %08X %08X %08X", saddr, aaddr, taddr, rng_rng, rng_rsl, rng_raw, rsl);//TODO keep only this nominal one!
//
//
send_usbmessage(&dataseq[0], n);
usb_run(); //TODO
+
+
+
+// uint8 debug_msg[100];
+// n = sprintf((char*)&debug_msg[0], "START");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run(); //TODO
+//
+// float distance = -0.50; //(-.5m to 300m)
+// while(distance < 300.0){
+//// float distance_to_correct = distance/1.51;
+// float distance_to_correct = distance;
+// double bias = dwt_getrangebias(inst->configData.chan, distance_to_correct, inst->configData.prf);
+//
+// //TODO set compiler options to optimize
+// n = sprintf((char*)&debug_msg[0], "%f, %lf", distance, bias);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run(); //TODO
+// distance += 0.25;
+// }
+//
+// n = sprintf((char*)&debug_msg[0], "STOP");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run(); //TODO
+
}
}
@@ -506,6 +564,12 @@ int dw_main(void)
{
strcpy(status, "SEARCHING");
range_result = 0;
+ inst->avgRSL = 0;
+ for(int j = 0; j < NUM_RSL_AVG; j++)
+ {
+ inst->RSL[j] = 0;
+ }
+
}
else
{
@@ -523,12 +587,16 @@ int dw_main(void)
if(inst->addrByteSize == 8)
{
sprintf((char*)&dataseq[0], "%s ", status);
- sprintf((char*)&dataseq1[0], "N%02u FL%03u %05.2fm", num_neighbors, framelength, range_result);
+// sprintf((char*)&dataseq1[0], "N%02u FL%03u %05.2fm", num_neighbors, framelength, range_result);
+ sprintf((char*)&dataseq1[0], "% 05.1fdB % 05.2fm", inst->avgRSL, range_result);
}
else
{
sprintf((char*)&dataseq[0], "%04llX %s", addr, status);
- sprintf((char*)&dataseq1[0], "N%02u FL%03d %05.2fm", num_neighbors, framelength, range_result);
+// sprintf((char*)&dataseq1[0], "N%02u FL%03d %05.2fm", num_neighbors, framelength, range_result); //TODO
+ sprintf((char*)&dataseq1[0], "% 05.1fdB % 05.2fm", inst->avgRSL, range_result);
+ //NXX FLXXX 00.00m //TODO
+ //RSL 00.00 00.00m
}
}
else //if(toggle == 2)
@@ -536,13 +604,16 @@ int dw_main(void)
if(inst->addrByteSize == 8)
{
sprintf((char*)&dataseq[0], "%016llX", addr);
- sprintf((char*)&dataseq1[0], "H%02u FL%03u %05.2fm", num_neighbors, framelength, range_result);
+// sprintf((char*)&dataseq1[0], "H%02u FL%03u %05.2fm", num_neighbors, framelength, range_result);
+ sprintf((char*)&dataseq1[0], "N%02u H%02u % 05.2fm", num_neighbors, num_hidden, range_result);
}
else
{
sprintf((char*)&dataseq[0], "%04llX %s", addr, status);
- sprintf((char*)&dataseq1[0], "H%02u FL%03d %05.2fm", num_hidden, framelength, range_result);
+ sprintf((char*)&dataseq1[0], "N%02u H%02u % 05.2fm", num_neighbors, num_hidden, range_result);
+// sprintf((char*)&dataseq1[0], "H%02u FL%03d %05.2fm", num_hidden, framelength, range_result);
+// sprintf((char*)&dataseq1[0], "RSL%05.2f %05.2fm", inst->avgRSL, range_result);//TODO
}
}
diff --git a/src/application/instance.c b/src/application/instance.c
index 847e118..b1a2e32 100644
--- a/src/application/instance.c
+++ b/src/application/instance.c
@@ -596,6 +596,8 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
// Write calculated TOF into response message
memcpy(&inst->report_msg.messageData[REPORT_TOF], &inst->tof[inst->uwbToRangeWith], 6);
+// memcpy(&inst->report_msg.messageData[REPORT_RSL], &inst->iRSL[inst->uwbToRangeWith], sizeof(double));
+ memcpy(&inst->report_msg.messageData[REPORT_RSL], &inst->rxPWR, sizeof(double));
memcpy(&inst->report_msg.messageData[REPORT_ADDR], &inst->uwbList[inst->uwbToRangeWith], inst->addrByteSize);
inst->report_msg.seqNum = inst->frameSN++;
@@ -1004,23 +1006,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->tof[inst->uwbToRangeWith] = (int64) ( RaRbxDaDb/(RbyDb + RayDa) );
inst->newRangeUWBIndex = inst->uwbToRangeWith;
- if(reportTOF(inst, inst->newRangeUWBIndex)==0)
+ if(reportTOF(inst, inst->newRangeUWBIndex, inst->rxPWR)==0)
{
inst->newRange = 1;
}
-
-
-// double idist = instance_get_idist(inst->newRangeUWBIndex);
-// int rng = (int)(idist*1000);
-// int rng_raw = (int)(instance_get_idistraw(inst->newRangeUWBIndex)*1000);
-//
-// uint8 debug_msg[100];//TODO remove
-// int n = sprintf((char *)&debug_msg, "%lld,%lld,%lld,%lld,%lld,%lld,%08i,%08i", Ra, Db, Rb, Da, Ra-Db, Rb-Da, rng, rng_raw);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-
tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith].lastRange = portGetTickCnt();
inst->newRangeTagAddress = instance_get_uwbaddr(inst->uwbToRangeWith);
@@ -1041,6 +1031,8 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
//copy previously calculated ToF
memcpy(&inst->tof[anchor_index], &messageData[REPORT_TOF], 6);
+ memcpy(&inst->rxPWR, &messageData[REPORT_RSL], sizeof(double));
+
inst->newRangeAncAddress = instance_get_uwbaddr(anchor_index);
inst->newRangeTagAddress = instance_get_uwbaddr(tag_index);
@@ -1048,7 +1040,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->newRangeUWBIndex = anchor_index;
if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report TODO is this true?
{
- if(reportTOF(inst, inst->newRangeUWBIndex)==0)
+ if(reportTOF(inst, inst->newRangeUWBIndex, inst->rxPWR)==0)
{
inst->newRange = 1;
}
@@ -1217,7 +1209,7 @@ void instance_init_timings(void)
// Compute frame lengths.
// First step is preamble plus SFD length.
- sfd_len = dwnsSFDlen[inst->configData.dataRate];
+ sfd_len = dwnsSFDlen[inst->configData.dataRate]; //TODO what if we are using standard SFD???
switch (inst->configData.txPreambLength)
{
case DWT_PLEN_4096:
@@ -1281,10 +1273,21 @@ void instance_init_timings(void)
inst->finalReplyDelay = convertmicrosectodevicetimeu(duration);
// Delay between blink reception and ranging init message transmission.
inst->rnginitReplyDelay = convertmicrosectodevicetimeu(MIN_DELAYED_TX_DLY_US + inst->storedPreLen_us); //rng_init tx cmd to rng_init tx ts
+
+ uint8 reply_margin_us = 25;
+ duration = 0;
+ duration += inst->frameLengths_us[POLL] - inst->storedPreLen_us + RX_TO_CB_DLY_US; //resp rx timestamp to resp rx cb
+ duration += RX_CB_TO_TX_CMD_DLY_US + MIN_DELAYED_TX_DLY_US + inst->storedPreLen_us; //resp rx cb to final tx timestamp
+ uint64 respDelay = convertmicrosectodevicetimeu(duration + reply_margin_us);
+
duration = 0;
duration += inst->frameLengths_us[RESP] - inst->storedPreLen_us + RX_TO_CB_DLY_US; //resp rx timestamp to resp rx cb
duration += RX_CB_TO_TX_CMD_DLY_US + MIN_DELAYED_TX_DLY_US + inst->storedPreLen_us; //resp rx cb to final tx timestamp
- inst->respReplyDelay = convertmicrosectodevicetimeu(duration);
+ uint64 finalDelay = convertmicrosectodevicetimeu(duration + reply_margin_us);
+
+ //make reply times the same to minimize clock drift error. See Application Note APS011 for more information
+ inst->respReplyDelay = inst->finalReplyDelay = MAX(respDelay, finalDelay);
+
margin_us = 1000;
//tx conf timeout durations
@@ -1369,6 +1372,8 @@ void instance_init_timings(void)
{
inst->smartPowerEn = 0;
}
+// dwt_setsmarttxpower(0);//TODO
+// inst->smartPowerEn = 0;
}
uint32 instance_getmessageduration_us(int data_length_bytes)
diff --git a/src/application/instance.h b/src/application/instance.h
index 95f5510..3a10da4 100644
--- a/src/application/instance.h
+++ b/src/application/instance.h
@@ -123,6 +123,12 @@ typedef struct
int64 tof[UWB_LIST_SIZE] ;
double clockOffset ;
+ //RSL reporting
+ uint8 rslCnt;
+ uint8 idxRSL;
+ double RSL[NUM_RSL_AVG];
+ double avgRSL;
+
//counts for debug
int txmsgcount;
int rxmsgcount;
@@ -135,7 +141,9 @@ typedef struct
uint64 newRangeTagAddress; //tag address for most recent ranging exchange
double idistance[UWB_LIST_SIZE];
- double idistanceraw[UWB_LIST_SIZE];
+ double idistancersl[UWB_LIST_SIZE];
+ double idistanceraw[UWB_LIST_SIZE];
+ double iRSL[UWB_LIST_SIZE];
uint8 uwbToRangeWith; //it is the index of the uwbList array which contains the address of the UWB we are ranging with
uint8 uwbListLen ;
@@ -158,6 +166,9 @@ typedef struct
uint32 rxCheckOnTime;
+ double rxPWR;
+ uint8 acc_adj;
+
instanceConfig_t chConfig[8];
} instance_data_t ;
@@ -171,7 +182,9 @@ typedef struct
//-------------------------------------------------------------------------------------------------------------
// function to calculate and report the Time of Flight to the GUI/display
-int reportTOF(instance_data_t *inst, uint8 uwb_index);
+double getrangebias_rng(uint8 channel, uint8 prf, double distance);
+double getrangebias_rsl(uint8 channel, uint8 prf, double rsl);
+int reportTOF(instance_data_t *inst, uint8 uwb_index, double rsl);
// clear the status/ranging data
void instanceclearcounts(void) ;
void instclearuwblist(void);
@@ -234,14 +247,13 @@ int instancegetrole(void) ;
uint32 instancereaddeviceid(void) ; // Return Device ID reg, enables validation of physical device presence
void instancerxon(instance_data_t *inst, int delayed, uint64 delayedReceiveTime);
-// double instance_get_adist(void);
double instance_get_idist(uint8 uwb_index);
+double instance_get_idistrsl(uint8 uwb_index);
double instance_get_idistraw(uint8 uwb_index);
-// int instance_get_lcount(void);
+double instance_get_irsl(uint8 uwb_index);
uint64 instance_get_addr(void); //get own address (8 bytes)
uint64 instance_get_uwbaddr(uint8 uwb_index); //get uwb address (8 bytes)
-// uint64 instance_get_anchaddr(void); //get anchor address (that sent the ToF)
uint64 instancenewrangeancadd(void);
uint64 instancenewrangetagadd(void);
diff --git a/src/application/instance_common.c b/src/application/instance_common.c
index ec3ea6f..5307538 100644
--- a/src/application/instance_common.c
+++ b/src/application/instance_common.c
@@ -155,8 +155,262 @@ double convertdevicetimetosec8(uint8* dt)
return f ;
}
+double getrangebias_rng(uint8 channel, uint8 prf, double distance)
+{
+ float bias = 0;
+ distance *= 1000.0; //convert to mm
-int reportTOF(instance_data_t *inst, uint8 uwb_index)
+ if(channel == 2)
+ {
+ if(prf == DWT_PRF_16M) //00
+ {
+ if(distance < 40800.0)
+ {
+ double a1 = -0.536346023;
+ double b1 = -0.0000935073535;
+ double c1 = 6.47523309;
+ double d1 = -85.2803987;
+
+ bias = a1*exp(b1*distance + c1) + d1;
+ }
+ else if(distance < 91000.0)
+ {
+ double a2 = -0.279365897;
+ double b2 = -0.0000136494099;
+ double c2 = 7.21745083;
+ double d2 = 171.189077;
+
+ bias = a2*exp(b2*distance + c2) + d2;
+ }
+ else
+ {
+ double a2 = -0.279365897;
+ double b2 = -0.0000136494099;
+ double c2 = 7.21745083;
+ double d2 = 171.189077;
+
+ bias = a2*exp(b2*91000.0 + c2) + d2;
+ }
+ }
+ else if(prf == DWT_PRF_64M) //10
+ {
+ double a = -0.746580470;
+ double b = -0.000100581928;
+ double c = 5.66511916;
+ double d = -16.8215660;
+ double e = 6.85427230;
+
+ if(distance < 67000)
+ {
+ bias = a * exp(b*distance+c) + d;
+ }
+ else
+ {
+ bias = e;
+ }
+ }
+ }
+ else if(channel == 5)
+ {
+ if(prf == DWT_PRF_16M) //01
+ {
+ double m = -0.0127206623;
+ double bx = 589.971543;
+ double by = -365.378639;
+ double cx = 12543.2809;
+ double cy = -163.200647;
+ double a1 = -0.970942265;
+ double b1 = -0.0000284505196;
+ double c1 = 6.40529252;
+ double d1 = 112.149301;
+
+ double x40k = 40000.0;
+ double x91k = 91000.0;
+
+
+ if(distance < bx)
+ {
+ bias = (bx - distance)*m+by;
+ }
+ else if(distance < cx)
+ {
+ bias = (distance-bx)/(cx-bx)*(cy-by)+by;
+ }
+ else if(distance < x40k)
+ {
+ double y40k = a1*exp(b1*x40k+c1) + d1;
+ bias = (distance-cx)/(x40k-cx)*(y40k-cy)+cy;
+ }
+ else if(distance < x91k)
+ {
+ bias = a1*exp(b1*distance+c1) + d1;
+ }
+ else
+ {
+ bias = a1*exp(b1*x91k+c1) + d1;
+ }
+ }
+ else if(prf == DWT_PRF_64M) //11
+ {
+ double a = -0.922756193;
+ double b = -0.0000694702324;
+ double c = 5.57166578;
+ double d = 10.5099467;
+
+ bias = a*exp(b*distance+c) + d;
+ }
+ }
+
+ return bias/1000.0; //convert to meters
+}
+
+double getrangebias_rsl(uint8 channel, uint8 prf, double rsl)
+{
+ float bias = 0;
+
+ if(channel == 2)
+ {
+ if(prf == DWT_PRF_16M) //00
+ {
+ float m = -62.04;
+ float bx = -82.83;
+ float by = -129.82;
+ float cx = -90.05;
+ float cy = 28.16;
+ float dx = -92.40;
+ float dy = 49.44;
+
+ if(rsl > bx) //region 1
+ {
+ bias = (rsl-bx)*m+by;
+ }
+ else if(rsl > cx) //region 2
+ {
+ bias = (rsl-cx)/(bx-cx)*(by-cy)+cy;
+ }
+ else if(rsl > dx) //region 3
+ {
+ bias = (rsl-dx)/(cx-dx)*(cy-dy)+dy;
+ }
+ else //region 4
+ {
+ bias = dy;
+ }
+ }
+ else if(prf == DWT_PRF_64M) //10
+ {
+ double m = -45.48484848;
+ double bx = -80.62270464;
+ double by = -92.36287762;
+ double cx = -82.03729358;
+ double cy = -109.84714946;
+ double dx = -82.83220438;
+ double dy = -50.94116132;
+ double ex = -85.42920938;
+ double ey = -22.6320239;
+ double fx = -89.88519826;
+ double fy = -38.18332969;
+ double gx = -91.7826361;
+ double gy = 7.25759428;
+
+ if(rsl > bx) //region 1
+ {
+ bias = (rsl-bx)*m+by;
+ }
+ else if(rsl > cx) //region 2
+ {
+ bias = (rsl-cx)/(bx-cx)*(by-cy)+cy;
+ }
+ else if(rsl > dx) //region 3
+ {
+ bias = (rsl-dx)/(cx-dx)*(cy-dy)+dy;
+ }
+ else if(rsl > ex) //region 4
+ {
+ bias = (rsl-ex)/(dx-ex)*(dy-ey)+ey;
+ }
+ else if(rsl > fx) //region 5
+ {
+ bias = (rsl-fx)/(ex-fx)*(ey-fy)+fy;
+ }
+ else if(rsl > gx) //region 6
+ {
+ bias = (rsl-gx)/(fx-gx)*(fy-gy)+gy;
+ }
+ else //region 7
+ {
+ bias = gy;
+ }
+ }
+ }
+ else if(channel == 5)
+ {
+ if(prf == DWT_PRF_16M) //01
+ {
+ float m = -60.771777;
+ float bx = -84.68474012;
+ float by = -61.42658856;
+ float cx = -88.40823804;
+ float cy = 55.46391624;
+ float dx = -90.59395975;
+ float dy = 1.48597902;
+ float ex = -95.42385593;
+ float ey = -52.4786514;
+
+ if(rsl > bx) //region 1
+ {
+ bias = (rsl-bx)*m+by;
+ }
+ else if(rsl > cx) //region 2
+ {
+ bias = (rsl-cx)/(bx-cx)*(by-cy)+cy;
+ }
+ else if( rsl > dx) //region 3
+ {
+ bias = (rsl-dx)/(cx-dx)*(cy-dy)+dy;
+ }
+ else if(rsl > ex) //region 4
+ {
+ bias = (rsl-ex)/(dx-ex)*(dy-ey)+ey;
+ }
+ else //region 5
+ {
+ bias = ey;
+ }
+ }
+ else if(prf == DWT_PRF_64M) //11
+ {
+ double m = -54.53010908;
+ double bx = -81.65862959;
+ double by = -4.6428097;
+ double cx = -85.18994963;
+ double cy = 12.25860862;
+ double dx = -88.01254788;
+ double dy = -27.64410538;
+
+ if(rsl > bx) //region 1
+ {
+ bias = (rsl-bx)*m+by;
+ }
+ else if(rsl > cx) //region 2
+ {
+ bias = (rsl-bx)/(cx-bx)*(cy-by)+by;
+ }
+ else if(rsl > dx) //region 3
+ {
+ bias = (rsl-cx)/(dx-cx)*(dy-cy)+cy;
+ }
+ else //region 4
+ {
+ bias = dy;
+ }
+ }
+ }
+
+ return bias/1000.0; //convert to meters
+}
+
+int reportTOF(instance_data_t *inst, uint8 uwb_index, double rsl)
{
//no TOF to report if not from a uwb that we are tracking
if(uwb_index > UWB_LIST_SIZE)
@@ -164,8 +418,9 @@ int reportTOF(instance_data_t *inst, uint8 uwb_index)
return -1;
}
- double distance ;
- double distance_to_correct ;
+ double distance_raw = 0;
+ double distance_rsl = 0;
+ double distance_rng = 0;
double tof ;
int64 tofi ;
@@ -179,44 +434,43 @@ int reportTOF(instance_data_t *inst, uint8 uwb_index)
// convert to seconds (as floating point)
tof = convertdevicetimetosec(tofi);
- inst->idistanceraw[uwb_index] = distance = tof * SPEED_OF_LIGHT;
+ inst->idistanceraw[uwb_index] = distance_raw = tof * SPEED_OF_LIGHT;
#if (CORRECT_RANGE_BIAS == 1)
- //for the 6.81Mb data rate we assume gating gain of 6dB is used,
- //thus a different range bias needs to be applied
- //if(inst->configData.dataRate == DWT_BR_6M8)
- if(inst->smartPowerEn)
+
+ distance_rng = distance_raw - getrangebias_rng(inst->configData.chan, inst->configData.prf, distance_raw);
+ distance_rsl = distance_raw - getrangebias_rsl(inst->configData.chan, inst->configData.prf, rsl);
+
+#endif
+
+ int retval = 0;
+
+ if (distance_rng > 20000.000) // discount any items with error
{
- //1.31 for channel 2 and 1.51 for channel 5
- if(inst->configData.chan == 5)
- {
- distance_to_correct = distance/1.51;
- }
- else //channel 2
- {
- distance_to_correct = distance/1.31;
- }
+ distance_rng = 0;
+ retval = -1;
}
- else
+
+ if (distance_rsl > 20000.000) // discount any items with error
{
- distance_to_correct = distance;
+ distance_rsl = 0;
+ retval = -1;
}
- distance = distance - dwt_getrangebias(inst->configData.chan, (float) distance_to_correct, inst->configData.prf);
-#endif
- if (distance > 20000.000) // discount any items with error
- {
- inst->idistance[uwb_index] = 0;
- return -1;
+
+ if(distance_rng < 0){
+ distance_rng = 0;
}
- if(distance < 0){
- distance = 0;
+ if(distance_rsl < 0){
+ distance_rsl = 0;
}
- inst->idistance[uwb_index] = distance;
+ inst->idistance[uwb_index] = distance_rng;
+ inst->idistancersl[uwb_index] = distance_rsl;
+ inst->iRSL[uwb_index] = rsl;
- return 0;
+ return retval;
}// end of reportTOF
@@ -384,6 +638,13 @@ int instance_init(void)
}
instance_data[instance].newRangeUWBIndex = 0;
+ instance_data[instance].rslCnt = 0;
+ instance_data[instance].idxRSL = 0;
+ instance_data[instance].avgRSL = 0;
+ for(uint8 i=0; i<NUM_RSL_AVG; i++)
+ {
+ instance_data[instance].RSL[i] = 0;
+ }
// Reset the IC (might be needed if not getting here from POWER ON)
@@ -494,15 +755,44 @@ void instance_config(instanceConfig_t *config)
instance_data[instance].rxAntennaDelay = instance_data[instance].txAntennaDelay = instance_data[instance].defaultAntennaDelay;
}
#else
- instance_data[instance].txAntennaDelay = (uint16)TX_ANT_DELAY; //TODO channel selectable
- instance_data[instance].rxAntennaDelay = (uint16)RX_ANT_DELAY;
+
+ if(instance_data[instance].configData.chan == 2)
+ {
+ if(instance_data[instance].configData.prf == DWT_PRF_16M) //00
+ {
+ instance_data[instance].txAntennaDelay = (uint16)TX_ANT_DELAY_00;
+ instance_data[instance].rxAntennaDelay = (uint16)RX_ANT_DELAY_00;
+ }
+ else if(instance_data[instance].configData.prf == DWT_PRF_64M) //10
+ {
+ instance_data[instance].txAntennaDelay = (uint16)TX_ANT_DELAY_10;
+ instance_data[instance].rxAntennaDelay = (uint16)RX_ANT_DELAY_10;
+ }
+ }
+ else if(instance_data[instance].configData.chan == 5)
+ {
+ if(instance_data[instance].configData.prf == DWT_PRF_16M) //01
+ {
+ instance_data[instance].txAntennaDelay = (uint16)TX_ANT_DELAY_01;
+ instance_data[instance].rxAntennaDelay = (uint16)RX_ANT_DELAY_01;
+ }
+ else if(instance_data[instance].configData.prf == DWT_PRF_64M) //11
+ {
+ instance_data[instance].txAntennaDelay = (uint16)TX_ANT_DELAY_11;
+ instance_data[instance].rxAntennaDelay = (uint16)RX_ANT_DELAY_11;
+ }
+ }
+
+
+// instance_data[instance].txAntennaDelay = (uint16)TX_ANT_DELAY; //TODO channel selectable
+// instance_data[instance].rxAntennaDelay = (uint16)RX_ANT_DELAY;
#endif
// -------------------------------------------------------------------------------------------------------------------
// set the antenna delay, we assume that the RX is the same as TX.
- dwt_setrxantennadelay(instance_data[instance].txAntennaDelay);
+ dwt_setrxantennadelay(instance_data[instance].rxAntennaDelay);
dwt_settxantennadelay(instance_data[instance].txAntennaDelay);
if((power == 0x0) || (power == 0xFFFFFFFF)) //if there are no calibrated values... need to use defaults
@@ -516,6 +806,54 @@ void instance_config(instanceConfig_t *config)
//configure the tx spectrum parameters (power and PG delay)
dwt_configuretxrf(&instance_data[instance].configTX);
+ //TODO remove!
+// instance_data[instance].configTX.power = 0xE0E0E0E0; //OFF -89.7
+// instance_data[instance].configTX.power = 0xE1E1E1E1; //0.5 -89.1
+// instance_data[instance].configTX.power = 0xE3E3E3E3; //1.5 -
+// instance_data[instance].configTX.power = 0xE5E5E5E5; //2.5 -
+// instance_data[instance].configTX.power = 0xE7E7E7E7; //3.5 -
+// instance_data[instance].configTX.power = 0xE9E9E9E9; //4.5 -
+// instance_data[instance].configTX.power = 0xEBEBEBEB; //5.5 -
+// instance_data[instance].configTX.power = 0xEDEDEDED; //6.5 - (cut out here at 5 meters)
+// instance_data[instance].configTX.power = 0xEFEFEFEF; //7.5? -82.5
+// instance_data[instance].configTX.power = 0xF1F1F1F1; //8.5 -
+// instance_data[instance].configTX.power = 0xF3F3F3F3; //9.5 -
+// instance_data[instance].configTX.power = 0xF5F5F5F5; //10.5 -
+// instance_data[instance].configTX.power = 0xF7F7F7F7; //11.5 -
+// instance_data[instance].configTX.power = 0xF9F9F9F9; //12.5 -
+// instance_data[instance].configTX.power = 0xFBFBFBFB; //13.5 -
+// instance_data[instance].configTX.power = 0xFDFDFDFD; //14.5 -
+// instance_data[instance].configTX.power = 0xC1C1C1C1; //0.5 -78.7
+// instance_data[instance].configTX.power = 0xFFFFFFFF; //15? -79.5
+// instance_data[instance].configTX.power = 0xDFDFDFDF; //15? -78.3
+// instance_data[instance].configTX.power = 0x01010101; //15.5 -78.5
+// instance_data[instance].configTX.power = 0x03030303; //16.5 -
+// instance_data[instance].configTX.power = 0x05050505; //17.5 -
+// instance_data[instance].configTX.power = 0x07070707; //18.5 -
+// instance_data[instance].configTX.power = 0x09090909; //19.5 -
+// instance_data[instance].configTX.power = 0x0B0B0B0B; //20.5 -
+// instance_data[instance].configTX.power = 0x0D0D0D0D; //21.5 -78.8
+// instance_data[instance].configTX.power = 0x0F0F0F0F; //22.5 -
+// instance_data[instance].configTX.power = 0x11111111; //23.5 -
+// instance_data[instance].configTX.power = 0x13131313; //24.5 -
+// instance_data[instance].configTX.power = 0x15151515; //25.5 -
+// instance_data[instance].configTX.power = 0x17171717; //26.5 -
+// instance_data[instance].configTX.power = 0x19191919; //27.5 -
+// instance_data[instance].configTX.power = 0x1B1B1B1B; //28.5 -
+// instance_data[instance].configTX.power = 0x1D1D1D1D; //29.5 -
+// instance_data[instance].configTX.power = 0x1F1F1F1F; //30.5 -76.87
+
+ //everything with coarse gain 000 seems too high...
+// instance_data[instance].configTX.power = 0xC0C0C0C0; //0
+// instance_data[instance].configTX.power = 0x00000000; //15
+// instance_data[instance].configTX.power = 0xA0A0A0A0; //2.5
+// instance_data[instance].configTX.power = 0x80808080; //5.0
+
+
+
+// dwt_configuretxrf(&instance_data[instance].configTX);
+
+
instance_data[instance].antennaDelayChanged = 0;
if(config->preambleLen == DWT_PLEN_64) //if preamble length is 64
@@ -529,13 +867,20 @@ void instance_config(instanceConfig_t *config)
}
-double instance_get_idist(uint8 uwb_index) //get instantaneous range
+double instance_get_idist(uint8 uwb_index) //get instantaneous range corrected by distance
{
double x = instance_data[0].idistance[uwb_index];
return (x);
}
+double instance_get_idistrsl(uint8 uwb_index) //get instantaneous range corrected by rsl
+{
+ double x = instance_data[0].idistancersl[uwb_index];
+
+ return (x);
+}
+
double instance_get_idistraw(uint8 uwb_index) //get instantaneous range
{
double x = instance_data[0].idistanceraw[uwb_index];
@@ -543,6 +888,13 @@ double instance_get_idistraw(uint8 uwb_index) //get instantaneous range
return (x);
}
+double instance_get_irsl(uint8 uwb_index) //get instantaneous rsl
+{
+ double x = instance_data[0].iRSL[uwb_index];
+
+ return (x);
+}
+
int instance_get_rxf(void) //get number of Rxed frames
{
int x = instance_data[0].rxmsgcount;
@@ -673,7 +1025,7 @@ void instance_txcallback(const dwt_cb_data_t *txd)
dw_event.typeSave = dw_event.type = DWT_SIG_TX_DONE ;
- //NOTE: to avoid timestamping issues in the forums, we aren't using the DW1000 RX auto re-enable function.
+ //NOTE: to avoid timestamping issues reported in the forums, we aren't using the DW1000 RX auto re-enable function.
//Rather, we turn it back on here after every TX. However, logic in TA_RXE_WAIT needs wait4ack to be set
//to function correctly. We don't want it to try to start RX during reception.
dwt_forcetrxoff();
@@ -1019,8 +1371,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
uint32 reply_time = ((dw_event.timeStamp + instance_data[instance].respReplyDelay) & MASK_TXDTS) >> 8;
- //TODO put start_immediate definition back in (here)
-// if(instancesendpacket(psduLength, DWT_START_TX_IMMEDIATE | instance_data[instance].wait4ack, 0))
if(instancesendpacket(psduLength, DWT_START_TX_DELAYED | instance_data[instance].wait4ack, reply_time))
{
instance_data[0].tx_anch_resp = FALSE;
@@ -1045,6 +1395,26 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
}
else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_ANCH_RESP)
{
+ // Embed into Final message: 40-bit pollTXTime, 40-bit respRxTime, 40-bit finalTxTime
+ uint64 tagCalculatedFinalTxTime; // time we should send the response
+ uint64 finalReplyDelay = instance_data[0].respReplyDelay;
+ tagCalculatedFinalTxTime = (dw_event.timeStamp + finalReplyDelay) & MASK_TXDTS;
+ instance_data[0].delayedReplyTime = tagCalculatedFinalTxTime >> 8;
+
+ // Calculate Time Final message will be sent and write this field of Final message
+ // Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by
+ // zeroing its low 9 bits, and then having the TX antenna delay added
+ // getting antenna delay from the device and add it to the Calculated TX Time
+ tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + instance_data[0].txAntennaDelay;
+ tagCalculatedFinalTxTime &= MASK_40BIT;
+
+ // Write Calculated TX time field of Final message
+ memcpy(&(instance_data[0].msg.messageData[FTXT]), (uint8 *)&tagCalculatedFinalTxTime, 5);
+ // Write Poll TX time field of Final message
+// memcpy(&(instance_data[0].msg.messageData[PTXT]), (uint8 *)&dw_event.timeStamp, 5); TODO
+
+
+
//process RTLS_DEMO_MSG_ANCH_RESP immediately.
int psduLength = FINAL_FRAME_LEN_BYTES;
@@ -1078,7 +1448,83 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
}
else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_FINAL)
{
- instance_data[instance].dwt_final_rx = dw_event.timeStamp;
+ dwt_rxdiag_t dwt_diag;
+ dwt_readdiagnostics(&dwt_diag);
+
+ uint8 RXPACC_NOSAT_OFFSET = 0x2C;
+ uint16 RXPACC_NOSAT = dwt_read16bitoffsetreg(DRX_CONF_ID, RXPACC_NOSAT_OFFSET);
+
+ //following adjustment from function Adjust_RXPACC in following link
+ //https://github.com/damaki/DW1000/blob/00da81fce9c11c2632c5776add0629971b1d5ba6/src/dw1000-reception_quality.adb
+ if(RXPACC_NOSAT == dwt_diag.rxPreamCount)
+ {
+ uint8 rxpacc_adj = 0;
+ instance_data[instance].acc_adj = 1;
+
+ if(instance_data[instance].configData.nsSFD == TRUE)
+ {
+ if(instance_data[instance].configData.dataRate == DWT_BR_110K)
+ {
+ rxpacc_adj = 82;
+ }
+ if(instance_data[instance].configData.dataRate == DWT_BR_850K)
+ {
+ rxpacc_adj = 18;
+ }
+ else //DWT_BR_6M8
+ {
+ rxpacc_adj = 10;
+ }
+ }
+ else
+ {
+ if(instance_data[instance].configData.dataRate == DWT_BR_110K){
+ rxpacc_adj = 64;
+ }
+ else //DWT_BR_850K or DWT_BR_6M8
+ {
+ rxpacc_adj = 5;
+ }
+ }
+
+ if (rxpacc_adj <= dwt_diag.rxPreamCount)
+ {
+ dwt_diag.rxPreamCount -= rxpacc_adj;
+ }
+
+ }
+ else
+ {
+ instance_data[instance].acc_adj = 0;
+ }
+
+
+ //10 log (C * 2^17 / N^2) - A
+ double C =(double)dwt_diag.maxGrowthCIR;
+ if (C <= 0.0)
+ {
+ C = 1;
+ }
+
+ double Nsquared = pow(dwt_diag.rxPreamCount,2);
+ if (Nsquared <= 0.0)
+ {
+ Nsquared = 1;
+ }
+
+ double A = 0;
+ if (instance_data[instance].configData.prf == DWT_PRF_16M)
+ {
+ A = 113.77;
+ }
+ else //DWT_PRF_64M
+ {
+ A = 121.74;
+ }
+
+ instance_data[instance].rxPWR = (double)(10.0*log10(C*pow(2,17)/Nsquared)) - A;
+
+ instance_data[instance].dwt_final_rx = dw_event.timeStamp; //TODO needed?
}
//we received response to our POLL, select oldest range UWB next poll
--
GitLab