Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
540 lines (431 sloc) 17 KB
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* LPS node firmware.
*
* Copyright 2016, Bitcraze AB
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* lpsTdoa2Tag.c is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with lpsTdoa2Tag.c. If not, see <http://www.gnu.org/licenses/>.
*/
#include <string.h>
#include "FreeRTOS.h"
#include "task.h"
#include "log.h"
#include "param.h"
#include "lpsTdoa2Tag.h"
#include "stabilizer_types.h"
#include "cfassert.h"
#include "estimator.h"
#include "physicalConstants.h"
#define MEASUREMENT_NOISE_STD 0.15f
#define STATS_INTERVAL 500
#define ANCHOR_OK_TIMEOUT 1500
// Config
static lpsTdoa2AlgoOptions_t defaultOptions = {
.anchorAddress = {
0xbccf000000000000,
0xbccf000000000001,
0xbccf000000000002,
0xbccf000000000003,
0xbccf000000000004,
0xbccf000000000005,
#if LOCODECK_NR_OF_TDOA2_ANCHORS > 6
0xbccf000000000006,
#endif
#if LOCODECK_NR_OF_TDOA2_ANCHORS > 7
0xbccf000000000007,
#endif
},
.combinedAnchorPositionOk = false,
// To set a static anchor position from startup, uncomment and modify the
// following code:
// .anchorPosition = {
// {timestamp: 1, x: 0.99, y: 1.49, z: 1.80},
// {timestamp: 1, x: 0.99, y: 3.29, z: 1.80},
// {timestamp: 1, x: 4.67, y: 2.54, z: 1.80},
// {timestamp: 1, x: 0.59, y: 2.27, z: 0.20},
// {timestamp: 1, x: 4.70, y: 3.38, z: 0.20},
// {timestamp: 1, x: 4.70, y: 1.14, z: 0.20},
// },
//
// .combinedAnchorPositionOk = true,
};
static lpsTdoa2AlgoOptions_t* options = &defaultOptions;
// State
typedef struct {
rangePacket2_t packet;
dwTime_t arrival;
double clockCorrection_T_To_A;
uint32_t anchorStatusTimeout;
} history_t;
static uint8_t previousAnchor;
// Holds data for the latest packet from all anchors
static history_t history[LOCODECK_NR_OF_TDOA2_ANCHORS];
// LPP packet handling
static lpsLppShortPacket_t lppPacket;
static bool lppPacketToSend;
static int lppPacketSendTryCounter;
static void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data);
// Log data
static float logUwbTdoaDistDiff[LOCODECK_NR_OF_TDOA2_ANCHORS];
static float logClockCorrection[LOCODECK_NR_OF_TDOA2_ANCHORS];
static uint16_t logAnchorDistance[LOCODECK_NR_OF_TDOA2_ANCHORS];
static struct {
uint32_t packetsReceived;
uint32_t packetsSeqNrPass;
uint32_t packetsDataPass;
uint32_t packetsToEstimator;
uint16_t packetsReceivedRate;
uint16_t packetsSeqNrPassRate;
uint16_t packetsDataPassRate;
uint32_t packetsToEstimatorRate;
uint32_t nextStatisticsTime;
uint32_t previousStatisticsTime;
uint8_t lastAnchor0Seq;
uint32_t lastAnchor0RxTick;
} stats;
static bool rangingOk;
static void clearStats() {
stats.packetsReceived = 0;
stats.packetsSeqNrPass = 0;
stats.packetsDataPass = 0;
stats.packetsToEstimator = 0;
}
static uint64_t truncateToLocalTimeStamp(uint64_t fullTimeStamp) {
return fullTimeStamp & 0x00FFFFFFFFul;
}
static uint64_t truncateToAnchorTimeStamp(uint64_t fullTimeStamp) {
return fullTimeStamp & 0x00FFFFFFFFul;
}
static void enqueueTDOA(uint8_t anchorA, uint8_t anchorB, double distanceDiff) {
tdoaMeasurement_t tdoa = {
.stdDev = MEASUREMENT_NOISE_STD,
.distanceDiff = distanceDiff,
.anchorPosition[0] = options->anchorPosition[anchorA],
.anchorPosition[1] = options->anchorPosition[anchorB]
};
if (options->combinedAnchorPositionOk ||
(options->anchorPosition[anchorA].timestamp && options->anchorPosition[anchorB].timestamp)) {
stats.packetsToEstimator++;
estimatorEnqueueTDOA(&tdoa);
}
}
// The default receive time in the anchors for messages from other anchors is 0
// and is overwritten with the actual receive time when a packet arrives.
// That is, if no message was received the rx time will be 0.
static bool isValidTimeStamp(const int64_t anchorRxTime) {
return anchorRxTime != 0;
}
static bool isSeqNrConsecutive(uint8_t prevSeqNr, uint8_t currentSeqNr) {
return (currentSeqNr == ((prevSeqNr + 1) & 0xff));
}
// A note on variable names. They might seem a bit verbose but express quite a lot of information
// We have three actors: Reference anchor (Ar), Anchor n (An) and the deck on the CF called Tag (T)
// rxAr_by_An_in_cl_An should be interpreted as "The time when packet was received from the Reference
// Anchor by Anchor N expressed in the clock of Anchor N"
static bool calcClockCorrection(double* clockCorrection, const uint8_t anchor, const rangePacket2_t* packet, const dwTime_t* arrival) {
if (! isSeqNrConsecutive(history[anchor].packet.sequenceNrs[anchor], packet->sequenceNrs[anchor])) {
return false;
}
const int64_t rxAn_by_T_in_cl_T = arrival->full;
const int64_t txAn_in_cl_An = packet->timestamps[anchor];
const int64_t latest_rxAn_by_T_in_cl_T = history[anchor].arrival.full;
const int64_t latest_txAn_in_cl_An = history[anchor].packet.timestamps[anchor];
const double frameTime_in_cl_An = truncateToAnchorTimeStamp(txAn_in_cl_An - latest_txAn_in_cl_An);
const double frameTime_in_T = truncateToLocalTimeStamp(rxAn_by_T_in_cl_T - latest_rxAn_by_T_in_cl_T);
*clockCorrection = frameTime_in_cl_An / frameTime_in_T;
return true;
}
static bool calcDistanceDiff(float* tdoaDistDiff, const uint8_t previousAnchor, const uint8_t anchor, const rangePacket2_t* packet, const dwTime_t* arrival) {
const bool isSeqNrInTagOk = isSeqNrConsecutive(history[anchor].packet.sequenceNrs[previousAnchor], packet->sequenceNrs[previousAnchor]);
const bool isSeqNrInAnchorOk = isSeqNrConsecutive(history[anchor].packet.sequenceNrs[anchor], packet->sequenceNrs[anchor]);
if (! (isSeqNrInTagOk && isSeqNrInAnchorOk)) {
return false;
}
stats.packetsSeqNrPass++;
const int64_t rxAn_by_T_in_cl_T = arrival->full;
const int64_t rxAr_by_An_in_cl_An = packet->timestamps[previousAnchor];
const int64_t tof_Ar_to_An_in_cl_An = packet->distances[previousAnchor];
const double clockCorrection = history[anchor].clockCorrection_T_To_A;
const bool isAnchorDistanceOk = isValidTimeStamp(tof_Ar_to_An_in_cl_An);
const bool isRxTimeInTagOk = isValidTimeStamp(rxAr_by_An_in_cl_An);
const bool isClockCorrectionOk = (clockCorrection != 0.0);
if (! (isAnchorDistanceOk && isRxTimeInTagOk && isClockCorrectionOk)) {
return false;
}
stats.packetsDataPass++;
const int64_t txAn_in_cl_An = packet->timestamps[anchor];
const int64_t rxAr_by_T_in_cl_T = history[previousAnchor].arrival.full;
const int64_t delta_txAr_to_txAn_in_cl_An = (tof_Ar_to_An_in_cl_An + truncateToAnchorTimeStamp(txAn_in_cl_An - rxAr_by_An_in_cl_An));
const int64_t timeDiffOfArrival_in_cl_An = truncateToAnchorTimeStamp(rxAn_by_T_in_cl_T - rxAr_by_T_in_cl_T) * clockCorrection - delta_txAr_to_txAn_in_cl_An;
*tdoaDistDiff = SPEED_OF_LIGHT * timeDiffOfArrival_in_cl_An / LOCODECK_TS_FREQ;
return true;
}
static void addToLog(const uint8_t anchor, const uint8_t previousAnchor, const float tdoaDistDiff, const rangePacket2_t* packet) {
// Only store diffs for anchors when we have consecutive anchor ids. In case of packet
// loss we can get ranging between any anchors and that messes up the graphs.
if (((previousAnchor + 1) & 0x07) == anchor) {
logUwbTdoaDistDiff[anchor] = tdoaDistDiff;
logAnchorDistance[anchor] = packet->distances[previousAnchor];
}
}
static void handleLppPacket(const int dataLength, const packet_t* rxPacket) {
const int32_t payloadLength = dataLength - MAC802154_HEADER_LENGTH;
const int32_t startOfLppDataInPayload = LPS_TDOA2_LPP_HEADER;
const int32_t lppDataLength = payloadLength - startOfLppDataInPayload;
if (lppDataLength > 0) {
const uint8_t lppPacketHeader = rxPacket->payload[LPS_TDOA2_LPP_HEADER];
if (lppPacketHeader == LPP_HEADER_SHORT_PACKET) {
int srcId = -1;
for (int i=0; i < LOCODECK_NR_OF_TDOA2_ANCHORS; i++) {
if (rxPacket->sourceAddress == options->anchorAddress[i]) {
srcId = i;
break;
}
}
if (srcId >= 0) {
lpsHandleLppShortPacket(srcId, &rxPacket->payload[LPS_TDOA2_LPP_TYPE]);
}
}
}
}
// Send an LPP packet, the radio will automatically go back in RX mode
static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
{
static packet_t txPacket;
dwIdle(dev);
MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
txPacket.payload[LPS_TDOA2_TYPE_INDEX] = LPP_HEADER_SHORT_PACKET;
memcpy(&txPacket.payload[LPS_TDOA2_SEND_LPP_PAYLOAD_INDEX], packet->data, packet->length);
txPacket.pan = 0xbccf;
txPacket.sourceAddress = 0xbccf000000000000 | 0xff;
txPacket.destAddress = options->anchorAddress[packet->dest];
dwNewTransmit(dev);
dwSetDefaults(dev);
dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
dwWaitForResponse(dev, true);
dwStartTransmit(dev);
}
static bool rxcallback(dwDevice_t *dev) {
stats.packetsReceived++;
int dataLength = dwGetDataLength(dev);
packet_t rxPacket;
dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
const rangePacket2_t* packet = (rangePacket2_t*)rxPacket.payload;
bool lppSent = false;
if (packet->type == PACKET_TYPE_TDOA2) {
const uint8_t anchor = rxPacket.sourceAddress & 0xff;
// Check if we need to send the current LPP packet
if (lppPacketToSend && lppPacket.dest == anchor) {
sendLppShort(dev, &lppPacket);
lppSent = true;
}
dwTime_t arrival = {.full = 0};
dwGetReceiveTimestamp(dev, &arrival);
if (anchor < LOCODECK_NR_OF_TDOA2_ANCHORS) {
#ifdef LPS_TDOA2_SYNCHRONIZATION_VARIABLE
// Storing timing
if (anchor == 0) {
stats.lastAnchor0Seq = packet->sequenceNrs[anchor];
stats.lastAnchor0RxTick = xTaskGetTickCount();
}
#endif
calcClockCorrection(&history[anchor].clockCorrection_T_To_A, anchor, packet, &arrival);
logClockCorrection[anchor] = history[anchor].clockCorrection_T_To_A;
if (anchor != previousAnchor) {
float tdoaDistDiff = 0.0;
if (calcDistanceDiff(&tdoaDistDiff, previousAnchor, anchor, packet, &arrival)) {
rangingOk = true;
enqueueTDOA(previousAnchor, anchor, tdoaDistDiff);
addToLog(anchor, previousAnchor, tdoaDistDiff, packet);
}
}
history[anchor].arrival.full = arrival.full;
memcpy(&history[anchor].packet, packet, sizeof(rangePacket2_t));
history[anchor].anchorStatusTimeout = xTaskGetTickCount() + ANCHOR_OK_TIMEOUT;
previousAnchor = anchor;
handleLppPacket(dataLength, &rxPacket);
}
}
return lppSent;
}
static void setRadioInReceiveMode(dwDevice_t *dev) {
dwNewReceive(dev);
dwSetDefaults(dev);
dwStartReceive(dev);
}
static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
switch(event) {
case eventPacketReceived:
if (rxcallback(dev)) {
lppPacketToSend = false;
} else {
setRadioInReceiveMode(dev);
// Discard lpp packet if we cannot send it for too long
if (++lppPacketSendTryCounter >= TDOA2_LPP_PACKET_SEND_TIMEOUT) {
lppPacketToSend = false;
}
}
if (!lppPacketToSend) {
// Get next lpp packet
lppPacketToSend = lpsGetLppShort(&lppPacket);
lppPacketSendTryCounter = 0;
}
break;
case eventTimeout:
setRadioInReceiveMode(dev);
break;
case eventReceiveTimeout:
setRadioInReceiveMode(dev);
break;
case eventPacketSent:
// Service packet sent, the radio is back to receive automatically
break;
default:
ASSERT_FAILED();
}
uint32_t now = xTaskGetTickCount();
if (now > stats.nextStatisticsTime) {
float interval = now - stats.previousStatisticsTime;
stats.packetsReceivedRate = (uint16_t)(1000.0f * stats.packetsReceived / interval);
stats.packetsSeqNrPassRate = (uint16_t)(1000.0f * stats.packetsSeqNrPass / interval);
stats.packetsDataPassRate = (uint16_t)(1000.0f * stats.packetsDataPass / interval);
stats.packetsToEstimatorRate = (uint16_t)(1000.0f * stats.packetsToEstimator / interval);
clearStats();
stats.previousStatisticsTime = now;
stats.nextStatisticsTime = now + STATS_INTERVAL;
}
uint16_t rangingState = 0;
for (int anchor = 0; anchor < LOCODECK_NR_OF_TDOA2_ANCHORS; anchor++) {
if (now < history[anchor].anchorStatusTimeout) {
rangingState |= (1 << anchor);
}
}
locoDeckSetRangingState(rangingState);
return MAX_TIMEOUT;
}
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
static void Initialize(dwDevice_t *dev) {
// Reset module state. Needed by unit tests
memset(history, 0, sizeof(history));
memset(logClockCorrection, 0, sizeof(logClockCorrection));
memset(logAnchorDistance, 0, sizeof(logAnchorDistance));
memset(logUwbTdoaDistDiff, 0, sizeof(logUwbTdoaDistDiff));
previousAnchor = 0;
lppPacketToSend = false;
locoDeckSetRangingState(0);
clearStats();
stats.packetsReceivedRate = 0;
stats.packetsSeqNrPassRate = 0;
stats.packetsDataPassRate = 0;
stats.packetsToEstimatorRate = 0;
stats.nextStatisticsTime = xTaskGetTickCount() + STATS_INTERVAL;
stats.previousStatisticsTime = 0;
dwSetReceiveWaitTimeout(dev, TDOA2_RECEIVE_TIMEOUT);
dwCommitConfiguration(dev);
rangingOk = false;
}
#pragma GCC diagnostic pop
static bool isRangingOk()
{
return rangingOk;
}
static bool getAnchorPosition(const uint8_t anchorId, point_t* position) {
if (anchorId < LOCODECK_NR_OF_TDOA2_ANCHORS) {
*position = options->anchorPosition[anchorId];
return true;
}
return false;
}
static uint8_t getAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
for (int i = 0; i < LOCODECK_NR_OF_TDOA2_ANCHORS; i++) {
unorderedAnchorList[i] = i;
}
return LOCODECK_NR_OF_TDOA2_ANCHORS;
}
static uint8_t getActiveAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
uint32_t now = xTaskGetTickCount();
uint8_t count = 0;
for (int i = 0; i < LOCODECK_NR_OF_TDOA2_ANCHORS; i++) {
if (now < history[i].anchorStatusTimeout) {
unorderedAnchorList[count] = i;
count++;
}
}
return count;
}
// Loco Posisioning Protocol (LPP) handling
static void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data)
{
uint8_t type = data[0];
if (type == LPP_SHORT_ANCHORPOS) {
if (srcId < LOCODECK_NR_OF_TDOA2_ANCHORS) {
struct lppShortAnchorPos_s *newpos = (struct lppShortAnchorPos_s*)&data[1];
options->anchorPosition[srcId].timestamp = xTaskGetTickCount();
options->anchorPosition[srcId].x = newpos->x;
options->anchorPosition[srcId].y = newpos->y;
options->anchorPosition[srcId].z = newpos->z;
}
}
}
uwbAlgorithm_t uwbTdoa2TagAlgorithm = {
.init = Initialize,
.onEvent = onEvent,
.isRangingOk = isRangingOk,
.getAnchorPosition = getAnchorPosition,
.getAnchorIdList = getAnchorIdList,
.getActiveAnchorIdList = getActiveAnchorIdList,
};
void lpsTdoa2TagSetOptions(lpsTdoa2AlgoOptions_t* newOptions) {
options = newOptions;
}
LOG_GROUP_START(tdoa)
LOG_ADD(LOG_FLOAT, d7-0, &logUwbTdoaDistDiff[0])
LOG_ADD(LOG_FLOAT, d0-1, &logUwbTdoaDistDiff[1])
LOG_ADD(LOG_FLOAT, d1-2, &logUwbTdoaDistDiff[2])
LOG_ADD(LOG_FLOAT, d2-3, &logUwbTdoaDistDiff[3])
LOG_ADD(LOG_FLOAT, d3-4, &logUwbTdoaDistDiff[4])
LOG_ADD(LOG_FLOAT, d4-5, &logUwbTdoaDistDiff[5])
LOG_ADD(LOG_FLOAT, d5-6, &logUwbTdoaDistDiff[6])
LOG_ADD(LOG_FLOAT, d6-7, &logUwbTdoaDistDiff[7])
LOG_ADD(LOG_FLOAT, cc0, &logClockCorrection[0])
LOG_ADD(LOG_FLOAT, cc1, &logClockCorrection[1])
LOG_ADD(LOG_FLOAT, cc2, &logClockCorrection[2])
LOG_ADD(LOG_FLOAT, cc3, &logClockCorrection[3])
LOG_ADD(LOG_FLOAT, cc4, &logClockCorrection[4])
LOG_ADD(LOG_FLOAT, cc5, &logClockCorrection[5])
LOG_ADD(LOG_FLOAT, cc6, &logClockCorrection[6])
LOG_ADD(LOG_FLOAT, cc7, &logClockCorrection[7])
LOG_ADD(LOG_UINT16, dist7-0, &logAnchorDistance[0])
LOG_ADD(LOG_UINT16, dist0-1, &logAnchorDistance[1])
LOG_ADD(LOG_UINT16, dist1-2, &logAnchorDistance[2])
LOG_ADD(LOG_UINT16, dist2-3, &logAnchorDistance[3])
LOG_ADD(LOG_UINT16, dist3-4, &logAnchorDistance[4])
LOG_ADD(LOG_UINT16, dist4-5, &logAnchorDistance[5])
LOG_ADD(LOG_UINT16, dist5-6, &logAnchorDistance[6])
LOG_ADD(LOG_UINT16, dist6-7, &logAnchorDistance[7])
LOG_ADD(LOG_UINT16, stRx, &stats.packetsReceivedRate)
LOG_ADD(LOG_UINT16, stSeq, &stats.packetsSeqNrPassRate)
LOG_ADD(LOG_UINT16, stData, &stats.packetsDataPassRate)
LOG_ADD(LOG_UINT16, stEst, &stats.packetsToEstimatorRate)
#ifdef LPS_TDOA2_SYNCHRONIZATION_VARIABLE
LOG_ADD(LOG_UINT8, a0Seq, &stats.lastAnchor0Seq)
LOG_ADD(LOG_UINT32, a0RxTick, &stats.lastAnchor0RxTick)
#endif
LOG_GROUP_STOP(tdoa)
You can’t perform that action at this time.